diff --git a/.github/CONTRIBUTING.md b/.github/CONTRIBUTING.md new file mode 100644 index 000000000..1851baf94 --- /dev/null +++ b/.github/CONTRIBUTING.md @@ -0,0 +1,35 @@ +# Contributing to the Robotics Toolbox + +## Reporting issues + +When reporting issues please include as much detail as possible about your +operating system, roboticstoolbox version and python version. Whenever possible, please +also include a brief, self-contained code example that demonstrates the problem. + +## Contributing code + +Thanks for your interest in contributing code! + +We welcome all kinds of contributions including: + ++ New features ++ Bug and issue fixes ++ Cleaning, adding or adding to documentation and docstrings ++ Adding or fixing Python types + + +Keep in mind the following when making your contribution: + ++ Keep pull requests to a **single** feature/bug fix. This makes it much easier to review and merge. If you wish to contribure multiple different fixes or features, that means you should make multiple pull requests. + ++ For API changes, propose the API change in the discussions first before opening a pull request. + ++ Code additions should be formatted using [black](https://pypi.org/project/black/). Our configuration for black can be found in the [pyproject.toml](https://github.com/petercorke/robotics-toolbox-python/blob/master/pyproject.toml) file under the heading `[tool.black]`. Avoid reformatting code using other formatters. + ++ Code addition should be linted using [flake8](https://pypi.org/project/flake8/). Our configuration for black can be found in the [pyproject.toml](https://github.com/petercorke/robotics-toolbox-python/blob/master/pyproject.toml) file under the heading `[tool.flake8]`. + ++ Any code addition needs to be covered by unit tests and not break existing tests. Our unit tests live in `robotics-toolbox-python/tests/`. You can install the dev dependencies using the command `pip install -e '.[dev,docs]'`. You can run the test suite using the command `pytest --cov=roboticstoolbox/ --cov-report term-missing`. Check the output to make sure your additions have been covered by the unit tests. + ++ All methods and classes need to be documented with an appropriate docstring. See our [style guide](https://github.com/petercorke/robotics-toolbox-python/wiki/Documentation-Style-Guide) for documentation. Keep the ordering and formatting as described by the style guide. + ++ New additions should be typed appropriately. See our typing [style guide](). diff --git a/.github/workflows/test.yml b/.github/workflows/test.yml index 27dc8b0bd..2fc23db67 100644 --- a/.github/workflows/test.yml +++ b/.github/workflows/test.yml @@ -28,7 +28,7 @@ jobs: run: | pip install .[dev,collision] pip install pytest-timeout - pytest --timeout=50 --timeout_method thread -s + pytest --ignore=roboticstoolbox/blocks --timeout=50 --timeout_method thread -s codecov: # If all tests pass: # Run coverage and upload to codecov @@ -46,7 +46,7 @@ jobs: - name: Run coverage run: | pip install -e .[dev,collision,vpython] - pytest --cov=roboticstoolbox --cov-report xml:coverage.xml + pytest --ignore=roboticstoolbox/blocks --cov=roboticstoolbox --cov-report xml:coverage.xml coverage report - name: upload coverage to Codecov uses: codecov/codecov-action@v3 diff --git a/.github/workflows/test_future.yml b/.github/workflows/test_future.yml index 59378be4c..ce8f8b0c6 100644 --- a/.github/workflows/test_future.yml +++ b/.github/workflows/test_future.yml @@ -23,7 +23,7 @@ jobs: uses: actions/checkout@v2 with: ref: future - path: rtb + path: robotics-toolbox-python - name: Checkout Swift uses: actions/checkout@v2 @@ -48,18 +48,25 @@ jobs: - name: Install dependencies run: | + echo "Update pip" python -m pip install --upgrade pip + pip install -U build cd sm - python -m pip install . + echo "Install sm" + pip install . cd ../sg - python -m pip install . + echo "Install sg" + pip install . cd ../swift - python -m pip install . - cd ../rtb/rtb-data - python -m pip install . + echo "Install swift" + pip install . + cd ../robotics-toolbox-python/rtb-data + pip install . - name: Test with pytest run: | - cd rtb - pip install .[dev,collision] + cd robotics-toolbox-python + pip install -e .[dev,collision] pip install pytest-timeout - pytest --timeout=50 --timeout_method thread -s + python -c "import spatialgeometry" + python -c "import roboticstoolbox" + pytest --ignore=roboticstoolbox/blocks --timeout=50 --timeout_method thread -s diff --git a/.gitignore b/.gitignore index d029e6c05..3207ee3c5 100644 --- a/.gitignore +++ b/.gitignore @@ -1,4 +1,6 @@ +.DS_Store +.pyodide*/** *.doctree # Vim diff --git a/CONTRIBUTING.md b/CONTRIBUTING.md deleted file mode 100644 index 448df9f59..000000000 --- a/CONTRIBUTING.md +++ /dev/null @@ -1,2 +0,0 @@ -There is lots to be done and we'd welcome assistance. Some notes about -contributing can be found [here](https://github.com/petercorke/robotics-toolbox-python/wiki/Contributing) \ No newline at end of file diff --git a/docs/figs/multirotorplot.png b/docs/figs/multirotorplot.png new file mode 100644 index 000000000..d31a7eac0 Binary files /dev/null and b/docs/figs/multirotorplot.png differ diff --git a/docs/figs/rvc4_4.gif b/docs/figs/rvc4_4.gif new file mode 100644 index 000000000..7785c99c3 Binary files /dev/null and b/docs/figs/rvc4_4.gif differ diff --git a/docs/source/IK/ik.rst b/docs/source/IK/ik.rst new file mode 100644 index 000000000..87529ba83 --- /dev/null +++ b/docs/source/IK/ik.rst @@ -0,0 +1,261 @@ +.. _IK: + + +Inverse Kinematics +================== + + +The Robotics Toolbox supports an extensive set of numerical inverse kinematics (IK) tools and we will demonstrate the different ways these IK tools can be interacted with in this document. + +For a **tutorial** on numerical IK, see `here `_. + +Within the Toolbox, we have two sets of solvers: solvers written in C++ and solvers written in Python. However, within all of our solvers there are several common arguments: + +.. rubric:: Tep + +``Tep`` represent the desired end-effector pose. + +A note on the semantics of the above variable: + +* **T** represents an SE(3) (a homogeneous tranformation matrix in 3 dimensions, a 4x4 matrix) +* *e* is short for end-effector referring to the end of the kinematic chain +* *p* is short for prime or desired +* Since there is no letter to the left of the **T**, the world or base reference frame is implied + +Therefore, ``Tep`` refers to the desired end-effector pose in the base robot frame represented as an SE(3). + +.. rubric:: ilimit + +The ``ilimit`` specifies how many iterations are allowed within a single search. After ``ilimit`` is reached, either, a new attempt is made or the IK solution has failed depending on ``slimit`` + +.. rubric:: slimit + +The ``slimit`` specifies how many searches are allowed before the problem is deemed unsolvable. The maximum number of iterations allowed is therefore ``ilimit`` x ``slimit``. By having ``slimit`` > 1, a global search is performed. Since finding a solution with numerical IK heavily depends on the initial choice of ``q0``, performing a global search where ``slimit`` >> 1 will provide a far greater chance of success. + +.. rubric:: q0 + +``q0`` is the inital joint coordinate vector. If ``q0`` is 1 dimensional (, ``n``), then ``q0`` is only used for the first attempt, after which a new random valid initial joint coordinate vector will be generated. If ``q0`` is 2 dimensional (``slimit``, ``n``), then the next vector within ``q0`` will be used for the next search. + +.. rubric:: tol + +``tol`` sets the error tolerance before the solution is deemed successful. The error is typically set by some quadratic error function + +.. math:: + + E = \frac{1}{2} \vec{e}^{\top} \mat{W}_e \vec{e} + +where :math:`\vec{e} \in \mathbb{R}^6` is the angle-axis error, and :math:`\mat{W}_e` assigns weights to Cartesian degrees-of-freedom + +.. rubric:: mask + +``mask`` is a (,6) array that sets :math:`\mat{W}_e` in error equation above. The vector has six elements that correspond to translation in X, Y and Z, and rotation about X, Y and Z respectively. The value can be 0 (for ignore) or above to assign a priority relative to other Cartesian DoF. + +For the case where the manipulator has fewer than 6 DoF the solution space has more dimensions than can be spanned by the manipulator joint coordinates. + +In this case we use the ``mask`` option where the ``mask`` vector specifies the Cartesian DOF that will be ignored in reaching a solution. The number of non-zero elements must equal the number of manipulator DOF. + +For example when using a 3 DOF manipulator tool orientation might be unimportant, in which case use the option ``mask=[1, 1, 1, 0, 0, 0]``. + +.. rubric:: joint_limits + +setting ``joint_limits = True`` will reject solutions with joint limit violations. Note that finding a solution with valid joint coordinates is likely to take longer than without. + +.. rubric:: Others + +There are other arguments which may be unique to the solver, so check the documentation of the solver you wish to use for a complete list and explanation of arguments. + +C++ Solvers +----------- + +These solvers are written in high performance C++ and wrapped in Python methods. The methods are made available within the :py:class:`~roboticstoolbox.robot.ETS.ETS` and :py:class:`~roboticstoolbox.robot.Robot.Robot` classes. Being written in C++, these solvers are extraordinarily fast and typically take 30 to 90 µs. However, these solvers are hard to extend or modify. + +These methods have been written purely for speed so they do not contain the niceties of the Python alternative. For example, if you give the incorrect length for the ``q0`` vector, you could end up with a ``seg-fault`` or other undetermined behaviour. Therefore, when using these methods it is very important that you understand each of the parameters and the parameters passed are of the correct type and length. + +The C++ solvers return a tuple with the following members: + +============== ========= ===================================================================================================== +Element Type Description +============== ========= ===================================================================================================== +``q`` `ndarray` The joint coordinates of the solution. Note that these will not be valid if failed to find a solution +``success`` `bool` True if a valid solution was found +``iterations`` `int` How many iterations were performed +``searches`` `int` How many searches were performed +``residual`` `float` The final error value from the cost function +============== ========= ===================================================================================================== + +The C++ solvers can be identified as methods which start with ``ik_``. + +.. rubric:: ETS C++ IK Methods + +.. autosummary:: + :toctree: stubs + + ~roboticstoolbox.robot.ETS.ETS.ik_LM + ~roboticstoolbox.robot.ETS.ETS.ik_GN + ~roboticstoolbox.robot.ETS.ETS.ik_NR + +.. rubric:: Robot C++ IK Methods + +.. autosummary:: + :toctree: stubs + + ~roboticstoolbox.robot.Robot.Robot.ik_LM + ~roboticstoolbox.robot.Robot.Robot.ik_GN + ~roboticstoolbox.robot.Robot.Robot.ik_NR + +In the following example, we create a :py:class:`~roboticstoolbox.models.URDF.Panda` robot and one of the fast IK solvers available within the :py:class:`~roboticstoolbox.robot.Robot.Robot` class. + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> # Make a Panda robot + >>> panda = rtb.models.Panda() + >>> # Make a goal pose + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> # Solve the IK problem + >>> panda.ik_LM(Tep) + +In the following example, we create a :py:class:`~roboticstoolbox.models.URDF.Panda` robot and and then get the :py:class:`~roboticstoolbox.robot.ETS.ETS` representation. Subsequently, we use one of the fast IK solvers available within the :py:class:`~roboticstoolbox.robot.ETS.ETS` class. + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> # Make a Panda robot + >>> panda = rtb.models.Panda() + >>> # Get the ETS + >>> ets = panda.ets() + >>> # Make a goal pose + >>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> # Solve the IK problem + >>> ets.ik_LM(Tep) + + + + + +Python Solvers +-------------- + +These solvers are Python classes which extend the abstract base class :py:class:`~roboticstoolbox.robot.IK.IKSolver` and the :py:meth:`~roboticstoolbox.robot.IK.IKSolver.solve` method returns an :py:class:`~roboticstoolbox.robot.IK.IKSolution`. These solvers are slow and will typically take 100 - 1000 ms. However, these solvers are easy to extend and modify. + +.. rubric:: The Abstract Base Class + +.. toctree:: + :maxdepth: 1 + + iksolver + +The :py:class:`~roboticstoolbox.robot.IK.IKSolver` provides basic functionality for performing numerical IK. Superclasses can inherit this class and must implement the :py:meth:`~roboticstoolbox.robot.IK.IKSolver.solve` method. Additionally a superclass redefine any other methods necessary such as :py:meth:`~roboticstoolbox.robot.IK.IKSolver.error` to provide a custom error function. + +.. rubric:: The Solution DataClass + +.. toctree:: + :maxdepth: 1 + + iksolution + +The :py:class:`~roboticstoolbox.robot.IK.IKSolution` is a :py:class:`dataclasses.dataclass` instance with the following members. + +============== ========= ===================================================================================================== +Element Type Description +============== ========= ===================================================================================================== +``q`` `ndarray` The joint coordinates of the solution. Note that these will not be valid if failed to find a solution +``success`` `bool` True if a valid solution was found +``iterations`` `int` How many iterations were performed +``searches`` `int` How many searches were performed +``residual`` `float` The final error value from the cost function +``reason`` `str` The reason the IK problem failed if applicable +============== ========= ===================================================================================================== + +.. rubric:: The Implemented IK Solvers + +These solvers can be identified as a :py:class:`Class` starting with ``IK_``. + +.. toctree:: + :maxdepth: 1 + + ik_lm + ik_qp + ik_gn + ik_nr + +.. rubric:: Example + +In the following example, we create an IK Solver class and pass an :py:class:`~roboticstoolbox.robot.ETS.ETS` to it to solve the problem. This style may be preferable to experiments where you wish to compare the same solver on different robots. + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> # Make a Panda robot + >>> panda = rtb.models.Panda() + >>> # Get the ETS of the Panda + >>> ets = panda.ets() + >>> # Make an IK solver + >>> solver = rtb.IK_LM() + >>> # Make a goal pose + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> # Solve the IK problem + >>> solver.solve(ets, Tep) + + + +.. IK Solvers Available with an ETS +.. ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ + +Additionally, these :py:class:`Class` based solvers have been implemented as methods within the :py:class:`~roboticstoolbox.robot.ETS.ETS` and :py:class:`~roboticstoolbox.robot.Robot.Robot` classes. The method names start with ``ikine_``. + + +.. toctree: + :caption: IK Solvers from an ETS + + +.. rubric:: ETS Python IK Methods + + +.. autosummary:: + :toctree: stubs + + ~roboticstoolbox.robot.ETS.ETS.ikine_LM + ~roboticstoolbox.robot.ETS.ETS.ikine_QP + ~roboticstoolbox.robot.ETS.ETS.ikine_GN + ~roboticstoolbox.robot.ETS.ETS.ikine_NR + + +.. rubric:: Robot Python IK Methods + +.. autosummary:: + :toctree: stubs + + ~roboticstoolbox.robot.Robot.Robot.ikine_LM + ~roboticstoolbox.robot.Robot.Robot.ikine_QP + ~roboticstoolbox.robot.Robot.Robot.ikine_GN + ~roboticstoolbox.robot.Robot.Robot.ikine_NR + + +.. rubric:: Example + +In the following example, we create a :py:class:`~roboticstoolbox.models.URDF.Panda` robot and one of the IK solvers available within the :py:class:`~roboticstoolbox.robot.Robot.Robot` class. This style is far more convenient than the above example. + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> # Make a Panda robot + >>> panda = rtb.models.Panda() + >>> # Make a goal pose + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> # Solve the IK problem + >>> panda.ikine_LM(Tep) + +In the following example, we create a :py:class:`~roboticstoolbox.models.URDF.Panda` robot and and then get the :py:class:`~roboticstoolbox.robot.ETS.ETS` representation. Subsequently, we use one of the IK solvers available within the :py:class:`~roboticstoolbox.robot.ETS.ETS` class. + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> # Make a Panda robot + >>> panda = rtb.models.Panda() + >>> # Get the ETS + >>> ets = panda.ets() + >>> # Make a goal pose + >>> Tep = ets.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> # Solve the IK problem + >>> ets.ikine_LM(Tep) diff --git a/docs/source/IK/ik_gn.rst b/docs/source/IK/ik_gn.rst new file mode 100644 index 000000000..9bea7bd0c --- /dev/null +++ b/docs/source/IK/ik_gn.rst @@ -0,0 +1,25 @@ +IK_GN - Gauss-Newton Numerical IK +--------------------------------- + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IK_GN + :show-inheritance: + + +.. rubric:: Methods + +.. autosummary:: + :toctree: stubs + + ~IK_GN.step + ~IK_GN.solve + ~IK_GN.error + +.. rubric:: Private Methods + +.. autosummary:: + :toctree: stubs + + ~IK_GN._random_q + ~IK_GN._check_jl diff --git a/docs/source/IK/ik_lm.rst b/docs/source/IK/ik_lm.rst new file mode 100644 index 000000000..41d337add --- /dev/null +++ b/docs/source/IK/ik_lm.rst @@ -0,0 +1,25 @@ +IK_LM - Levemberg-Marquadt Numerical IK +--------------------------------------- + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IK_LM + :show-inheritance: + + +.. rubric:: Methods + +.. autosummary:: + :toctree: stubs + + ~IK_LM.step + ~IK_LM.solve + ~IK_LM.error + +.. rubric:: Private Methods + +.. autosummary:: + :toctree: stubs + + ~IK_LM._random_q + ~IK_LM._check_jl diff --git a/docs/source/IK/ik_nr.rst b/docs/source/IK/ik_nr.rst new file mode 100644 index 000000000..5aa345482 --- /dev/null +++ b/docs/source/IK/ik_nr.rst @@ -0,0 +1,25 @@ +IK_NR - Newton-Raphson Numerical IK +----------------------------------- + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IK_NR + :show-inheritance: + + +.. rubric:: Methods + +.. autosummary:: + :toctree: stubs + + ~IK_NR.step + ~IK_NR.solve + ~IK_NR.error + +.. rubric:: Private Methods + +.. autosummary:: + :toctree: stubs + + ~IK_NR._random_q + ~IK_NR._check_jl diff --git a/docs/source/IK/ik_qp.rst b/docs/source/IK/ik_qp.rst new file mode 100644 index 000000000..06767812b --- /dev/null +++ b/docs/source/IK/ik_qp.rst @@ -0,0 +1,25 @@ +IK_QP - Numerical IK with Quadratic Programming +----------------------------------------------- + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IK_QP + :show-inheritance: + + +.. rubric:: Methods + +.. autosummary:: + :toctree: stubs + + ~IK_QP.step + ~IK_QP.solve + ~IK_QP.error + +.. rubric:: Private Methods + +.. autosummary:: + :toctree: stubs + + ~IK_QP._random_q + ~IK_QP._check_jl diff --git a/docs/source/IK/iksolution.rst b/docs/source/IK/iksolution.rst new file mode 100644 index 000000000..2f8ff05bf --- /dev/null +++ b/docs/source/IK/iksolution.rst @@ -0,0 +1,4 @@ +IKSolution - as IK Solution dataclass +------------------------ + +.. autoclass:: roboticstoolbox.robot.IK.IKSolution diff --git a/docs/source/IK/iksolver.rst b/docs/source/IK/iksolver.rst new file mode 100644 index 000000000..ce3563c8d --- /dev/null +++ b/docs/source/IK/iksolver.rst @@ -0,0 +1,25 @@ +IKSolver - IK Solver Abstract Base Class +----------------------------------------- + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IKSolver + :show-inheritance: + + +.. rubric:: Methods + +.. autosummary:: + :toctree: stubs + + ~IKSolver.step + ~IKSolver.solve + ~IKSolver.error + +.. rubric:: Private Methods + +.. autosummary:: + :toctree: stubs + + ~IKSolver._random_q + ~IKSolver._check_jl diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_gn.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_gn.rst new file mode 100644 index 000000000..fdf679e58 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_gn.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ik\_gn +====== + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ik_gn \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_chan.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_chan.rst new file mode 100644 index 000000000..3bd7825a9 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_chan.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ik\_lm\_chan +============ + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ik_lm_chan \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_sugihara.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_sugihara.rst new file mode 100644 index 000000000..dd1d4885c --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_sugihara.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ik\_lm\_sugihara +================ + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ik_lm_sugihara \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_wampler.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_wampler.rst new file mode 100644 index 000000000..0dd62f6be --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_lm_wampler.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ik\_lm\_wampler +=============== + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ik_lm_wampler \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_nr.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_nr.rst new file mode 100644 index 000000000..66c3b2b97 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ik_nr.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ik\_nr +====== + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ik_nr \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ikine_LM.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ikine_LM.rst new file mode 100644 index 000000000..3e9cd371a --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ERobot.ERobot.ikine_LM.rst @@ -0,0 +1,8 @@ +:orphan: + +ERobot.ikine\_LM +========= + +.. currentmodule:: roboticstoolbox.robot.ERobot + +.. automethod:: roboticstoolbox.robot.ERobot.ERobot.ikine_LM \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_LM.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_LM.rst new file mode 100644 index 000000000..5cfdf9ca9 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_LM.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_LM +====== + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_LM \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_gn.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_gn.rst new file mode 100644 index 000000000..ca4e3e0ef --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_gn.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_GN +====== + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_GN \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_chan.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_chan.rst new file mode 100644 index 000000000..774638c3c --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_chan.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_lm\_chan +============ + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_lm_chan \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_sugihara.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_sugihara.rst new file mode 100644 index 000000000..a09648073 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_sugihara.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_lm\_sugihara +================ + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_lm_sugihara \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_wampler.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_wampler.rst new file mode 100644 index 000000000..f0ef8c419 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_lm_wampler.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_lm\_wampler +=============== + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_lm_wampler \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_nr.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_nr.rst new file mode 100644 index 000000000..18feaa8b9 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ik_nr.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ik\_NR +====== + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ik_NR \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_GN.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_GN.rst new file mode 100644 index 000000000..8af50e3d6 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_GN.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ikine\_GN +========= + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ikine_GN \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_LM.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_LM.rst new file mode 100644 index 000000000..d803faf5f --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_LM.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ikine\_LM +========= + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ikine_LM \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_NR.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_NR.rst new file mode 100644 index 000000000..1bff10322 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_NR.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ikine\_NR +========= + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ikine_NR \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_QP.rst b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_QP.rst new file mode 100644 index 000000000..f2a06048d --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.ETS.ETS.ikine_QP.rst @@ -0,0 +1,8 @@ +:orphan: + +ETS.ikine\_QP +========= + +.. currentmodule:: roboticstoolbox.robot.ETS + +.. automethod:: roboticstoolbox.robot.ETS.ETS.ikine_QP \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolution.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolution.rst new file mode 100644 index 000000000..813c4992b --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolution.rst @@ -0,0 +1,33 @@ +roboticstoolbox.robot.IK.IKSolution +=================================== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IKSolution + + + .. automethod:: __init__ + + + .. rubric:: Methods + + .. autosummary:: + + ~IKSolution.__init__ + + + + + + .. rubric:: Attributes + + .. autosummary:: + + ~IKSolution.iterations + ~IKSolution.reason + ~IKSolution.residual + ~IKSolution.searches + ~IKSolution.q + ~IKSolution.success + + \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._check_jl.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._check_jl.rst new file mode 100644 index 000000000..a44d0046a --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._check_jl.rst @@ -0,0 +1,8 @@ +:orphan: + +IKSolver.\_check\_jl +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IKSolver._check_jl \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._random_q.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._random_q.rst new file mode 100644 index 000000000..e2ac97a55 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver._random_q.rst @@ -0,0 +1,8 @@ +:orphan: + +IKSolver.\_random\_q +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IKSolver._random_q \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.error.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.error.rst new file mode 100644 index 000000000..1ecf6e394 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.error.rst @@ -0,0 +1,8 @@ +:orphan: + +IKSolver.error +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IKSolver.error \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.rst new file mode 100644 index 000000000..9f4fba004 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.rst @@ -0,0 +1,25 @@ +roboticstoolbox.robot.IK.IKSolver +================================= + +.. currentmodule:: roboticstoolbox.robot.IK + +.. autoclass:: IKSolver + + + .. automethod:: __init__ + + + .. rubric:: Methods + + .. autosummary:: + + ~IKSolver.__init__ + ~IKSolver.error + ~IKSolver.solve + ~IKSolver.step + + + + + + \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.solve.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.solve.rst new file mode 100644 index 000000000..657efe5a1 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.solve.rst @@ -0,0 +1,8 @@ +:orphan: + +IKSolver.solve +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IKSolver.solve \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.step.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.step.rst new file mode 100644 index 000000000..a920c8b42 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IKSolver.step.rst @@ -0,0 +1,8 @@ +:orphan: + +IKSolver.step +==== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IKSolver.step \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._check_jl.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._check_jl.rst new file mode 100644 index 000000000..f6a045055 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._check_jl.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_GN.\_check\_jl +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_GN._check_jl \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._random_q.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._random_q.rst new file mode 100644 index 000000000..2f53e86bc --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN._random_q.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_GN.\_random\_q +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_GN._random_q \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.error.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.error.rst new file mode 100644 index 000000000..b5470417d --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.error.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_GN.error +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_GN.error \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.solve.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.solve.rst new file mode 100644 index 000000000..cd10a778e --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.solve.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_GN.solve +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_GN.solve \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.step.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.step.rst new file mode 100644 index 000000000..06478d52f --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_GN.step.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_GN.step +==== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_GN.step \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._check_jl.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._check_jl.rst new file mode 100644 index 000000000..f49f0296b --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._check_jl.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_LM.\_check\_jl +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_LM._check_jl \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._random_q.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._random_q.rst new file mode 100644 index 000000000..51717cbae --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM._random_q.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_LM.\_random\_q +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_LM._random_q \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.error.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.error.rst new file mode 100644 index 000000000..0892ec42c --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.error.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_LM.error +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_LM.error \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.solve.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.solve.rst new file mode 100644 index 000000000..26a7075a8 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.solve.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_LM.solve +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_LM.solve \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.step.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.step.rst new file mode 100644 index 000000000..c830e597a --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_LM.step.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_LM.step +==== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_LM.step \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._check_jl.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._check_jl.rst new file mode 100644 index 000000000..a92f46426 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._check_jl.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_NR.\_check\_jl +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_NR._check_jl \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._random_q.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._random_q.rst new file mode 100644 index 000000000..84dd879bb --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR._random_q.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_NR.\_random\_q +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_NR._random_q \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.error.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.error.rst new file mode 100644 index 000000000..cd58f9362 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.error.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_NR.error +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_NR.error \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.solve.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.solve.rst new file mode 100644 index 000000000..689014fd2 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.solve.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_NR.solve +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_NR.solve \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.step.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.step.rst new file mode 100644 index 000000000..2b5cbbabc --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_NR.step.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_NR.step +==== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_NR.step \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._check_jl.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._check_jl.rst new file mode 100644 index 000000000..6dfb4df30 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._check_jl.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_QP.\_check\_jl +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_QP._check_jl \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._random_q.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._random_q.rst new file mode 100644 index 000000000..efc847e80 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP._random_q.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_QP.\_random\_q +=========== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_QP._random_q \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.error.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.error.rst new file mode 100644 index 000000000..43f371457 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.error.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_QP.error +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_QP.error \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.solve.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.solve.rst new file mode 100644 index 000000000..44083aafe --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.solve.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_QP.solve +===== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_QP.solve \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.step.rst b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.step.rst new file mode 100644 index 000000000..469e47497 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.IK.IK_QP.step.rst @@ -0,0 +1,8 @@ +:orphan: + +IK_QP.step +==== + +.. currentmodule:: roboticstoolbox.robot.IK + +.. automethod:: roboticstoolbox.robot.IK.IK_QP.step \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_GN.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_GN.rst new file mode 100644 index 000000000..407d8871c --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_GN.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ik\_GN +====== + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ik_GN \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_LM.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_LM.rst new file mode 100644 index 000000000..1a1e970cc --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_LM.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ik\_LM +====== + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ik_LM \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_NR.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_NR.rst new file mode 100644 index 000000000..8238f56a7 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ik_NR.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ik\_NR +====== + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ik_NR \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_GN.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_GN.rst new file mode 100644 index 000000000..908b999ba --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_GN.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ikine\_GN +========= + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ikine_GN \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_LM.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_LM.rst new file mode 100644 index 000000000..8be1903f3 --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_LM.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ikine\_LM +========= + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ikine_LM \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_NR.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_NR.rst new file mode 100644 index 000000000..1192f7dfb --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_NR.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ikine\_NR +========= + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ikine_NR \ No newline at end of file diff --git a/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_QP.rst b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_QP.rst new file mode 100644 index 000000000..b766ab3ba --- /dev/null +++ b/docs/source/IK/stubs/roboticstoolbox.robot.Robot.Robot.ikine_QP.rst @@ -0,0 +1,8 @@ +:orphan: + +Robot.ikine\_QP +========= + +.. currentmodule:: roboticstoolbox.robot.Robot + +.. automethod:: roboticstoolbox.robot.Robot.Robot.ikine_QP \ No newline at end of file diff --git a/docs/source/_static/android-chrome-192x192.png b/docs/source/_static/android-chrome-192x192.png new file mode 100644 index 000000000..80f98a133 Binary files /dev/null and b/docs/source/_static/android-chrome-192x192.png differ diff --git a/docs/source/_static/android-chrome-512x512.png b/docs/source/_static/android-chrome-512x512.png new file mode 100644 index 000000000..1f0ce81fb Binary files /dev/null and b/docs/source/_static/android-chrome-512x512.png differ diff --git a/docs/source/_static/apple-touch-icon.png b/docs/source/_static/apple-touch-icon.png new file mode 100644 index 000000000..bfa68ccee Binary files /dev/null and b/docs/source/_static/apple-touch-icon.png differ diff --git a/docs/source/_static/favicon-16x16.png b/docs/source/_static/favicon-16x16.png new file mode 100644 index 000000000..604c505db Binary files /dev/null and b/docs/source/_static/favicon-16x16.png differ diff --git a/docs/source/_static/favicon-32x32.png b/docs/source/_static/favicon-32x32.png new file mode 100644 index 000000000..caef72a56 Binary files /dev/null and b/docs/source/_static/favicon-32x32.png differ diff --git a/docs/source/_static/favicon.ico b/docs/source/_static/favicon.ico new file mode 100644 index 000000000..b87fc7e72 Binary files /dev/null and b/docs/source/_static/favicon.ico differ diff --git a/docs/source/_templates/autosummary/method.rst b/docs/source/_templates/autosummary/method.rst new file mode 100644 index 000000000..819ffa497 --- /dev/null +++ b/docs/source/_templates/autosummary/method.rst @@ -0,0 +1,7 @@ +:orphan: + +{{ class + '.' + name | escape | underline}} + +.. currentmodule:: {{ module }} + +.. auto{{ objtype }}:: {{ fullname }} diff --git a/docs/source/arm.rst b/docs/source/arm.rst index 27f693cdb..035ab26fb 100644 --- a/docs/source/arm.rst +++ b/docs/source/arm.rst @@ -1,10 +1,11 @@ -**************** -Manipulator arms -**************** +********** +Robot Arms +********** .. toctree:: - :maxdepth: 2 + :maxdepth: 4 arm_models - arm_backend - arm_trajectory \ No newline at end of file + .. arm_backend + arm_trajectory + IK/ik \ No newline at end of file diff --git a/docs/source/arm_backend.rst b/docs/source/arm_backend.rst index 7f4dd41e7..e14c48bce 100644 --- a/docs/source/arm_backend.rst +++ b/docs/source/arm_backend.rst @@ -8,5 +8,3 @@ physical robots. :maxdepth: 2 arm_backend_pyplot - arm_backend_vpython - arm_backend_swift \ No newline at end of file diff --git a/docs/source/arm_backend_swift.rst b/docs/source/arm_backend_swift.rst deleted file mode 100644 index 02f7e752e..000000000 --- a/docs/source/arm_backend_swift.rst +++ /dev/null @@ -1,8 +0,0 @@ -Swift ------ - -.. automodule:: roboticstoolbox.backends.Swift - :members: - :undoc-members: - :show-inheritance: - :inherited-members: \ No newline at end of file diff --git a/docs/source/arm_backend_vpython.rst b/docs/source/arm_backend_vpython.rst deleted file mode 100644 index 550474b2f..000000000 --- a/docs/source/arm_backend_vpython.rst +++ /dev/null @@ -1,9 +0,0 @@ -VPython -------- - -.. automodule:: roboticstoolbox.backends.VPython - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - \ No newline at end of file diff --git a/docs/source/arm_trajectory.rst b/docs/source/arm_trajectory.rst index f7281c411..253d37af0 100644 --- a/docs/source/arm_trajectory.rst +++ b/docs/source/arm_trajectory.rst @@ -6,4 +6,5 @@ Trajectories :undoc-members: :show-inheritance: :inherited-members: - :special-members: \ No newline at end of file + :special-members: + :exclude-members: __dict__, __module__, __weakref__ \ No newline at end of file diff --git a/docs/source/blocks-spatial.rst b/docs/source/blocks-spatial.rst new file mode 100644 index 000000000..2a44ce62c --- /dev/null +++ b/docs/source/blocks-spatial.rst @@ -0,0 +1,13 @@ +Spatial maths blocks +==================== + +.. image:: https://raw.githubusercontent.com/petercorke/bdsim/master/figs/BDSimLogo_NoBackgnd@2x.png + :width: 300 + +.. automodule:: roboticstoolbox.blocks.spatial + :members: + :undoc-members: + :show-inheritance: + :special-members: __init__ + :exclude-members: output, reset, step, start, done, deriv, nin, nout, inlabels, outlabels + diff --git a/docs/source/blocks.rst b/docs/source/blocks.rst index dc0760c0b..0457e3116 100644 --- a/docs/source/blocks.rst +++ b/docs/source/blocks.rst @@ -4,13 +4,13 @@ bdsim blocks .. image:: https://raw.githubusercontent.com/petercorke/bdsim/master/figs/BDSimLogo_NoBackgnd@2x.png :width: 400 -A set of block definitions that provide robotic capability to `bdsim `_. - -.. warning:: This capability is still experimental +A set of block definitions that add robotic capability to the `bdsim `_ +block diagram simulation environment. .. toctree:: :maxdepth: 2 + blocks-spatial blocks-arm blocks-mobile blocks-uav diff --git a/docs/source/conf.py b/docs/source/conf.py index cc6dce936..d383814d3 100644 --- a/docs/source/conf.py +++ b/docs/source/conf.py @@ -1,38 +1,32 @@ -# spatialmath # Configuration file for the Sphinx documentation builder. # # This file only contains a selection of the most common options. For a full # list see the documentation: # https://www.sphinx-doc.org/en/master/usage/configuration.html -# -- Path setup -------------------------------------------------------------- - # If extensions (or modules to document with autodoc) are in another directory, # add these directories to sys.path here. If the directory is relative to the # documentation root, use os.path.abspath to make it absolute, like shown here. -# + import os import sys -import sphinx_rtd_theme import re -# defined relative to configuration directory which is where this file conf.py lives +# Defined relative to configuration directory which is where this file conf.py lives sys.path.append(os.path.abspath("exts")) -# -- Project information ----------------------------------------------------- +# -------- Project information -------------------------------------------------------# project = "Robotics Toolbox for Python" -copyright = "2020, Jesse Haviland and Peter Corke" +copyright = "2023, Jesse Haviland and Peter Corke" author = "Jesse Haviland and Peter Corke" -# print(__file__) - -# parse version number out of setup.py +# Parse version number out of setup.py with open("../../setup.py", encoding="utf-8") as f: setup_py = f.read() - m = re.search("version='([0-9\.]*)',", setup_py, re.MULTILINE) + m = re.search(r"version='([0-9\.]*)',", setup_py, re.MULTILINE) -# -- General configuration --------------------------------------------------- +# -------- General configuration -----------------------------------------------------# # Add any Sphinx extension module names here, as strings. They can be # extensions coming with Sphinx (named 'sphinx.ext.*') or your custom @@ -45,9 +39,16 @@ "sphinx.ext.coverage", "sphinx.ext.doctest", "sphinx.ext.inheritance_diagram", - "sphinx_autorun", + "sphinx.ext.autosummary", "blockname", "sphinx.ext.intersphinx", + "matplotlib.sphinxext.plot_directive", + "format_example", + "sphinx.ext.napoleon", + "sphinx_autodoc_typehints", + # "scanpydoc.elegant_typehints", + "sphinx_autorun", + "sphinx_favicon", ] autosummary_generate = True @@ -58,9 +59,9 @@ exclude_patterns = ["test_*"] -# options for spinx_autorun, used for inline examples -# choose UTF-8 encoding to allow for Unicode characters, eg. ansitable -# Python session setup, turn off color printing for SE3, set NumPy precision +# Options for spinx_autorun, used for inline examples +# choose UTF-8 encoding to allow for Unicode characters, eg. ansitable +# Python session setup, turn off color printing for SE3, set NumPy precision autorun_languages = {} autorun_languages["pycon_output_encoding"] = "UTF-8" autorun_languages["pycon_input_encoding"] = "UTF-8" @@ -75,28 +76,24 @@ ANSITable._color = False """ -# -- Options for HTML output ------------------------------------------------- +# -------- Options for HTML output ---------------------------------------------------# html_theme = "sphinx_rtd_theme" html_theme_options = { - #'github_user': 'petercorke', - #'github_repo': 'spatialmath-python', - #'logo_name': False, "logo_only": False, "display_version": True, - "prev_next_buttons_location": "both", + "prev_next_buttons_location": "None", "analytics_id": "G-11Q6WJM565", - "style_external_links": True, + "style_external_links": False, + "navigation_depth": 5, } + html_logo = "../figs/RobToolBox_RoundLogoB.png" html_last_updated_fmt = "%d-%b-%Y" +html_show_sourcelink = False show_authors = True - -# mathjax_config = { -# "jax": ["input/TeX","output/HTML-CSS"], -# "displayAlign": "left" -# } +html_show_sphinx = False # Add any paths that contain custom static files (such as style sheets) here, # relative to this directory. They are copied after the builtin static files, @@ -105,29 +102,25 @@ html_css_files = [ "css/custom.css", ] -# autodoc_mock_imports = ["numpy", "scipy"] +default_role = "py:obj" +# -------- Options for LaTeX/PDF output ----------------------------------------------# -# -- Options for LaTeX/PDF output -------------------------------------------- latex_engine = "xelatex" -# maybe need to set graphics path in here somewhere -# \graphicspath{{figures/}{../figures/}{C:/Users/me/Documents/project/figures/}} -# https://stackoverflow.com/questions/63452024/how-to-include-image-files-in-sphinx-latex-pdf-files + latex_elements = { # The paper size ('letterpaper' or 'a4paper'). "papersize": "a4paper", - #'releasename':" ", - # Sonny, Lenny, Glenn, Conny, Rejne, Bjarne and Bjornstrup - # 'fncychap': '\\usepackage[Lenny]{fncychap}', "fncychap": "\\usepackage{fncychap}", "maketitle": "blah blah blah", } # Use RVC book notation for maths -# see https://stackoverflow.com/questions/9728292/creating-latex-math-macros-within-sphinx -mathjax_config = { - "TeX": { - "Macros": { +# see +# https://stackoverflow.com/questions/9728292/creating-latex-math-macros-within-sphinx +mathjax3_config = { + "tex": { + "macros": { # RVC Math notation # - not possible to do the if/then/else approach # - subset only @@ -160,10 +153,64 @@ } } } + +# -------- Options InterSphinx -------------------------------------------------------# + intersphinx_mapping = { + "python": ("https://docs.python.org/3", None), "numpy": ("http://docs.scipy.org/doc/numpy/", None), "scipy": ("http://docs.scipy.org/doc/scipy/reference/", None), "matplotlib": ("http://matplotlib.sourceforge.net/", None), + "spatialmath": ("https://petercorke.github.io/spatialmath-python/", None), } -# 'python': ('http://docs.python.org/2', None), + +# -------- Options Napoleon ----------------------------------------------------------# + +# Include special members (like __membername__) with docstrings in +# the documentation +napoleon_include_special_with_doc = True + +napoleon_custom_sections = ["Synopsis"] + +# -------- Options AutoSummary -------------------------------------------------------# + +# autodoc_default_flags = ["members"] +autosummary_generate = True + +# -------- Options favicon -------------------------------------------------------# + +html_static_path = ["_static"] +# create favicons online using https://favicon.io/favicon-converter/ +favicons = [ + { + "rel": "icon", + "sizes": "16x16", + "static-file": "favicon-16x16.png", + "type": "image/png", + }, + { + "rel": "icon", + "sizes": "32x32", + "static-file": "favicon-32x32.png", + "type": "image/png", + }, + { + "rel": "apple-touch-icon", + "sizes": "180x180", + "static-file": "apple-touch-icon.png", + "type": "image/png", + }, + { + "rel": "android-chrome", + "sizes": "192x192", + "static-file": "android-chrome-192x192.png ", + "type": "image/png", + }, + { + "rel": "android-chrome", + "sizes": "512x512", + "static-file": "android-chrome-512x512.png ", + "type": "image/png", + }, +] diff --git a/docs/source/exts/blockname.py b/docs/source/exts/blockname.py index 8454c9928..93fca00c7 100644 --- a/docs/source/exts/blockname.py +++ b/docs/source/exts/blockname.py @@ -1,5 +1,6 @@ from docutils import nodes -#from docutils.parsers.rst import Directive + +# from docutils.parsers.rst import Directive def block_name(name, rawtext, text, lineno, inliner, options={}, content=[]): @@ -11,14 +12,40 @@ def block_name(name, rawtext, text, lineno, inliner, options={}, content=[]): # inliner: The docutils.parsers.rst.states.Inliner object that called role_fn. It contains the several attributes useful for error reporting and document tree access. # options: A dictionary of directive options for customization (from the "role" directive), to be interpreted by the role function. Used for additional attributes for the generated elements and other functionality. # content: A list of strings, the directive content for customization (from the "role" directive). To be interpreted by the role function. - html_node = nodes.raw(text='

' + text + '

', format='html') + + html = """ + + + + + + + + + +
+

+ {0} +

+ 		+ +
+ """ + + # this is the path to the icons within the github repo + path = ( + "https://github.com/petercorke/robotics-toolbox-python/raw/master/roboticstoolbox/blocks/Icons/" + + text.lower() + + ".png" + ) + html_node = nodes.raw(text=html.format(text, path), format="html") return [html_node], [] def setup(app): app.add_role("blockname", block_name) return { - 'version': '0.1', - 'parallel_read_safe': True, - 'parallel_write_safe': True, - } \ No newline at end of file + "version": "0.1", + "parallel_read_safe": True, + "parallel_write_safe": True, + } diff --git a/docs/source/exts/format_example.py b/docs/source/exts/format_example.py new file mode 100644 index 000000000..d28eeaa2e --- /dev/null +++ b/docs/source/exts/format_example.py @@ -0,0 +1,67 @@ +import re + +""" +This extension looks for examples under the `.. runblock:: pycon` directive and indents the +code underneath. + +For example: + +.. runblock:: pycon +>>> import roboticstoolbox as rtb +>>> panda = rtb.models.Panda().ets() +>>> solver = rtb.IK_NR(pinv=True) +>>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) +>>> solver.solve(panda, Tep) + +becomes + +.. runblock:: pycon + + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> solver = rtb.IK_NR(pinv=True) + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> solver.solve(panda, Tep) + +""" + + +def process_doc(app, what, name, obj, options, lines): + + run_directive_re = r"( )*(\.\. runblock:: pycon)" + python_re = r"( )*(>>>)" + + for i, line in enumerate(lines): + + if re.match(run_directive_re, line): + # We have matched " .. runblock:: pycon" + + # Insert a blank line + lines.insert(i + 1, "") + + searching = True + j = 2 + + while searching: + try: + if re.match(python_re, lines[i + j]): + # We have matched " >>>" + lines[i + j] = " " + lines[i + j] + else: + # We have reached the end of the example + searching = False + + j += 1 + except IndexError: + # End of the docstring has been reached + return + + +def setup(app): + app.connect("autodoc-process-docstring", process_doc) + + return { + "version": "0.1", + "parallel_read_safe": True, + "parallel_write_safe": True, + } diff --git a/docs/source/intro.rst b/docs/source/intro.rst index 7b04435b9..569f553b9 100644 --- a/docs/source/intro.rst +++ b/docs/source/intro.rst @@ -359,25 +359,25 @@ or pure rotation -- each with either a constant parameter or a free parameter wh .. runblock:: pycon :linenos: - >>> from roboticstoolbox import ETS as ET + >>> from roboticstoolbox import ET >>> import roboticstoolbox as rtb >>> # Puma dimensions (m), see RVC2 Fig. 7.4 for details >>> l1 = 0.672; l2 = -0.2337; l3 = 0.4318; l4 = 0.0203; l5 = 0.0837; l6 = 0.4318 - >>> e = ET.tz(l1) * ET.rz() * ET.ty(l2) * ET.ry() * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.ry() * ET.tz(l6) * ET.rz() * ET.ry() * ET.rz() + >>> e = ET.tz(l1) * ET.Rz() * ET.ty(l2) * ET.Ry() * ET.tz(l3) * ET.tx(l4) * ET.ty(l5) * ET.Ry() * ET.tz(l6) * ET.Rz() * ET.Ry() * ET.Rz() >>> print(e) - >>> robot = rtb.ERobot(e) + >>> robot = rtb.Robot(e) >>> print(robot) Line 3 defines the unique lengths of the Puma robot, and line 4 defines the kinematic chain in terms of elementary transforms. -In line 7 we pass this to the constructor for an ``ERobot`` which partitions the +In line 7 we pass this to the constructor for a ``Robot`` which partitions the elementary transform sequence into a series of links and joints -- link frames are declared after each joint variable as well as the start and end of the sequence. -The ``ERobot`` can represent single-branched robots with any combination of revolute and prismatic joints, but +The ``Robot`` can represent single-branched robots with any combination of revolute and prismatic joints, but can also represent more general branched mechanisms. -ERobot: rigid-body tree and URDF import +Robot: rigid-body tree and URDF import ^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^ The final approach to manipulator modeling is to an import a URDF file. The Toolbox includes a parser with built-in xacro processor @@ -458,20 +458,20 @@ function:: Straight line (Cartesian) paths can be generated in a similar way between two points specified by a pair of poses in :math:`\SE{3}` -.. runblock:: pycon - :linenos: - - >>> import numpy as np - >>> from spatialmath import SE3 - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> t = np.arange(0, 2, 0.010) - >>> T0 = SE3(0.6, -0.5, 0.0) - >>> T1 = SE3(0.4, 0.5, 0.2) - >>> Ts = rtb.tools.trajectory.ctraj(T0, T1, len(t)) - >>> len(Ts) - >>> sol = puma.ikine_LM(Ts) # named tuple of arrays - >>> sol.q.shape +.. .. runblock:: pycon +.. :linenos: + +.. >>> import numpy as np +.. >>> from spatialmath import SE3 +.. >>> import roboticstoolbox as rtb +.. >>> puma = rtb.models.DH.Puma560() +.. >>> t = np.arange(0, 2, 0.010) +.. >>> T0 = SE3(0.6, -0.5, 0.0) +.. >>> T1 = SE3(0.4, 0.5, 0.2) +.. >>> Ts = rtb.tools.trajectory.ctraj(T0, T1, len(t)) +.. >>> len(Ts) +.. >>> sol = puma.ikine_LM(Ts) # named tuple of arrays +.. >>> sol.q.shape At line 9 we see that the resulting trajectory, ``Ts``, is an ``SE3`` instance with 200 values. @@ -539,7 +539,7 @@ At a singular configuration Jacobians can also be computed for symbolic joint variables as for forward kinematics above. -For ``ERobot`` instances we can also compute the Hessians:: +For ``Robot`` instances we can also compute the Hessians:: >>> H = puma.hessian0(q) >>> H = puma.hessiane(q) @@ -568,7 +568,7 @@ for the Yoshikawa and Asada measures respectively, and is the Yoshikawa measure computed for just the task space translational degrees of freedom. -For ``ERobot`` instances we can also compute the manipulability +For ``Robot`` instances we can also compute the manipulability Jacobian:: >>> Jm = puma.manipm(q, J, H) @@ -626,7 +626,7 @@ Python version takes 1.5ms (:math:`65\times` slower). With symbolic operands it takes 170ms (:math:`113\times` slower) to produce the unsimplified torque expressions. -For ``ERobot`` subclasses there is also an implementation of Featherstone's spatial vector +For ``Robot`` subclasses there is also an implementation of Featherstone's spatial vector method, ``rne()``, and SMTB-P provides a set of classes for spatial velocity, acceleration, momentum, force and inertia. diff --git a/docs/source/mobile_SLAM.rst b/docs/source/mobile-SLAM.rst similarity index 100% rename from docs/source/mobile_SLAM.rst rename to docs/source/mobile-SLAM.rst diff --git a/docs/source/mobile_drivers.rst b/docs/source/mobile-drivers.rst similarity index 72% rename from docs/source/mobile_drivers.rst rename to docs/source/mobile-drivers.rst index 079998fa3..950a59d17 100644 --- a/docs/source/mobile_drivers.rst +++ b/docs/source/mobile-drivers.rst @@ -1,8 +1,8 @@ -Mobile robot drivers -==================== +Drivers +======= -These classes can drive a mobile robot around a workplace. Useful for testing -localization algorithms. +These classes define agents thta can drive a mobile robot around a workplace, and are +useful for testing localization algorithms. Random Path ^^^^^^^^^^^ diff --git a/docs/source/mobile-planner-base.rst b/docs/source/mobile-planner-base.rst new file mode 100644 index 000000000..9808ee045 --- /dev/null +++ b/docs/source/mobile-planner-base.rst @@ -0,0 +1,26 @@ +Supporting classes +================== + +Planner superclass +------------------ + +.. autoclass:: roboticstoolbox.mobile.PlannerBase + :members: + :undoc-members: + :show-inheritance: + + +Occupancy grid base classes +--------------------------- + +.. autoclass:: roboticstoolbox.mobile.OccGrid.BaseMap + :members: + :undoc-members: + :show-inheritance: + +.. autoclass:: roboticstoolbox.mobile.OccGrid.BaseOccupancyGrid + :members: + :undoc-members: + :show-inheritance: + + diff --git a/docs/source/mobile-planner-continuous.rst b/docs/source/mobile-planner-continuous.rst new file mode 100644 index 000000000..90bfeaed8 --- /dev/null +++ b/docs/source/mobile-planner-continuous.rst @@ -0,0 +1,80 @@ +Continuous configuration-space planners +======================================= + + +========================================================= ==================== =================== =================== +Planner Plans in Discrete/Continuous Obstacle avoidance +========================================================= ==================== =================== =================== +:class:`~roboticstoolbox.mobile.DubinsPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.ReedsSheppPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.CurvaturePolyPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.QuinticPolyPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.RRTPlanner` :math:`\SE{2}` continuous yes +========================================================= ==================== =================== =================== + + +These planners do not support planning around obstacles, but allow for the +start and goal configuration :math:`(x, y, \theta)` to be specified. + +PRM planner +----------- + +.. autoclass:: roboticstoolbox.mobile.PRMPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + + +Dubins path planner +------------------- + +.. autoclass:: roboticstoolbox.mobile.DubinsPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + + +Reeds-Shepp path planner +------------------------ + +.. autoclass:: roboticstoolbox.mobile.ReedsSheppPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + +Curvature-polynomial planner +---------------------------- + +.. autoclass:: roboticstoolbox.mobile.CurvaturePolyPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + +Quintic-polynomial planner +-------------------------- + +.. autoclass:: roboticstoolbox.mobile.QuinticPolyPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + +RRT planner +----------- + +.. autoclass:: roboticstoolbox.mobile.RRTPlanner + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg + diff --git a/docs/source/mobile_planner.rst b/docs/source/mobile-planner-discrete.rst similarity index 54% rename from docs/source/mobile_planner.rst rename to docs/source/mobile-planner-discrete.rst index bb162e5da..eb4302706 100644 --- a/docs/source/mobile_planner.rst +++ b/docs/source/mobile-planner-discrete.rst @@ -1,15 +1,6 @@ -Mobile robot path planning -************************** - -A set of path planners for robots operating in planar environments with -configuration :math:`\vec{q} \in \mathbb{R}^2` or :math:`\vec{q} \in \SE{2}`. -All inherit from :class:`PlannerBase`. - -Some planners are based on code from the PathPlanning category of -`PythonRobotics `_ by Atsushi Sakai. +Discrete (Grid-based) planners +============================== -.. inheritance-diagram:: roboticstoolbox.mobile.DistanceTransformPlanner roboticstoolbox.mobile.DstarPlanner roboticstoolbox.mobile.DubinsPlanner roboticstoolbox.mobile.ReedsSheppPlanner roboticstoolbox.mobile.QuinticPolyPlanner roboticstoolbox.mobile.CurvaturePolyPlanner roboticstoolbox.mobile.RRTPlanner - :parts: 1 ========================================================= ==================== =================== =================== Planner Plans in Discrete/Continuous Obstacle avoidance @@ -17,21 +8,9 @@ Planner Plans in :class:`~roboticstoolbox.mobile.Bug2` :math:`\mathbb{R}^2` discrete yes :class:`~roboticstoolbox.mobile.DistanceTransformPlanner` :math:`\mathbb{R}^2` discrete yes :class:`~roboticstoolbox.mobile.DstarPlanner` :math:`\mathbb{R}^2` discrete yes -:class:`~roboticstoolbox.mobile.PRMPlanner` :math:`\mathbb{R}^2` continuous yes -:class:`~roboticstoolbox.mobile.LatticePlanner` :math:`\SE{2}` discrete yes -:class:`~roboticstoolbox.mobile.DubinsPlanner` :math:`\SE{2}` continuous no -:class:`~roboticstoolbox.mobile.ReedsSheppPlanner` :math:`\SE{2}` continuous no -:class:`~roboticstoolbox.mobile.CurvaturePolyPlanner` :math:`\SE{2}` continuous no -:class:`~roboticstoolbox.mobile.QuinticPolyPlanner` :math:`\SE{2}` continuous no -:class:`~roboticstoolbox.mobile.RRTPlanner` :math:`\SE{2}` continuous yes ========================================================= ==================== =================== =================== - -Discrete (Grid-based) planners -============================== - - Distance transform planner -------------------------- @@ -52,16 +31,6 @@ D* planner :inherited-members: :exclude-members: next, isoccupied, random, random_init, progress_start, progress_end, progress_next, message -PRM planner ------------ - -.. autoclass:: roboticstoolbox.mobile.PRMPlanner - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg - Lattice planner --------------- @@ -129,68 +98,3 @@ RRT planner :show-inheritance: :inherited-members: :exclude-members: isoccupied, random, random_init, progress_start, progress_end, progress_next, message, plot_bg - -Map classes -=========== - -Occupancy grid classes ----------------------- - -Binary Occupancy grid -^^^^^^^^^^^^^^^^^^^^^ - -.. autoclass:: roboticstoolbox.mobile.BinaryOccupancyGrid - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :special-members: __init__ - - -Occupancy grid -^^^^^^^^^^^^^^ - -.. autoclass:: roboticstoolbox.mobile.OccupancyGrid - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :special-members: __init__ - -Polygon map ------------ - -.. autoclass:: roboticstoolbox.mobile.PolygonMap - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :special-members: __init__ - - -Supporting classes -================== - -Planner superclass ------------------- - -.. autoclass:: roboticstoolbox.mobile.PlannerBase - :members: - :undoc-members: - :show-inheritance: - - -Occupancy grid base classes ---------------------------- - -.. autoclass:: roboticstoolbox.mobile.OccGrid.BaseMap - :members: - :undoc-members: - :show-inheritance: - -.. autoclass:: roboticstoolbox.mobile.OccGrid.BaseOccupancyGrid - :members: - :undoc-members: - :show-inheritance: - - diff --git a/docs/source/mobile-planner-map.rst b/docs/source/mobile-planner-map.rst new file mode 100644 index 000000000..638b97730 --- /dev/null +++ b/docs/source/mobile-planner-map.rst @@ -0,0 +1,36 @@ +Map classes +=========== + +Occupancy grid classes +---------------------- + +Binary Occupancy grid +^^^^^^^^^^^^^^^^^^^^^ + +.. autoclass:: roboticstoolbox.mobile.BinaryOccupancyGrid + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :special-members: __init__ + + +Occupancy grid +^^^^^^^^^^^^^^ + +.. autoclass:: roboticstoolbox.mobile.OccupancyGrid + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :special-members: __init__ + +Polygon map +----------- + +.. autoclass:: roboticstoolbox.mobile.PolygonMap + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :special-members: __init__ diff --git a/docs/source/mobile-planner.rst b/docs/source/mobile-planner.rst new file mode 100644 index 000000000..5eb30faef --- /dev/null +++ b/docs/source/mobile-planner.rst @@ -0,0 +1,36 @@ +Path planning +************* + +A set of path planners for robots operating in planar environments with +configuration :math:`\vec{q} \in \mathbb{R}^2` or :math:`\vec{q} \in \mathbb{R}^2 \times S^1 \sim \SE{2}`. +All inherit from :class:`PlannerBase`. + +Some planners are based on code from the PathPlanning category of +`PythonRobotics `_ by Atsushi Sakai. + +.. inheritance-diagram:: roboticstoolbox.mobile.DistanceTransformPlanner roboticstoolbox.mobile.DstarPlanner roboticstoolbox.mobile.DubinsPlanner roboticstoolbox.mobile.ReedsSheppPlanner roboticstoolbox.mobile.QuinticPolyPlanner roboticstoolbox.mobile.CurvaturePolyPlanner roboticstoolbox.mobile.RRTPlanner + :parts: 1 + +========================================================= ==================== =================== =================== +Planner Plans in Discrete/Continuous Obstacle avoidance +========================================================= ==================== =================== =================== +:class:`~roboticstoolbox.mobile.Bug2` :math:`\mathbb{R}^2` discrete yes +:class:`~roboticstoolbox.mobile.DistanceTransformPlanner` :math:`\mathbb{R}^2` discrete yes +:class:`~roboticstoolbox.mobile.DstarPlanner` :math:`\mathbb{R}^2` discrete yes +:class:`~roboticstoolbox.mobile.PRMPlanner` :math:`\mathbb{R}^2` continuous yes +:class:`~roboticstoolbox.mobile.LatticePlanner` :math:`\SE{2}` discrete yes +:class:`~roboticstoolbox.mobile.DubinsPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.ReedsSheppPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.CurvaturePolyPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.QuinticPolyPlanner` :math:`\SE{2}` continuous no +:class:`~roboticstoolbox.mobile.RRTPlanner` :math:`\SE{2}` continuous yes +========================================================= ==================== =================== =================== + + +.. toctree:: + :maxdepth: 2 + + mobile-planner-continuous + mobile-planner-discrete + mobile-planner-map + mobile-planner-base \ No newline at end of file diff --git a/docs/source/mobile_reactive.rst b/docs/source/mobile-reactive.rst similarity index 77% rename from docs/source/mobile_reactive.rst rename to docs/source/mobile-reactive.rst index 949c60eee..10f8978f5 100644 --- a/docs/source/mobile_reactive.rst +++ b/docs/source/mobile-reactive.rst @@ -1,7 +1,7 @@ Reactive navigation =================== -These planners work with an occupancy grid representation of the world. Start +These algorithms work with an occupancy grid representation of the world. Start and goal are specified by 2D :math:`(x, y)` coordinates in the plane. .. autoclass:: roboticstoolbox.mobile.Bug2 diff --git a/docs/source/mobile_vehicle_animation.rst b/docs/source/mobile-vehicle-animation.rst similarity index 95% rename from docs/source/mobile_vehicle_animation.rst rename to docs/source/mobile-vehicle-animation.rst index 2e3341e21..1a61e4227 100644 --- a/docs/source/mobile_vehicle_animation.rst +++ b/docs/source/mobile-vehicle-animation.rst @@ -1,5 +1,5 @@ -Mobile robot animations ------------------------ +Animations +---------- These classes create a graphical object that can be animated to show vehicle position or pose. diff --git a/docs/source/mobile-vehicle-base.rst b/docs/source/mobile-vehicle-base.rst new file mode 100644 index 000000000..4405e1450 --- /dev/null +++ b/docs/source/mobile-vehicle-base.rst @@ -0,0 +1,8 @@ +Vehicle abstract baseclass +-------------------------- + +.. autoclass:: roboticstoolbox.mobile.VehicleBase + :members: + :undoc-members: + :inherited-members: + :special-members: __init__ diff --git a/docs/source/mobile_vehicle_bicycle.rst b/docs/source/mobile-vehicle-bicycle.rst similarity index 77% rename from docs/source/mobile_vehicle_bicycle.rst rename to docs/source/mobile-vehicle-bicycle.rst index 5522aacf8..483d78440 100644 --- a/docs/source/mobile_vehicle_bicycle.rst +++ b/docs/source/mobile-vehicle-bicycle.rst @@ -1,8 +1,9 @@ -Bicycle model -------------- +Bicycle +------- .. autoclass:: roboticstoolbox.mobile.Bicycle :members: :undoc-members: + :show-inheritance: :inherited-members: :special-members: __init__ diff --git a/docs/source/mobile-vehicle-diffsteer.rst b/docs/source/mobile-vehicle-diffsteer.rst new file mode 100644 index 000000000..b8a345d8d --- /dev/null +++ b/docs/source/mobile-vehicle-diffsteer.rst @@ -0,0 +1,9 @@ +Differential steering +--------------------- + +.. autoclass:: roboticstoolbox.mobile.DiffSteer + :members: + :undoc-members: + :show-inheritance: + :inherited-members: + :special-members: __init__ diff --git a/docs/source/mobile_vehicle_unicycle.rst b/docs/source/mobile-vehicle-unicycle.rst similarity index 83% rename from docs/source/mobile_vehicle_unicycle.rst rename to docs/source/mobile-vehicle-unicycle.rst index 56152941b..da6b6268e 100644 --- a/docs/source/mobile_vehicle_unicycle.rst +++ b/docs/source/mobile-vehicle-unicycle.rst @@ -1,5 +1,5 @@ -Unicycle model --------------- +Unicycle +-------- .. autoclass:: roboticstoolbox.mobile.Unicycle :members: diff --git a/docs/source/mobile_vehicle_vehicle.rst b/docs/source/mobile-vehicle-vehicle.rst similarity index 100% rename from docs/source/mobile_vehicle_vehicle.rst rename to docs/source/mobile-vehicle-vehicle.rst diff --git a/docs/source/mobile-vehicle.rst b/docs/source/mobile-vehicle.rst new file mode 100644 index 000000000..ecaa1b509 --- /dev/null +++ b/docs/source/mobile-vehicle.rst @@ -0,0 +1,17 @@ +Motion models +============= + +These classes describe vehicle motion models, sometimes referred to as +kinematic or kino-dynamic models. They provide methods to: + +* predict new configuration based on odometry +* compute configuration derivative +* simulate and animate motion +* compute Jacobians + +.. toctree:: + + mobile-vehicle-bicycle + mobile-vehicle-unicycle + mobile-vehicle-diffsteer + mobile-vehicle-base diff --git a/docs/source/mobile.rst b/docs/source/mobile.rst index 39d77afcf..f65450212 100644 --- a/docs/source/mobile.rst +++ b/docs/source/mobile.rst @@ -9,9 +9,9 @@ and state estimation. .. toctree:: :maxdepth: 2 - mobile_vehicle - mobile_vehicle_animation - mobile_drivers - mobile_reactive - mobile_planner - mobile_SLAM + mobile-vehicle + mobile-vehicle-animation + mobile-drivers + mobile-planner + mobile-reactive + mobile-SLAM diff --git a/docs/source/mobile_vehicle.rst b/docs/source/mobile_vehicle.rst deleted file mode 100644 index 406e13cd3..000000000 --- a/docs/source/mobile_vehicle.rst +++ /dev/null @@ -1,40 +0,0 @@ -Mobile robot kinematic models -============================= - -These vehicle kinematic classes have methods to: - -* predict new configuration based on odometry -* compute configuration derivative -* simulate and animate motion -* compute Jacobians - -Bicycle model -^^^^^^^^^^^^^ - - .. autoclass:: roboticstoolbox.mobile.Bicycle - :members: - :undoc-members: - :inherited-members: - :show-inheritance: - :special-members: __init__ - -Unicycle model -^^^^^^^^^^^^^^ - -.. autoclass:: roboticstoolbox.mobile.Unicycle - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :special-members: __init__ - - -Superclass -^^^^^^^^^^ - -.. autoclass:: roboticstoolbox.mobile.VehicleBase - :members: - :undoc-members: - :show-inheritance: - :inherited-members: - :special-members: __init__ \ No newline at end of file diff --git a/notebooks/chap7.ipynb b/notebooks/chap7.ipynb deleted file mode 100644 index bee3cbf47..000000000 --- a/notebooks/chap7.ipynb +++ /dev/null @@ -1,154 +0,0 @@ -{ - "cells": [ - { - "cell_type": "code", - "execution_count": 1, - "metadata": { - "tags": [] - }, - "outputs": [], - "source": [ - "%matplotlib widget\n", - "import roboticstoolbox as rp\n", - "import numpy as np\n", - "\n", - "panda = rp.models.DH.Panda()" - ] - }, - { - "cell_type": "code", - "execution_count": 2, - "metadata": { - "scrolled": true, - "tags": [] - }, - "outputs": [ - { - "name": "stdout", - "output_type": "stream", - "text": [ - " \u001b[38;5;1m\u001b[48;5;255m 0.707107 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m 0.707107 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;4m\u001b[48;5;255m 0.088 \u001b[0m\u001b[48;5;255m \u001b[0m\n", - " \u001b[38;5;1m\u001b[48;5;255m 0.707107 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m-0.707107 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;4m\u001b[48;5;255m 0 \u001b[0m\u001b[48;5;255m \u001b[0m\n", - " \u001b[38;5;1m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;1m\u001b[48;5;255m-1 \u001b[0m\u001b[38;5;4m\u001b[48;5;255m 0.823 \u001b[0m\u001b[48;5;255m \u001b[0m\n", - " \u001b[38;5;244m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;244m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;244m\u001b[48;5;255m 0 \u001b[0m\u001b[38;5;244m\u001b[48;5;255m 1 \u001b[0m\u001b[48;5;255m \u001b[0m\n", - "\n" - ] - } - ], - "source": [ - "print(panda.fkine())" - ] - }, - { - "cell_type": "code", - "execution_count": 3, - "metadata": { - "tags": [] - }, - "outputs": [ - { - "data": { - "application/vnd.jupyter.widget-view+json": { - "model_id": "5311f173c29848c0872ecde54e7c67ba", - "version_major": 2, - "version_minor": 0 - }, - "text/plain": [ - "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "data": { - "text/plain": [ - "" - ] - }, - "execution_count": 3, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "panda.teach(block=True)" - ] - }, - { - "cell_type": "code", - "execution_count": 5, - "metadata": {}, - "outputs": [ - { - "data": { - "application/vnd.jupyter.widget-view+json": { - "model_id": "abfe9e42b1d34ac3bb698b7e89efe2e9", - "version_major": 2, - "version_minor": 0 - }, - "text/plain": [ - "Canvas(toolbar=Toolbar(toolitems=[('Home', 'Reset original view', 'home', 'home'), ('Back', 'Back to previous …" - ] - }, - "metadata": {}, - "output_type": "display_data" - }, - { - "data": { - "text/plain": [ - "" - ] - }, - "execution_count": 5, - "metadata": {}, - "output_type": "execute_result" - } - ], - "source": [ - "panda.plot(block=False)" - ] - }, - { - "cell_type": "code", - "execution_count": null, - "metadata": {}, - "outputs": [], - "source": [] - } - ], - "metadata": { - "kernelspec": { - "display_name": "Python 3", - "language": "python", - "name": "python3" - }, - "language_info": { - "codemirror_mode": { - "name": "ipython", - "version": 3 - }, - "file_extension": ".py", - "mimetype": "text/x-python", - "name": "python", - "nbconvert_exporter": "python", - "pygments_lexer": "ipython3", - "version": "3.7.6" - }, - "toc": { - "base_numbering": 1, - "nav_menu": {}, - "number_sections": true, - "sideBar": true, - "skip_h1_title": false, - "title_cell": "Table of Contents", - "title_sidebar": "Contents", - "toc_cell": false, - "toc_position": {}, - "toc_section_display": true, - "toc_window_display": false - } - }, - "nbformat": 4, - "nbformat_minor": 2 -} diff --git a/notebooks/ik_benchmark.ipynb b/notebooks/ik_benchmark.ipynb new file mode 100644 index 000000000..2aee45b5d --- /dev/null +++ b/notebooks/ik_benchmark.ipynb @@ -0,0 +1,284 @@ +{ + "cells": [ + { + "cell_type": "code", + "execution_count": 7, + "metadata": {}, + "outputs": [], + "source": [ + "try:\n", + " # We must install required packages if we are in Google Colab\n", + " import google.colab\n", + " %pip install roboticstoolbox-python>=1.0.2\n", + " %pip install qpsolvers[quadprog]\n", + " COLAB = True\n", + "except:\n", + " pass" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "# Numerical Inverse Kinematics Benchmark\n", + "\n", + "
\n", + "\n", + "In this Notebook we benchmark the performance of each numerical IK solver we provide with the toolbox.\n", + "\n", + "There are several ways to use IK solvers with the Toolbox:\n", + "\n", + "* The **fast** methods which are implemented in C++ and wrapped in Python. These methods are available from ``robot`` and ``ets`` classes and start with ``ik_``. For example ``ik_LM`` is a fast C++ based solver.\n", + "\n", + "* The **slow** methods which are implemented in Python. These methods are available from ``robot`` and ``ets`` classes and start with ``ikine_``. For example ``ikine_LM`` is a slow Python based solver.\n", + "\n", + "* The **slow** class-based operation which is implemented in Python. These classes inherit from the ``IKSolver`` class and start with ``IK_``. For example ``IK_LM`` is a class which provides a slow Python based solver.\n", + "\n", + "### Contents\n", + "\n", + "[1.0 Fast IK Methods](#1)\n", + "\n", + "[2.0 Python IK Methods](#2)\n", + "\n", + "[3.0 Python Class Based IK Methods](#3)\n", + "\n" + ] + }, + { + "cell_type": "code", + "execution_count": 8, + "metadata": {}, + "outputs": [], + "source": [ + "# We will do the imports required for this notebook here\n", + "\n", + "# numpy provides import array and linear algebra utilities\n", + "import numpy as np\n", + "\n", + "# the robotics toolbox provides robotics specific functionality\n", + "import roboticstoolbox as rtb\n", + "\n", + "# ansitable is a great package for printing tables in a terminal\n", + "from ansitable import ANSITable\n", + "\n", + "# a package for creating dynamic progress bars\n", + "from progress.bar import Bar\n", + "\n", + "# swift is a lightweight browser-based simulator which comes with the toolbox\n", + "from swift import Swift\n", + "\n", + "# spatialgeometry is a utility package for dealing with geometric objects\n", + "import spatialgeometry as sg\n", + "\n", + "# provides sleep functionaly\n", + "import time\n", + "\n", + "# suppress warnings\n", + "import warnings\n", + "warnings.filterwarnings('ignore')" + ] + }, + { + "attachments": {}, + "cell_type": "markdown", + "metadata": {}, + "source": [ + "
\n", + "\n", + "\n", + "\n", + "### 1.0 Fast IK Methods\n", + "---" + ] + }, + { + "cell_type": "code", + "execution_count": 9, + "metadata": {}, + "outputs": [], + "source": [ + "# Our robot and ETS\n", + "robot = rtb.models.Panda()\n", + "ets = robot.ets()" + ] + }, + { + "cell_type": "code", + "execution_count": 10, + "metadata": {}, + "outputs": [], + "source": [ + "# Number of problems to solve\n", + "speed_problems = 10000\n", + "\n", + "# Cartesion DoF priority matrix\n", + "mask = np.array([1.0, 1.0, 1.0, 1.0, 1.0, 1.0])\n", + "\n", + "# random valid q values which will define Tep\n", + "q_rand = ets.random_q(speed_problems)\n", + "\n", + "# Our desired end-effector poses\n", + "Tep = np.zeros((speed_problems, 4, 4))\n", + "\n", + "for i in range(speed_problems):\n", + " Tep[i] = ets.eval(q_rand[i])\n", + "\n", + "# Maximum iterations allowed in a search\n", + "ilimit = 30\n", + "\n", + "# Maximum searches allowed per problem\n", + "slimit = 100\n", + "\n", + "# Solution tolerance\n", + "tol = 1e-6\n", + "\n", + "joint_limits = False" + ] + }, + { + "cell_type": "code", + "execution_count": 11, + "metadata": {}, + "outputs": [], + "source": [ + "speed_solvers = [\n", + " lambda Tep: ets.ik_NR(\n", + " Tep,\n", + " ilimit=ilimit,\n", + " slimit=slimit,\n", + " tol=tol,\n", + " mask=mask,\n", + " pinv=True,\n", + " joint_limits=joint_limits,\n", + " ),\n", + " lambda Tep: ets.ik_GN(\n", + " Tep,\n", + " ilimit=ilimit,\n", + " slimit=slimit,\n", + " tol=tol,\n", + " mask=mask,\n", + " pinv=True,\n", + " joint_limits=joint_limits,\n", + " ),\n", + " lambda Tep: ets.ik_LM(\n", + " Tep, ilimit=ilimit, slimit=slimit, tol=tol, mask=mask, k=1e-4, joint_limits=joint_limits, method=\"wampler\"\n", + " ),\n", + " lambda Tep: ets.ik_LM(\n", + " Tep,\n", + " q0=None,\n", + " ilimit=ilimit,\n", + " slimit=slimit,\n", + " tol=tol,\n", + " mask=mask,\n", + " k=0.1,\n", + " joint_limits=joint_limits,\n", + " method=\"chan\",\n", + " ),\n", + " lambda Tep: ets.ik_LM(\n", + " Tep, ilimit=ilimit, slimit=slimit, tol=tol, mask=mask, k=0.0001, joint_limits=joint_limits, method=\"sugihara\"\n", + " ),\n", + "]\n", + "\n", + "speed_names = [\n", + " \"Newton Raphson (pinv=True)\",\n", + " \"Gauss Newton (pinv=True)\",\n", + " \"LM Wampler 1e-4\",\n", + " \"LM Chan 0.1\",\n", + " \"LM Sugihara 0.0001\",\n", + "]\n", + "\n", + "times = []" + ] + }, + { + "cell_type": "code", + "execution_count": 12, + "metadata": {}, + "outputs": [ + { + "name": "stdout", + "output_type": "stream", + "text": [ + "Solving with Newton Raphson (pinv=True)\n", + "Solving with Gauss Newton (pinv=True)\n", + "Solving with LM Wampler 1e-4\n", + "Solving with LM Chan 0.1\n", + "Solving with LM Sugihara 0.0001\n", + "\n", + "Numerical Inverse Kinematics Methods Times Compared over 10000 problems\n", + "\n", + "┌───────────────────────────┬────────────────┬────────────────────────────────┐\n", + "│ Method │ Total Time (s) │ Average Time per Solution (μs) │\n", + "├───────────────────────────┼────────────────┼────────────────────────────────┤\n", + "│Newton Raphson (pinv=True)\u001b[0m │ 1.8708\u001b[0m │ 187.0795\u001b[0m │\n", + "│ Gauss Newton (pinv=True)\u001b[0m │ 2.474\u001b[0m │ 247.3969\u001b[0m │\n", + "│ LM Wampler 1e-4\u001b[0m │ 0.6079\u001b[0m │ 60.7928\u001b[0m │\n", + "│ LM Chan 0.1\u001b[0m │ 0.2724\u001b[0m │ 27.2353\u001b[0m │\n", + "│ LM Sugihara 0.0001\u001b[0m │ 0.3546\u001b[0m │ 35.4575\u001b[0m │\n", + "└───────────────────────────┴────────────────┴────────────────────────────────┘\n", + "\n" + ] + } + ], + "source": [ + "for name, solver in zip(speed_names, speed_solvers):\n", + " print(f\"Solving with {name}\")\n", + "\n", + " start = time.time()\n", + "\n", + " for i in range(speed_problems):\n", + " solver(Tep[i])\n", + "\n", + " total_time = time.time() - start\n", + " times.append(total_time)\n", + "\n", + "\n", + "print(f\"\\nNumerical Inverse Kinematics Methods Times Compared over {speed_problems} problems\\n\")\n", + "\n", + "table = ANSITable(\n", + " \"Method\",\n", + " \"Total Time (s)\",\n", + " \"Average Time per Solution (μs)\",\n", + " border=\"thin\",\n", + ")\n", + "\n", + "for name, t in zip(speed_names, times):\n", + " table.row(\n", + " name,\n", + " np.round(t, 4),\n", + " np.round((t / speed_problems) * 1e6, 4),\n", + " )\n", + "\n", + "table.print()" + ] + } + ], + "metadata": { + "kernelspec": { + "display_name": "rtb", + "language": "python", + "name": "python3" + }, + "language_info": { + "codemirror_mode": { + "name": "ipython", + "version": 3 + }, + "file_extension": ".py", + "mimetype": "text/x-python", + "name": "python", + "nbconvert_exporter": "python", + "pygments_lexer": "ipython3", + "version": "3.11.0" + }, + "orig_nbformat": 4, + "vscode": { + "interpreter": { + "hash": "4e8f2602a343ae62ff483b4df7805ab0eb447ccc1a56caf0631d5620d36b21ab" + } + } + }, + "nbformat": 4, + "nbformat_minor": 2 +} diff --git a/pyproject.toml b/pyproject.toml index c2adc7322..85c016caa 100644 --- a/pyproject.toml +++ b/pyproject.toml @@ -4,7 +4,7 @@ name = "roboticstoolbox-python" description = "A Python library for robotics education and research" -version = "1.0.3" +version = "1.1.0" authors = [ { name = "Jesse Haviland", email = "j.haviland@qut.edu.au" }, @@ -13,37 +13,52 @@ authors = [ dependencies = [ "numpy>=1.17.4", - "spatialmath-python~=1.0.0", - "spatialgeometry~=1.0.0", + "spatialmath-python>=1.1.5", + "spatialgeometry>=1.0.0", "pgraph-python", "scipy", "matplotlib", "ansitable", - "swift-sim~=1.0.0", + "swift-sim>=1.0.0", "rtb-data", "progress", + "typing_extensions", ] -license = {file = "LICENSE"} +license = { file = "LICENSE" } readme = "README.md" requires-python = ">=3.7" -keywords = ["python", "robotics", "robotics-toolbox", "kinematics", "dynamics", "motion-planning", "trajectory-generation", "jacobian", "hessian", "control", "simulation", "robot-manipulator", "mobile-robot"] +keywords = [ + "python", + "robotics", + "robotics-toolbox", + "kinematics", + "dynamics", + "motion-planning", + "trajectory-generation", + "jacobian", + "hessian", + "control", + "simulation", + "robot-manipulator", + "mobile-robot", +] classifiers = [ - "Development Status :: 5 - Production/Stable", - # Indicate who your project is intended for - "Intended Audience :: Developers", - # Pick your license as you wish (should match "license" above) - "License :: OSI Approved :: MIT License", - # Specify the Python versions you support here. - "Programming Language :: Python :: 3.7", - "Programming Language :: Python :: 3.8", - "Programming Language :: Python :: 3.9", - "Programming Language :: Python :: 3.10", - "Programming Language :: Python :: 3.11", + "Development Status :: 5 - Production/Stable", + # Indicate who your project is intended for + "Intended Audience :: Developers", + # Pick your license as you wish (should match "license" above) + "License :: OSI Approved :: MIT License", + # Specify the Python versions you support here. + "Programming Language :: Python :: 3.7", + "Programming Language :: Python :: 3.8", + "Programming Language :: Python :: 3.9", + "Programming Language :: Python :: 3.10", + "Programming Language :: Python :: 3.11", ] @@ -53,12 +68,9 @@ documentation = "https://petercorke.github.io/robotics-toolbox-python/" repository = "https://github.com/petercorke/robotics-toolbox-python" - [project.optional-dependencies] -collision = [ - "pybullet" -] +collision = ["pybullet"] dev = [ "black", @@ -67,16 +79,20 @@ dev = [ "flake8", "pyyaml", "sympy", + "qpsolvers", + "quadprog", + "pybullet", + "bdsim", ] docs = [ "sphinx", "sphinx_rtd_theme", - "sphinx-autorun", + "sphinx-autorun", + "sphinx_autodoc_typehints", ] - [project.scripts] eigdemo = "roboticstoolbox.examples.eigdemo:main" @@ -87,14 +103,10 @@ rtbtool = "roboticstoolbox.bin.rtbtool:main" [build-system] -requires = [ - "setuptools", - "oldest-supported-numpy" -] +requires = ["setuptools", "oldest-supported-numpy"] build-backend = "setuptools.build_meta" - [tool.setuptools] packages = [ @@ -102,7 +114,6 @@ packages = [ "roboticstoolbox.backends", "roboticstoolbox.backends.PyPlot", "roboticstoolbox.backends.swift", - "roboticstoolbox.backends.VPython", "roboticstoolbox.bin", "roboticstoolbox.blocks", "roboticstoolbox.examples", @@ -119,11 +130,12 @@ packages = [ ] - [tool.black] line-length = 88 target_version = ['py37'] +args = '--experimental-string-processing' + include = '\.pyi?$' exclude = ''' @@ -146,9 +158,9 @@ exclude = ''' ''' - [tool.flake8] +ignore = ['F821', 'W503', 'E741'] max-line-length = 88 extend-ignore = 'E203' @@ -187,4 +199,5 @@ archs = ["x86_64", "aarch64"] archs = ["AMD64", "x86"] - +[tool.coverage.run] +omit = ["roboticstoolbox/examples/*", "test_*.py"] diff --git a/roboticstoolbox/__init__.py b/roboticstoolbox/__init__.py index 53471bb88..2890c4d5a 100644 --- a/roboticstoolbox/__init__.py +++ b/roboticstoolbox/__init__.py @@ -13,6 +13,7 @@ "backends", # robot "Robot", + "Robot2", "SerialLink", "DHRobot", "Link", @@ -25,7 +26,6 @@ "PoELink", "PoEPrismatic", "PoERevolute", - "ERobot", "ELink", "ELink2", "Link", @@ -40,6 +40,7 @@ # tools "null", "angle_axis", + "angle_axis_python", "p_servo", "Ticker", "quintic", @@ -94,3 +95,9 @@ "EKF", "ParticleFilter", ] + +try: + import importlib.metadata + __version__ = importlib.metadata.version("roboticstoolbox-python") +except: + pass diff --git a/roboticstoolbox/backends/PyPlot/EllipsePlot.py b/roboticstoolbox/backends/PyPlot/EllipsePlot.py index e61276b82..1fff39ecf 100644 --- a/roboticstoolbox/backends/PyPlot/EllipsePlot.py +++ b/roboticstoolbox/backends/PyPlot/EllipsePlot.py @@ -4,6 +4,7 @@ """ import numpy as np + # import scipy as sp from spatialmath import base import matplotlib.pyplot as plt @@ -11,7 +12,6 @@ class ShapePlot: def __init__(self, shape, wireframe=True, **kwargs): - self.shape = shape # reference to the spatialgeom shape self.wireframe = wireframe self.args = kwargs @@ -52,8 +52,7 @@ def make(self): class EllipsePlot: - def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1): - + def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1.0): super(EllipsePlot, self).__init__() try: @@ -64,7 +63,7 @@ def __init__(self, robot, q, etype, opt="trans", centre=[0, 0, 0], scale=1): except TypeError: if centre != "ee": raise ValueError( - "Centre must be a three vector or 'ee' meaning" "end-effector" + "Centre must be a three vector or 'ee' meaningend-effector" ) self.ell = None @@ -90,7 +89,11 @@ def draw(self): self.make_ellipsoid() if self.ell is not None: - self.ax.collections.remove(self.ell) + self.ell.remove() + # print(type(self.ell)) + # print(type(self.ax)) + # assert True == False + # self.ax.collections.remove(self.ell) self.ell = self.ax.plot_wireframe( self.x, self.y, self.z, rstride=6, cstride=6, color="#2980b9", alpha=0.2 @@ -180,29 +183,21 @@ def make_ellipsoid2(self): A = J @ J.T elif self.opt == "rot": raise ValueError( - "Can not do rotational ellipse for a 2d robot plot." " Set opt='trans'" + "Can not do rotational ellipse for a 2d robot plot. Set opt='trans'" ) - # if not self.vell: - # # Do the extra step for the force ellipse - # try: - # A = np.linalg.inv(A) - # except: - # A = np.zeros((2,2)) + # velocity ellipsoid is E = A^-1 + # force ellipsoid is E = A + # + # rather than compute the inverse we can have base.ellipse() do it for us + # using the inverted argument. For the velocity ellipse we already have the + # inverse of E, it is A, so we set inverted=True. if isinstance(self.centre, str) and self.centre == "ee": centre = self.robot.fkine(self.q).A[:3, -1] else: centre = self.centre - # points on unit circle - # theta = np.linspace(0.0, 2.0 * np.pi, 50) - # y = np.array([np.cos(theta), np.sin(theta)]) - # RVC2 p 602 - # x = sp.linalg.sqrtm(A) @ y - - x, y = base.ellipse(A, inverted=True, centre=centre[:2], scale=self.scale) + x, y = base.ellipse(A, inverted=self.vell, centre=centre[:2], scale=self.scale) self.x = x self.y = y - # = x[0,:] * self.scale + centre[0] - # self.y = x[1,:] * self.scale + centre[1] diff --git a/roboticstoolbox/backends/PyPlot/PyPlot.py b/roboticstoolbox/backends/PyPlot/PyPlot.py index 5d9d86d4d..055ec0c94 100644 --- a/roboticstoolbox/backends/PyPlot/PyPlot.py +++ b/roboticstoolbox/backends/PyPlot/PyPlot.py @@ -20,6 +20,7 @@ try: import matplotlib import matplotlib.pyplot as plt + # from mpl_toolkits.mplot3d import Axes3D from matplotlib.widgets import Slider @@ -77,10 +78,18 @@ def __init__(self): ) def __repr__(self): - s = f"PyPlot3D backend, t = {self.sim_time}, scene:" + s = "" for robot in self.robots: - s += f"\n {robot.robot.name}" - return s + s += f" robot: {robot.name}\n" + for ellipse in self.ellipses: + s += f" ellipse: {ellipse}\n" + for shape in self.shapes: + s += f" shape: {shape}\n" + + if s == "": + return f"PyPlot3D backend, t = {self.sim_time}, empty scene" + else: + return f"PyPlot3D backend, t = {self.sim_time}, scene:\n" + s def launch(self, name=None, fig=None, limits=None, **kwargs): """ @@ -189,7 +198,7 @@ def step(self, dt=0.05): pass else: # pragma: no cover raise ValueError( - "Invalid robot.control_type. " "Must be one of 'p', 'v', or 'a'" + "Invalid robot.control_type. Must be one of 'p', 'v', or 'a'" ) # plt.ioff() @@ -311,7 +320,11 @@ def add( super().add() - if isinstance(ob, rp.DHRobot) or isinstance(ob, rp.ERobot): + if ( + isinstance(ob, rp.DHRobot) + or isinstance(ob, rp.ERobot) + or isinstance(ob, rp.Robot) + ): self.robots.append( RobotPlot( ob, @@ -345,6 +358,7 @@ def add( plt.show(block=False) self._set_axes_equal() + return id def remove(self, id): diff --git a/roboticstoolbox/backends/PyPlot/PyPlot2.py b/roboticstoolbox/backends/PyPlot/PyPlot2.py index 7691a1eee..d37016b7e 100644 --- a/roboticstoolbox/backends/PyPlot/PyPlot2.py +++ b/roboticstoolbox/backends/PyPlot/PyPlot2.py @@ -47,10 +47,16 @@ def __init__(self): ) def __repr__(self): - s = f"PyPlot2D backend, t = {self.sim_time}, scene:" + s = "" for robot in self.robots: - s += f"\n {robot.name}" - return s + s += f" robot: {robot.name}\n" + for ellipse in self.ellipses: + s += f" ellipse: {ellipse}\n" + + if s == "": + return f"PyPlot2D backend, t = {self.sim_time}, empty scene" + else: + return f"PyPlot2D backend, t = {self.sim_time}, scene:\n" + s def launch(self, name=None, limits=None, **kwargs): """ @@ -62,8 +68,9 @@ def launch(self, name=None, limits=None, **kwargs): labels = ["X", "Y"] - if name is not None: - self.fig = plt.figure(name) + if name is not None and not _isnotebook(): + # jupyter does weird stuff when figures have the same name + self.fig = plt.figure() else: self.fig = plt.figure() @@ -172,7 +179,7 @@ def add(self, ob, readonly=False, display=True, eeframe=True, name=False, **kwar super().add() - if isinstance(ob, rp.ERobot2): + if isinstance(ob, rp.Robot2): self.robots.append(RobotPlot2(ob, self, readonly, display, eeframe, name)) self.robots[len(self.robots) - 1].draw() @@ -228,7 +235,7 @@ def _step_robots(self, dt): else: # pragma: no cover # Should be impossible to reach raise ValueError( - "Invalid robot.control_type. " "Must be one of 'p', 'v', or 'a'" + "Invalid robot.control_type. Must be one of 'p', 'v', or 'a'" ) def _update_robots(self): diff --git a/roboticstoolbox/backends/PyPlot/README.md b/roboticstoolbox/backends/PyPlot/README.md new file mode 100644 index 000000000..565a8a74b --- /dev/null +++ b/roboticstoolbox/backends/PyPlot/README.md @@ -0,0 +1,67 @@ +Backend methods + +- launch, creates the axes +- close, close the axes +- add, a robot, ellipse/oid, shape +- remove, a robot, ellipse/oid, shape +- step, update all shapes + + +## add + +``` +add(shape, [shape specific options]) + + if robot: + # create a mirror object that has a reference to the user's object + # call .draw() on it + if ellipse: + # add it to list + # call .draw() on it +``` + +robot object options: + +- readonly, +- display, +- jointaxes, show joint axes +- jointlabels, show joint labels +- eeframe, show ee frame +- shadow, show shadow +- name, show name +- options, detailed appearance options for robot, shadow, labels, eeframe + +`.draw()` +- onetime, init the line segments +- compute fkine_all +- update line segments, shadows, ee frame + + +ellipse object options: + +- robot, robot it is attached to +- q +- etype, ellipse type: v or f +- opt +- centre +- scale + +Attributes: +- ell, the MPL handle + +`.draw()` +- remove old ellipse +- compute Jacobian to get ellipse matrix +- compute fkine to get ellipse centre +- draw new ellipse + +## step + +``` +step(dt) + + # update state by control mode p/v/a + # call .draw() on all robots in list + # call .draw() on all ellipses in list +``` + diff --git a/roboticstoolbox/backends/PyPlot/RobotPlot.py b/roboticstoolbox/backends/PyPlot/RobotPlot.py index 023aaf168..5bed8c96f 100644 --- a/roboticstoolbox/backends/PyPlot/RobotPlot.py +++ b/roboticstoolbox/backends/PyPlot/RobotPlot.py @@ -22,7 +22,6 @@ def __init__( name=True, options=None, ): - super(RobotPlot, self).__init__() # Readonly - True for this robot is for displaying only @@ -129,7 +128,7 @@ def draw(self): # add new ee coordinate frame for link in self.robot.ee_links: - Te = T[link.number] + Te = T[link.number] * self.robot.tool # ee axes arrows Tex = Te * Tjx @@ -156,11 +155,15 @@ def draw(self): if self.jointaxes: # Plot joint z coordinates for link in self.robot: - direction = None if isinstance(self.robot, rp.DHRobot): # should test MDH I think - Tj = T[link.number - 1] + + if self.robot.mdh: + Tj = T[link.number] + else: + Tj = T[link.number - 1] + R = Tj.R direction = R[:, 2] # z direction elif link.isjoint: @@ -178,7 +181,6 @@ def draw(self): self.joints.extend(arrow) def init(self): - self.drawn = True if self.env.limits is None: @@ -203,7 +205,6 @@ def init(self): self.links = [] self.sh_links = [] for i in range(len(self.segments)): - # Plot the shadow of the robot links, draw first so robot is always # in front if self.shadow: diff --git a/roboticstoolbox/backends/VPython/README.md b/roboticstoolbox/backends/VPython/README.md deleted file mode 100644 index b695e8a97..000000000 --- a/roboticstoolbox/backends/VPython/README.md +++ /dev/null @@ -1,495 +0,0 @@ -This folder contains the VPython graphics backends of the toolbox. -Instructions on how to use VPython, and the fundamentals of how it works are below. -(Pictures to come) - -# Scene Controls -3D Controls - * Pan - * W -> Forward - * A -> Left - * S -> Backward - * D -> Right - * SPACE -> Up - * SHIFT -> Down - * Spin - * CTRL + LMB -> Free spin - * Left -> Left - * Right -> Right - * Up -> Up - * Down -> Down - * Q -> Roll Left - * E -> Roll Right - * Zoom - * Scrollwheel - -2D Controls - * Pan - * SHIFT + LMB -> Free pan - * W -> Up - * A -> Left - * S -> Down - * D -> Right - * Spin - * Left -> Left - * Right -> Right - * Up -> Up - * Down -> Down - * Q -> Roll Left - * E -> Roll Right - * Zoom - * Scrollwheel - - -# How To -## Setup -To use VPython, simply select is as your backend. -Note: 2D functionality is currently not implemented via the backends. It can still be used outside the backend, however. -```python -import roboticstoolbox as rp - -env = rp.backends.VPython() -``` - -## Setting Up The Backend -Any use of VPython objects requires a scene. - -To create a scene to draw objects to, a canvas must be created. Upon creation, a localhost http server will be opened. -If launched through Jupyter Notebooks, it will be displayed where executed. - -Different attributes can be supplied to the environment for some customisation. The display width, height, title, and -caption can be manually input. Booleans representing the number of dimensions, and grid visibility and colour can be -set. - -Firstly, decide whether a 3D or 2D world is required (even though the 2D is represented in a 3D world, it will have 2D -capabilities) -```python -# Create a default canvas (3D, 500*888, with grid displayed, no title or caption, black opaque grid) -env.launch() - -# Alternatively create a grid with specified parameters -env.launch(is_3d=True, height=768, width=1024, title="Scene 1", caption="This scene shows...", - grid=False, g_col=[0.2, 0.2, 0.2]) - -env.launch(is_3d=False, height=768, width=1024, title="Scene 2", caption="This scene shows...", - grid=False, g_col=[0.2, 0.2, 0.2]) -``` -Multiple scenes can be created, simply execute `launch` for every scene required. Keeping note of the order for future -reference. - -Each scene has a GUI. It gives an interface with canvas controls and a Teachpanel. -The same functionality can be done in code as will be mentioned. - -3D options: - * Toggle the UI mode between Canvas Controls, and a Teachpanel - * The Teachpanel will have a slider for each joint to rotate between all available angles for that joint - * Take a screenshot of the scene. Saves to user's downloads folder. - * Choose which robot to edit - * Toggle robot/frame visibility - * Change robot opacity - * Toggle grid visibility - * Delete a robot from the scene - * Clear all robots visuals from the scene - * Toggle camera lock - * Toggle grid dynamic/static positioning - -2D options: - * Toggle grid visibility - * Toggle camera lock - * Toggle grid dynamic/static positioning - - -## Closing The Backend -You can restart VPython to the initial state of "launch" through `reset`. There is no way to close and reopen a new -session. Thus, `restart` simply calls `reset`. To end the session, simply execute `close`. -```python -# Put VPython back to the state defined by "launch" -env.reset() - -# Restart does the same as reset() -env.restart() - -# Close the session -env.close() -``` - -## Using Robots in VPython -To add a robot to the scene, you require a `Robot` object. -There must also be a scene loaded through `launch`. -Robots can be added to any scene. -`add` takes in a scene number (0-indexed, a name for the robot, and a `Robot` object. -```python -# Add a Puma560 robot to the default scene -from roboticstoolbox.models.DH.Puma560 import Puma560 -puma = Puma560() -env.add(0, 'Puma560', puma) -``` - -Robots can also be removed from a scene. Through a similar procedure, they can be removed given a `Robot` object and a -figure (defaults to 0) to delete from. `VPython.GraphicalRobot` is also accepted in lue of a `Robot`. `GraphicalRobot`s -can be obtained through the `env.robots` list. -```python -# Remove the Puma560 robot from scene 0 -env.remove(puma, 0) -``` - -The `step` function will update a specific robots pose given a set of joint angles. -The function accepts a `Robot`. The joint angles and figure number are optional. -If no joint angles given, it will default to the joint angles internally in the `Robot`. Figure number defaults to 0. -```python -# Set the pose of the Puma560 to the "ready" position -env.step(puma, puma.qr, 0) -``` - -## Recording VPython -In order to record the canvas, there are unique backend functions to call: `record_start` and `record_stop`. Currently, -only one scene in the backend can be recorded at a time. The start function requires inputs of FPS, and a scene number -(defaults to 0). The stop function requires a filename with extension. Files are saved relative to the roboticstoolbox -home directory. Imageio is used to convert all image files into a movie file. - -**NOTE**: The scene is still interactive during recording. E.g. The teachpanel can be used to manipulate the robot while -being recorded for later viewing. - -**WARNING**: Before trying to record, it is suggested to try the "Screenshot" button in the UI first. VPython uses the -browsers download functionality, so enabling auto-download of the file types may be required. Otherwise you could be -spammed with download requests. (Files get removed when converted to a video, so your downloads folder will be -unmodified). -```python -env.record_start(10) # Record scene 0 at 10 fps - -# Sleep for X seconds, and/or manipulate robot here -# e.g. -# env.step(puma, puma.qn, 0) -# time.sleep(3) - -env.record_stop('my_video.avi') # Save as an avi -``` - -  - -  -________________________________________________________________________________________________________________________ - -The following section is not required to know to use the VPython backend in the toolbox. This is documentation for how -to use VPython outside of the backend. -________________________________________________________________________________________________________________________ -# VPython Documentation -## Common Functionality -VPython has its own data types that can be used. Firstly, the `radians()`, and `degrees()` functions convert between -radians and degrees. -The `vector` class is also very crucial to the graphics. It can either represent a vector or a 3D point. - -For convenience, some functions and variables are provided for easy use. `wrap_to_pi()` takes in an angle, and -specification on degrees or radians. It returns the respective angle between -pi and pi. -Three vectors are also supplied for readability to ensure correct axes are used when referencing. `x_axis_vector`, -`y_axis_vector`, `z_axis_vector` can be used to get vectors of an object, for example. -```python -# Wrap an angle (deg) to the range [-pi pi]. use "rad" instead of "deg" for radian angles. -gph.wrap_to_pi("deg", 450) -# Obtain the Z vector representation of the robot link -my_link.get_axis_vector(z_axis_vector) -``` - -## Setting up the scene -To create a scene to draw object to, a canvas must be created. Upon creation, a localhost http server will be opened. -Different attributes can be supplied to the function for some customisation. The display width, height, title, and -caption can be manually input. The grid colour and opacity can also be set. Lastly, a boolean representing the grid -visibility can be set. - -Firstly, decide whether a 3D or 2D world is required (even though the 2D is represented in a 3D world, it will have 2D -capabilities). The scene has a GUI underneath the canvas. It gives an interface to toggle graphics and visibilities. -```python -# Create a default canvas (500*888, with grid displayed, no title or caption, black opaque grid) -g_canvas = GraphicalCanvas3D() -g_canvas = GraphicalCanvas2D() - -# Alternatively create a grid with specified parameters -g_canvas = gph.GraphicsCanvas3D(height=768, width=1024, title="Scene 1", caption="This scene shows...", - grid=False, g_col=[0.2, 0.2, 0.2], g_opc=0.3) - -g_canvas = gph.GraphicsCanvas2D(height=768, width=1024, title="Scene 1", caption="This scene shows...", - grid=False, g_col=[0.2, 0.2, 0.2], g_opc=0.3) -``` - -The GraphicsGrid object has functions to toggle grid visibility. -```python -# Turn off the grid display -g_canvas.grid_visibility(False) -``` -Now that the scene is created, a robot must be created to be displayed. - -At anytime you can clear the scene of all robots (The grid will remain if visible). Note: This will note delete the -objects, they still exist, and can be rendered visible afterwards. However, overwriting/deleting the variables will -free the memory. If an object is overwritten/deleted while still visible, the objects will remain in the scene. - -Isolated joint objects will not be cleared. Only robots in the scene. -```python -g_canvas.clear_scene() -``` - -# 3D Functionality -## Creating Robots -If you want to use the example puma560 robot, simply add the `Robot` to the scene. -It will be displayed in the scene that is provided. -```python -# Add the Puma560 to the scene -from roboticstoolbox.models.DH.Puma560 import Puma560 -puma = Puma560() -g_canvas.add_robot(puma) -``` -Otherwise, robots can be manually created using the `GraphicalRobot` class. -The joints for the robot can be manually or automatically created. -Creation takes in the canvas object to be displayed in, as well as a name for the robot. - -The `GraphicalRobot` optionally takes in a `Robot` object. If given in the initialiser, no joints have to be made, -they are automatically created. The name clause is also overridden by the name specified in the Robot object. - -Firstly, create a `GraphicalRobot` object -```python -# Create an empty robot -my_robot = GraphicalRobot(g_canvas, 'My Robot') - -# Create a robot based on a SerialLink object. -my_robot = GraphicalRobot(g_canvas, '', seriallink=serial_link_obj) -``` -Now we can add joints. The joints added to the robot act like a stack. First joints added will be last to be removed -(if called to). - -### Automatically -If you wish to automatically add joints, use `append_link()`. Add the joints from base to gripper. -This function takes in three arguments. - -Firstly, the type of joint: rotational, prismatic, static, or gripper. -The input is the first letter of the type (case-insensitive). e.g. (rotational = "r" or "R"). - -Next is the initial pose (SE3 type) of the joint. - -The 'structure' of the robot. This variable must either be a `float` representing the joint length, or a `str` -representing a full file path to an STL file. - -If a `float` length is given, a custom rectangular object will represent it in the scene. Otherwise if a `str` path is given, -the STL object will be loaded in and used in place of a rectangular joint. - -Finally, an optional parameter `axis_through` can be used to specify which (local) axis the robot arm is drawn along -It is of a numpy array type, and defaults to `array([0, 1, 0])` (y-axis). - -```python -from numpy import array - -# Append a default base joint of length 2. -my_robot.append_link('r', SE3(), 2.0, axis_through=array([0, 1, 0])) - -# Append an STL obj rotational joint. -my_robot.append_link('r', SE3(), './path/to/file.stl') -``` - -### Manually -Manually adding joints consists of creating a Joint object to add to the robot. -The types that can be created are identical to previously mentioned in the Automatic section. - -`RotationalJoint`, `PrismaticJoint`, `StaticJoint`, `Gripper` are the class names of the different joints. - -To create a joint, each class requires the same variables as the automatic version (minus the joint type string). -Also included must be the scene to display the joint in, similar to importing the Puma560 model. -The `axis_through` parameter is still optional. - -Although the creation process is "the same", manually creating a joint lets you more easily update any graphical issues associated with it. -For example, the STL you want to load may not be orientated/positioned correctly (How to fix is mentioned later) - -WARNING: The joint must be in the same scene as the robot. - -```python -# Create two basic rotational links -link1 = gph.RotationalJoint(SE3(), 1.0, g_canvas, axis_through=array([1, 0, 0])) -link2 = gph.RotationalJoint(SE3(), 1.4, g_canvas) - -# Add to the robot -my_robot.append_made_link(link1) -my_robot.append_made_link(link2) -``` - -To remove the end effector joint, use the `detach_link()` function. Acting like a stack, it will pop the latest joint created off the robot. -```python -# Remove the end effector joint -my_robot.detach_link() -``` - -## Applying Textures / Colours / Opacity -Joints can be coloured, given textures, or both. Using a joint object, calling `set_texture()` will apply the given options. -Textures are given as `str` paths to a local file, or a web hosted image. Colours are input as a list of RGB values. -These values must be normalised from 0 to 1. When both are given, the texture gets colour shifted. -Not supplying a texture will remove any texture already supplied. -Not supplying a colour will set it to white (default). - -**WARNING:** If the texture can't be loaded, the object will have no texture -(appear invisible, but not set as invisible). - -**WARNING:** If the image has a width or height that is not a power of 2 -(that is, not 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, etc.), -the image is stretched to the next larger width or height that is a power of 2. - -```python -new_rot = gph.RotationalJoint(SE3(), 1.0, g_canvas) - -# Load a sample texture -new_rot.set_texture(texture_link="https://s3.amazonaws.com/glowscript/textures/flower_texture.jpg") - -# Green shift the texture -new_rot.set_texture(colour=[0, 0.75, 0], - texture_link="https://s3.amazonaws.com/glowscript/textures/flower_texture.jpg") - -# Remove the texture and red shift -new_rot.set_texture(colour=[1, 0, 0]) - -# Remove all details -new_rot.set_texture() -``` - -Changing the opacity can help show reference frames that are hidden by the robot graphics. -The opacity values must be between 0 and 1 inclusively. -```python -# Change a joint opacity to near invisible -new_rot.set_transparency(0.3) - -# Change a robot to near invisible -my_robot.set_transparency(0.3) -``` - -## Importing an STL object -STL files may not be correctly positioned/oriented when loaded in. -Depending on where the object triangles are configured from the file, the origin of the object may not be where intended. - -When loaded in, the origin is set (by default) to (0, 0, 0). - -Upon observation (through VPython or 3D editor software), you can find the coordinates of the origin in respect to the world. - -A function `set_stl_joint_origin()` is supplied to change the origin. -This method is part of all joint types. It takes two 3D coordinates representing the world coordinates of where the desired origin currently is, and where the desired origin should be. - -For example, if an STL object loads in and the origin is below (-z axis) where it should be, and the origin is at the bottom of the object, the following code will translate it up and set the origin. -```python -# Load the mesh in the link -link = gph.RotationalLink(SE3(), './path/to/file.stl', g_canvas) - -# Obtain the graphical object to help with coordinates -# May not be necessary if you already know the 3D coordinates -stl_obj = link.get_graphic_object() - -# Z origin is below where the current position is. (this e.g. is at bottom of object) -stl_obj_z_origin = stl_obj.pos.z - stl_obj.width / 2 - -# 3D pos of where the origin is -stl_obj_current_origin_location = vector(stl_obj.pos.x, stl_obj.pos.y, stl_obj_z_origin) - -# 3D pos of where the origin should be set in the world -stl_obj_required_origin_location = vector(stl_obj.pos.x, stl_obj.pos.y, 0) - -# Move the object to place origin where it should be, and apply the new origin to the object -link.set_stl_joint_origin(stl_obj_current_origin_location, stl_obj_required_origin_location) -``` - -## Using A GraphicalRobot -The robot class has two functions that handle the display. One function each to toggle the visibility for the joints and reference frames. -```python -# Turn off the robots reference frame displays -my_graphic_robot.set_reference_visibility(False) - -# Toggle the robot visibility -my_graphic_robot.set_robot_visibility(not my_graphic_robot.is_shown) -``` - -To update the joint positions, use the `set_joint_poses()` function. It takes in a list of SE3 objects for each of the joints. -There must be 1 pose for each joint, in order from base to gripper (order of appending in creation) -```python -# Set all joint poses to a random configuration -# Assuming 3 joint robot -my_graphical_robot.set_joint_poses([ - SE3().Rand(), - SE3().Rand(), - SE3().Rand() -]) -``` - -Alternatively, an `animate` function allows the robot to iterate through given poses to simulate movement. -Given an array of poses (per frame), and a frame rate, the robot will transition through each pose. -```python -my_graphical_robot.animate([ - [pose1, pose2, pose3], # Frame 1 - [pose1, pose2, pose3], # Frame 2 - [pose1, pose2, pose3], # Frame 3 - [pose1, pose2, pose3]], # Frame 4 - 4) # 4 FPS -``` - -Lastly, a print function `print_joint_poses()` will print out the current poses of all joints. -```python -# Print joint poses -my_graphical_robot.print_joint_poses() -``` - -# 2D Functionality -## Plotting -Plotting lines in 2D works the same way as it does in MATLAB plotting. -If you are familiar with the plot syntax, you can effectively skip this part. - -The `plot()` function takes in lists of axis values, and an optional string for plot options. - -If given one axis of data, it will plot along the Y axis, with respect to index on the X axis. -If given two axes of data, it will plot first list (X) by second list (Y). - -The string option input is the same as MATLAB's plot options. -The string can contain up to three options (one of each): -1. Line type -2. Marker type -3. Colour - - - Colour Marker Type Line Type - b blue . point - solid - g green o circle : dotted - r red x x-mark -. dashdot - c cyan + plus -- dashed - m magenta * star (none) no line - y yellow s square - k black d diamond - w white v triangle (down) - ^ triangle (up) - < triangle (left) - > triangle (right) - p pentagram - h hexagram - (none) no marker - -Notes: -* If you do not specify a marker type, plot uses no marker. -* If you do not specify a line style, plot uses a solid line (unless a marker type is given, then no line). -* Options can be in any order - -```python -# Plot a list of values in default options -g_canvas.plot([1, 2, 3, 4]) - -# Plot a list of values in red dashed lines -g_canvas.plot([1, 2, 3, 4], 'r--') # '--r' is also valid - -# Plot a list of values with no line between, and diamond markers in blue -g_canvas.plot([1, 2, 3, 4], 'bd') # 'db' is also valid - -# Plot X by Y, in green, with cross markers, dashdot line, in yellow -g_canvas.plot([3, 5, 2, 7], [6, 2, 8, 4], 'gx-.') # 'x-.g' (etc) is also valid -``` - -## 2D Objects -Similar to a robot in the 3D scene, you can create a 2D object to move around the scene. -Currently, only shapes are supported for the 2D object, but future additions will allow STLs. -Object position/graphic update functions to come. - -The object initialiser takes in an SE2 pose, the scene to draw in, it's shape and colour. - -```python -# Position at (1, 1) -se2 = SE2(x=1, y=1, theta=0) -colour = [0, 0, 0] # Black -shape = '*' - -my_object = gph.Object2D(se2, g_canvas, shape, colour) -``` \ No newline at end of file diff --git a/roboticstoolbox/backends/VPython/VPython.py b/roboticstoolbox/backends/VPython/VPython.py deleted file mode 100644 index fe0a1245a..000000000 --- a/roboticstoolbox/backends/VPython/VPython.py +++ /dev/null @@ -1,527 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -import importlib -import threading -import glob -import os -import platform -import warnings -from time import perf_counter, sleep -import imageio -from roboticstoolbox.backends.Connector import Connector - -_GraphicsCanvas3D = None -_GraphicsCanvas2D = None -_GraphicalRobot = None -close_localhost_session = None - -try: - from roboticstoolbox.backends.VPython.canvas import GraphicsCanvas2D, GraphicsCanvas3D, UImode - from roboticstoolbox.backends.VPython.graphicalrobot import GraphicalRobot - from roboticstoolbox.backends.VPython.grid import GridType -except ImportError: - print( - '\nYou must install the VPython component of the toolbox, do: \n' - 'pip install roboticstoolbox[vpython]\n\n') - - -class VPython(Connector): # pragma nocover - """ - Graphical backend using VPython - - VPython is a Python API that connects to a JavaScript/WebGL 3D graphics - engine in a browser tab. It supports many 3D graphical primitives - including meshes, boxes, ellipsoids and lines. It can not render in - full color. - - Example: - - .. code-block:: python - :linenos: - - import roboticstoolbox as rtb - - robot = rtb.models.DH.Panda() # create a robot - - pyplot = rtb.backends.VPython() # create a VPython backend - pyplot.add(robot) # add the robot to the backend - robot.q = robot.qz # set the robot configuration - pyplot.step() # update the backend and graphical view - - :references: - - - https://vpython.org - - """ - # TODO be able to add ellipsoids (vellipse, fellipse) - # TODO be able add lines (for end-effector paths) - - def __init__(self, **kwargs): - """ - Open a localhost session with no canvases - - """ - super(VPython, self).__init__() - - # Init vars - self.canvases = [] - # 2D array of [is_3d, height, width, title, caption, grid] per canvas - self.canvas_settings = [] - self.robots = [] - self._recording = False - self._recording_thread = None - self._recording_fps = None - self._thread_lock = threading.Lock() - - self.launch_options = kwargs # save launch options - - self._create_empty_session() - - def __repr__(self): - s = f"VPython backend, t = {self.sim_time}, scene:" - for robot in self.robots: - s += f"\n {robot.name}" - return s - - def launch( - self, **kwargs): - """ - Launch a graphical backend in a browser tab - - ``env = launch(args)` create a 3D scene in a new browser tab as - defined by args, and returns a reference to the backend. - - """ - - # merge instantiation & launch options - args = {**self.launch_options, **kwargs} - is_3d = args.get('is_3d', True) - height = args.get('height', 500) - width = args.get('width', 888) - title = args.get('title', 'Robotics Toolbox for Python: VPython display') - caption = args.get('caption', '') - grid = args.get('grid', False) - if is_3d: - g_type = args.get('g_type', GridType.XY3D) - else: - g_type = args.get('g_type', GridType.XY2D) - g_col = args.get('g_col', None) - - super().launch() - - self.canvas_settings.append( - [is_3d, height, width, title, caption, grid, g_type, g_col]) - - # Create the canvas with the given information - if is_3d: - self.canvases.append( - GraphicsCanvas3D(height, width, title, caption, - grid, g_type, g_col)) - else: - self.canvases.append( - GraphicsCanvas2D(height, width, title, caption, - grid, g_type, g_col)) - - self.sim_time = 0 - - def step(self, dt=None, id=None, q=None, fig_num=0): - """ - Update the graphical scene - - :param id: The Identification of the robot to move. Can be either the - DHRobot or GraphicalRobot - :type id: :class:`~roboticstoolbox.robot.DHRobot.DHRobot`, - :class:`roboticstoolbox.backends.VPython.graphics_robot.GraphicalRobot` - :param q: The joint angles/configuration of the robot (Optional, if not - supplied will use the stored q values). - :type q: float ndarray(n) - :param fig_num: The canvas index to delete the robot from, defaults to - the initial one - :type fig_num: int, optional - :raises ValueError: Figure number must be between 0 and total number of - canvases - :raises TypeError: Input must be a DHLink or GraphicalRobot - - ``env.step(args)`` triggers an update of the 3D scene in the browser - window referenced by ``env``. - - .. note:: - - - Each robot in the scene is updated based on - their control type (position, velocity, acceleration, or torque). - - Upon acting, the other three of the four control types will be - updated in the internal state of the robot object. - - The control type is defined by the robot object, and not all - robot objects support all control types. - - Execution is blocked for the specified interval - - """ - - super().step() - - self.sim_time += dt - - if fig_num < 0 or fig_num >= len(self.canvases): - raise ValueError( - "Figure number must be between 0 and total number of canvases") - - # If GraphicalRobot given - if isinstance(id, GraphicalRobot): - if self.canvases[fig_num].is_robot_in(id): - id.fkine_and_set(q) - if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL: - # Reload the joint sliders - self.canvases[fig_num].teach_mode(teach=True) - - # If DHRobot is given (or equivalent) - else: - graphical_dh_robot = None - # If no ID given, and there are robots available - if id is None and len(self.robots) > 0: - # Obtain the first one - graphical_dh_robot = self.robots[0] - # If no ID, and no robots available - elif id is None: - print("No robot found") - return - else: - # Find first occurrence of it that is in the correct canvas - for i in range(len(self.robots)): - if self.robots[i].robot is id and \ - self.canvases[fig_num].is_robot_in_canvas( - self.robots[i]): - graphical_dh_robot = self.robots[i] - break - - # If no graphical equivalent found, return - if graphical_dh_robot is None: - print("No robot found") - return - # Set poses of graphical robot - graphical_dh_robot.fkine_and_set(q) - - if self.canvases[fig_num].current_mode() == UImode.TEACHPANEL: - # Reload the joint sliders - self.canvases[fig_num].teach_mode(teach=True) - - if dt is not None: - sleep(dt) - - def reset(self): - """ - Reset the graphical scene - - ``env.reset()`` triggers a reset of the 3D scene in the browser window - referenced by ``env``. It is restored to the original state defined by - ``launch()``. - - """ - - super().reset() - - if len(self.canvases) > 0: - # Clear localhost - self.canvases[0].scene.append_to_caption(''' - - ''') - - # Delete all sessions - self.canvases = [] - - self._create_empty_session() - for settings in self.canvas_settings: - # Create the canvas with the given information - if settings[0]: - self.canvases.append(GraphicsCanvas3D( - settings[1], settings[2], settings[3], - settings[4], settings[5], settings[6], settings[7])) - else: - self.canvases.append(GraphicsCanvas2D( - settings[1], settings[2], settings[3], - settings[4], settings[5], settings[6], settings[7])) - - def restart(self): - """ - Restart the graphics display - - ``env.restart()`` triggers a restart of the browser view referenced by - ``env``. It is closed and relaunched to the original state defined by - ``launch()``. - - """ - - super().restart() - - self.reset() - - def close(self): - """ - Close the graphics display - - ``env.close()`` gracefully closes the browser tab browser view - referenced by ``env``. - - """ - - super().close() - - # Close session - if len(self.canvases) > 0: - # if a canvas made - close_localhost_session(self.canvases[0]) - else: - # No canvas, so make one - temp = GraphicsCanvas2D() - close_localhost_session(temp) - - self.canvases = [] - - def add(self, dhrobot, fig_num=0, name=None, **kwargs): - """ - Add a robot to the graphical scene - - :param dhrobot: The ``DHRobot`` object (if applicable) - :type dhrobot: class:`~roboticstoolbox.robot.DHRobot.DHRobot`, None - :param fig_num: The canvas number to place the robot in - :type fig_num: int - :param name: The name of the robot - :type name: `str` - :raises ValueError: Figure number must be between 0 and number of - figures created - :return: object id within visualizer - :rtype: int - - ``id = env.add(robot)`` adds the ``robot`` to the graphical - environment. - - .. note:: - - - ``robot`` must be of an appropriate class. - - Adds the robot object to a list of robots which will be updated - when the ``step()`` method is called. - - """ - - # TODO - name can come from the robot object, maybe an override name? - # Micah: "Name is used from robot class, unless robot is not given" - - # TODO - why dhrobot "if applicable"? - # Micah: "It's possible to create a graphical robot - # in VPython not using a robot class." - - # TODO - what about other classes of robot? - # Micah: "I use specific parameters in dhrobots. - # If they exist in other robot classes, it should work." - - # TODO - what about adding ellipsoids? - - super().add() - - if name is None: - name = dhrobot.name - - # Sanity check input - if fig_num < 0 or fig_num > len(self.canvases) - 1: - raise ValueError( - "Figure number must be between 0 and number " - "of figures created") - - # Add robot to canvas - self.robots.append( - GraphicalRobot(self.canvases[fig_num], name, dhrobot)) - # self.canvases[fig_num].add_robot(self.robots[len(self.robots)-1]) - - def remove(self, id, fig_num=0): - """ - Remove a robot to the graphical scene - - :param id: The id of the robot to remove. Can be either the DHLink or - GraphicalRobot - :type id: class:`~roboticstoolbox.robot.DHRobot.DHRobot`, - class:`roboticstoolbox.backends.VPython.graphics_robot.GraphicalRobot` - :param fig_num: The canvas index to delete the robot from, defaults to - the initial one - :type fig_num: int, optional - :raises ValueError: Figure number must be between 0 and total number - of canvases - :raises TypeError: Input must be a DHLink or GraphicalRobot - - ``env.remove(robot)`` removes the ``robot`` from the graphical - environment. - - """ - - super().remove() - - if fig_num < 0 or fig_num >= len(self.canvases): - raise ValueError( - "Figure number must be between 0 and total number of canvases") - - # If DHLink given - if isinstance(id, DHLink): - robot = None - # Find first occurrence of it that is in the correct canvas - for i in range(len(self.robots)): - if self.robots[i].seriallink.equal(id) and \ - self.canvases[fig_num].is_robot_in(self.robots[i]): - robot = self.robots[i] - break - if robot is None: - return - else: - self.canvases[fig_num].delete_robot(robot) - # ElseIf GraphicalRobot given - elif isinstance(id, GraphicalRobot): - if self.canvases[fig_num].is_robot_in(id): - self.canvases[fig_num].delete_robot(id) - # Else - else: - raise TypeError("Input must be a DHLink or GraphicalRobot") - - def hold(self): # pragma: no cover - ''' - hold() keeps the tab open i.e. stops the tab from closing once - the main script has finished. - - ''' - - while True: - pass - - def _add_teach_panel(self): - # just need to change the display mode - self.canvases[0].teach_mode(True) - - # - # Public non-standard methods - # - def record_start(self, fps, scene_num=0): - """ - Start recording screencaps of a scene - """ - self._thread_lock.acquire() - - if not self._recording: - print("VPython Recording...") - if fps > 10: - warnings.warn("The chosen recording fps ({0}) could result in lagging video quality." - "Consider lowering fps and robot speed (e.g. 5fps)".format(fps), RuntimeWarning) - self._recording = True - self._recording_fps = fps - # Spawn a thread - self._recording_thread = threading.Thread(target=self._record_scene, args=(scene_num, fps,)) - self._recording_thread.start() - - self._thread_lock.release() - - def record_stop(self, filename, save_fps=None): - """ - Stop recording screencaps of a scene and combine them into a movie - Save_fps is different to record fps. Will save the media file at the given save fps. - """ - # - self._thread_lock.acquire() - if self._recording: - self._recording = False - print("VPython Recording Stopped...") - print("VPython Recording Saving... DO NOT EXIT") - else: - self._thread_lock.release() - return - self._thread_lock.release() - - # Wait for thread to finish - self._recording_thread.join() - - sleep(3) # Quick sleep to ensure all downloads are done - # (higher framerates can lag behind) - - # Get downloads directory - opsys = platform.system() - if opsys == 'Windows': # Windows - path_in = os.path.join(os.getenv('USERPROFILE'), 'downloads') - - elif opsys == 'Linux' or opsys == 'Darwin': # Linux / Mac - path_in = os.path.join(os.getenv('HOME'), 'downloads') - - else: # Undefined OS - # lets assume 'HOME' for now - path_in = os.path.join(os.getenv('HOME'), 'downloads') - - fp_out = filename - fp_in = path_in + "/vpython_*.png" - - files = [file for file in glob.glob(fp_in)] - - if save_fps is None: - save_fps = self._recording_fps - writer = imageio.get_writer(fp_out, fps=save_fps) - - for f in files: - writer.append_data(imageio.imread(f)) # Add it to the video - os.remove(f) # Clean up file - - writer.close() - - print("VPython Recording Saved... It is safe to exit") - - # - # Private Methods - # - - @staticmethod - def _create_empty_session(): - """ - Create a canvas to ensure the localhost session has been opened. - Then clear the browser tab - """ - # Create a canvas to initiate the connection - temp = GraphicsCanvas3D() - - # Delete the canvas to leave a blank screen - temp.scene.append_to_caption(''' - - ''') - - def _record_scene(self, scene_num, fps): - """ - Thread-called function to continuously record screenshots - """ - frame_num = 0 - if fps <= 0: - raise ValueError("fps must be greater than 0.") - f = 1 / fps - - self._thread_lock.acquire() - recording = self._recording - self._thread_lock.release() - - while recording: - # Get current time - t_start = perf_counter() - - # Take screenshot - filename = "vpython_{:04d}.png".format(frame_num) - self.canvases[scene_num].take_screenshot(filename) - frame_num += 1 - - # Get current time - t_stop = perf_counter() - - # Wait for time of frame to finish - # If saving takes longer than frame frequency, this while is skipped - while t_stop - t_start < f: - t_stop = perf_counter() - - self._thread_lock.acquire() - recording = self._recording - self._thread_lock.release() diff --git a/roboticstoolbox/backends/VPython/__init__.py b/roboticstoolbox/backends/VPython/__init__.py deleted file mode 100644 index 66cf62bfd..000000000 --- a/roboticstoolbox/backends/VPython/__init__.py +++ /dev/null @@ -1,5 +0,0 @@ -from roboticstoolbox.backends.VPython.VPython import VPython - -__all__ = [ - 'VPython' -] diff --git a/roboticstoolbox/backends/VPython/canvas.py b/roboticstoolbox/backends/VPython/canvas.py deleted file mode 100644 index aa880a6f4..000000000 --- a/roboticstoolbox/backends/VPython/canvas.py +++ /dev/null @@ -1,1553 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -from vpython import canvas, color, arrow, compound, keysdown, rate, norm, \ - sqrt, cos, button, menu, checkbox, slider, wtext, degrees, vector, \ - radians -from roboticstoolbox.backends.VPython.common_functions import \ - get_pose_x_vec, get_pose_y_vec, get_pose_pos, \ - x_axis_vector, y_axis_vector, z_axis_vector -from roboticstoolbox.backends.VPython.grid import GraphicsGrid, create_line, \ - create_segmented_line, create_marker -from roboticstoolbox.backends.VPython.text import GridType -from enum import Enum -from collections.abc import MutableMapping - - -class UImode(Enum): # pragma nocover - CANVASCONTROL = 1 - TEACHPANEL = 2 - - -class UIMMap(MutableMapping): # pragma nocover - def __init__(self, *args, **kwargs): - self.__dict__.update(*args, **kwargs) - - def __setitem__(self, key, value): - self.__dict__[key] = value - - def __getitem__(self, key): - return self.__dict__[key] - - def __delitem__(self, key): - del self.__dict__[key] - - def __iter__(self): - return iter(self.__dict__) - - def __len__(self): - return len(self.__dict__) - - -class GraphicsCanvas3D: # pragma nocover - """ - Set up the scene with initial conditions. - - White background - - Width, height - - Title, caption - - Axes drawn (if applicable) - - :param height: Height of the canvas on screen (Pixels), defaults to 360. - :type height: `int`, optional - :param width: Width of the canvas on screen (Pixels), defaults to 640. - :type width: `int`, optional - :param title: Title of the plot. Gets displayed above canvas, defaults - to ''. - :type title: `str`, optional - :param caption: Caption (subtitle) of the plot. Gets displayed below the - canvas, defaults to ''. - :type caption: `str`, optional - :param grid: Whether a grid should be displayed in the plot, defaults - to `True`. - :type grid: `bool`, optional - :param g_col: The RGB grid colour - :type g_col: `list`, optional - """ - - def __init__(self, height=500, width=888, title='', caption='', - grid=True, g_type=GridType.XY3D, g_col=None): - - # Create a new independent scene - self.scene = canvas() - - # Apply the settings - self.scene.background = color.white - self.scene.width = width - self.scene.height = height - self.scene.autoscale = False - - # Disable default controls - # Remove shift+mouse panning (not very good controls) - self.scene.userpan = False - # Keep zoom controls (scrollwheel) - self.scene.userzoom = True - # Keep ctrl+mouse enabled to rotate (keyboard rotation more tedious) - self.scene.userspin = True - - # Apply HTML title/caption - if title != '': - self.scene.title = title - - self.scene.append_to_title( - '' \ - '' \ - '' - # noqa - ) - # Disable the arrow keys from scrolling in the browser - # https://stackoverflow.com/questions/8916620/disable-arrow-key-scrolling-in-users-browser - - # Prevent the space bar from toggling the active checkbox/button/etc - # (default browser behaviour) - # https://stackoverflow.com/questions/22280139/prevent-space-button-from-triggering-any-other-button-click-in-jquery - - self.__default_caption = caption - if caption != '': - self.scene.caption = caption - - # List of robots currently in the scene - self.__robots = [] - self.__selected_robot = 0 - # List of joint sliders per robot - self.__teachpanel = [] # 3D, robot -> joint -> options - self.__teachpanel_sliders = [] - self.__idx_qlim_min, self.__idx_qlim_max, self.__idx_theta, \ - self.__idx_text = 0, 1, 2, 3 - # Checkbox states - self.__grid_visibility = grid - self.__camera_lock = False - self.__grid_relative = True - # Screen Shot tally - self._ss_tally = 0 - - # Create the UI - self.__ui_mode = UImode.CANVASCONTROL - self.__toggle_button_text_dict = { - UImode.CANVASCONTROL: "Canvas Controls", - UImode.TEACHPANEL: "Robot Controls" - } - self.__ui_controls = UIMMap() - self.__add_mode_button() - self.__setup_ui_controls([]) - - # Rotate the camera - convert_grid_to_z_up(self.scene) - self.scene.waitfor("draw_complete") # Ensure camera updates before grid is created - - # Any time a key or mouse is held down, run the callback function - rate(30) # 30Hz - self.scene.bind('keydown', self.__handle_keyboard_inputs) - - # Create the grid, and display if wanted - self.__graphics_grid = GraphicsGrid(self.scene, colour=g_col) - self.__graphics_grid.set_mode(g_type) - if not self.__grid_visibility: - self.__graphics_grid.set_visibility(False) - - ####################################### - # Canvas Management - ####################################### - def clear_scene(self): - """ - This function will clear the screen of all objects - """ - # Set all robots variables as invisible - for robot in self.__robots: - robot.set_reference_visibility(False) - robot.set_robot_visibility(False) - - self.scene.waitfor("draw_complete") - - new_list = [] - for name in self.__ui_controls.get('menu_robots').choices: - new_list.append(name) - - self.__selected_robot = 0 - self.__reload_caption(new_list) - - def grid_visibility(self, is_visible): - """ - Update the grid visibility in the scene - - :param is_visible: Whether the grid should be visible or not - :type is_visible: `bool` - """ - self.__graphics_grid.set_visibility(is_visible) - - def add_robot(self, robot): - """ - This function is called when a new robot is created. It adds it to - the drop down menu. - - :param robot: A graphical robot to add to the scene - :type robot: class:`graphics.graphics_robot.GraphicalRobot` - """ - # ALTHOUGH THE DOCUMENTATION SAYS THAT MENU CHOICES CAN BE UPDATED, - # THE PACKAGE DOES NOT ALLOW IT. - # THUS THIS 'HACK' MUST BE DONE TO REFRESH THE UI WITH AN UPDATED LIST - - # Save the list of robot names - new_list = [] - for name in self.__ui_controls.get('menu_robots').choices: - new_list.append(name) - # Add the new one - new_list.append(robot.name) - - # Add robot to list - self.__robots.append(robot) - self.__selected_robot = len(self.__robots) - 1 - - num_options = 4 - # Add spot for current robot settings - self.__teachpanel.append([[0] * num_options] * robot.num_joints) - - # Add robot joint sliders - i = 0 - for joint in robot.joints: - if joint.qlim[0] == joint.qlim[1]: - self.__teachpanel[self.__selected_robot][i] = [ - joint.qlim[0], joint.qlim[1], - joint.theta, None] - else: - string = "{:.2f} rad ({:.2f} deg)".format( - joint.theta, degrees(joint.theta)) - self.__teachpanel[self.__selected_robot][i] = [ - joint.qlim[0], joint.qlim[1], - joint.theta, wtext(text=string)] - i += 1 - - # Refresh the caption - self.__reload_caption(new_list) - - # Set it as selected - if self.__ui_mode == UImode.CANVASCONTROL: - self.__ui_controls.get('menu_robots').index = \ - len(self.__robots) - 1 - - # Place camera based on robots effective radius * 1.25 - if robot.robot is not None: - radius = sum([abs(link.a) + abs(link.d) for link in robot.robot.links]) * 1.25 - self.scene.camera.pos = vector(radius, radius, radius) + get_pose_pos(robot.joints[0].get_pose()) - self.scene.camera.axis = vector(-radius, -radius, -radius) - - def delete_robot(self, robot): - """ - This function is called when a new robot is to be deleted - from the scene. - - :param robot: A graphical robot to add to the scene - :type robot: class:`graphics.graphics_robot.GraphicalRobot` - """ - if len(self.__robots) == 0 or robot not in self.__robots: - return - - robot_index = self.__robots.index(robot) - - # Clear the robot visuals - self.__robots[robot_index].set_reference_visibility(False) - self.__robots[robot_index].set_robot_visibility(False) - - # Remove from UI - new_list = [] - for name in self.__ui_controls.get('menu_robots').choices: - new_list.append(name) - - del new_list[robot_index] - del self.__robots[robot_index] - del self.__teachpanel[robot_index] - - self.__selected_robot = 0 - # Update UI - self.__reload_caption(new_list) - # Select the top item - if len(self.__ui_controls.get('menu_robots').choices) > 0: - self.__ui_controls.get('menu_robots').index = 0 - - def is_robot_in_canvas(self, robot): - """ - Checks whether the given robot is in the canvas - - :param robot: A graphical robot to add to the scene - :type robot: class:`graphics.graphics_robot.GraphicalRobot` - """ - return robot in self.__robots - - def take_screenshot(self, filename): - """ - Take a screenshot and save it - """ - self.scene.capture(filename) - - def set_grid_mode(self, mode): - """ - - """ - self.__graphics_grid.set_mode(mode) - - def current_mode(self): - return self.__ui_mode - - ####################################### - # UI Management - ####################################### - def __add_mode_button(self): - """ - Adds a button to the UI that toggles the UI mode - """ - btn_text = self.__toggle_button_text_dict.get( - self.__ui_mode, "Unknown Mode Set") - btn_text = "" + btn_text + "" - - btn_toggle = button(bind=self.__toggle_mode, text=btn_text) - self.__ui_controls.btn_toggle = btn_toggle - self.scene.append_to_caption('\n') - - def __del_robot(self): - """ - Remove a robot from the scene and the UI controls - """ - if len(self.__robots) == 0: - # Alert the user and return - self.scene.append_to_caption( - '') - return - - # Clear the robot visuals - self.__robots[self.__selected_robot].set_reference_visibility(False) - self.__robots[self.__selected_robot].set_robot_visibility(False) - - # Remove from UI - new_list = [] - for name in self.__ui_controls.get('menu_robots').choices: - new_list.append(name) - - del new_list[self.__selected_robot] - del self.__robots[self.__selected_robot] - del self.__teachpanel[self.__selected_robot] - - self.__selected_robot = 0 - # Update UI - self.__reload_caption(new_list) - # Select the top item - if len(self.__ui_controls.get('menu_robots').choices) > 0: - self.__ui_controls.get('menu_robots').index = 0 - - def __handle_keyboard_inputs(self): - """ - Pans amount dependent on distance between camera and focus point. - Closer = smaller pan amount - - A = move left (pan) - D = move right (pan) - W = move forward (pan) - S = move backward (pan) - - <- = rotate left along camera axes (rotate) - -> = rotate right along camera axes (rotate) - ^ = rotate up along camera axes (rotate) - V = rotate down along camera axes (rotate) - - Q = roll left (rotate) - E = roll right (rotate) - - space = move up (pan) - shift = move down (pan) - - ctrl + LMB = rotate (Default Vpython) - """ - # If camera lock, just skip the function - if self.__camera_lock: - return - - # Constants - pan_amount = 0.02 # units - rot_amount = 1.0 # deg - - # Current settings - cam_distance = self.scene.camera.axis.mag - cam_pos = vector(self.scene.camera.pos) - cam_focus = vector(self.scene.center) - - # Weird manipulation to get correct vector directions. - # (scene.camera.up always defaults to world up) - cam_axis = (vector(self.scene.camera.axis)) # X - cam_side_axis = self.scene.camera.up.cross(cam_axis) # Y - cam_up = cam_axis.cross(cam_side_axis) # Z - - cam_up.mag = cam_axis.mag - - # Get a list of keys - keys = keysdown() - - # Userspin uses ctrl, so skip this check to avoid changing camera pose - # while ctrl is held - if 'ctrl' in keys: - return - - ###################################################################### - # PANNING - # Check if the keys are pressed, update vectors as required - # Changing camera position updates the scene center to - # follow same changes - if 'w' in keys: - cam_pos = cam_pos + cam_axis * pan_amount - if 's' in keys: - cam_pos = cam_pos - cam_axis * pan_amount - if 'a' in keys: - cam_pos = cam_pos + cam_side_axis * pan_amount - if 'd' in keys: - cam_pos = cam_pos - cam_side_axis * pan_amount - if ' ' in keys: - cam_pos = cam_pos + cam_up * pan_amount - if 'shift' in keys: - cam_pos = cam_pos - cam_up * pan_amount - - # Update camera position before rotation - # (to keep pan and rotate separate) - self.scene.camera.pos = cam_pos - - ###################################################################### - # Camera Roll - # If only one rotation key is pressed - if 'q' in keys and 'e' not in keys: - # Rotate camera up - cam_up = cam_up.rotate(angle=-radians(rot_amount), axis=cam_axis) - # Set magnitude as it went to inf - cam_up.mag = cam_axis.mag - # Set - self.scene.up = cam_up - - # If only one rotation key is pressed - if 'e' in keys and 'q' not in keys: - # Rotate camera up - cam_up = cam_up.rotate(angle=radians(rot_amount), axis=cam_axis) - # Set magnitude as it went to inf - cam_up.mag = cam_axis.mag - # Set - self.scene.up = cam_up - - ###################################################################### - # CAMERA ROTATION - d = cam_distance - move_dist = sqrt(d ** 2 + d ** 2 - 2 * d * d * cos( - radians(rot_amount))) # SAS Cosine - - # If only left not right key - if 'left' in keys and 'right' not in keys: - # Calculate distance to translate - cam_pos = cam_pos + norm(cam_side_axis) * move_dist - # Calculate new camera axis - cam_axis = -(cam_pos - cam_focus) - if 'right' in keys and 'left' not in keys: - cam_pos = cam_pos - norm(cam_side_axis) * move_dist - cam_axis = -(cam_pos - cam_focus) - if 'up' in keys and 'down' not in keys: - cam_pos = cam_pos + norm(cam_up) * move_dist - cam_axis = -(cam_pos - cam_focus) - if 'down' in keys and 'up' not in keys: - cam_pos = cam_pos - norm(cam_up) * move_dist - cam_axis = -(cam_pos - cam_focus) - - # Update camera position and axis - self.scene.camera.pos = cam_pos - self.scene.camera.axis = cam_axis - - def __reload_caption(self, new_list): - """ - Reload the UI with the new list of robot names - - :param new_list: The new list to apply to the menu - :type new_list: `list` - """ - # Remove all UI elements - for item in self.__ui_controls: - if self.__ui_controls.get(item) is None: - continue - self.__ui_controls.get(item).delete() - for item in self.__teachpanel_sliders: - item.delete() - self.__teachpanel_sliders = [] - # Restore the caption - self.scene.caption = self.__default_caption - # Create the updated caption. - self.__load_mode_ui(new_list) - - def __load_mode_ui(self, new_list): - """ - Load the UI menu depending on the current mode - - :param new_list: The new list to apply to the menu - :type new_list: `list` - """ - self.__add_mode_button() - if self.__ui_mode == UImode.CANVASCONTROL: - self.__setup_ui_controls(new_list) - elif self.__ui_mode == UImode.TEACHPANEL: - self.__setup_joint_sliders() - else: - self.scene.append_to_caption("UNKNOWN MODE ENTERED\n") - - def __setup_ui_controls(self, list_of_names): - """ - The initial configuration of the user interface - - :param list_of_names: A list of names of the robots in the screen - :type list_of_names: `list` - """ - - ################################################## - self.scene.append_to_caption('

Scene Settings

\n') - # Button to reset camera - reset_button = button( - bind=self.__reset_camera, text="Reset Camera") - self.__ui_controls.btn_reset = reset_button - self.scene.append_to_caption('\t') - - screenshot_button = button(bind=self.__screenshot, text="Take Screenshot") - self.__ui_controls.btn_ss = screenshot_button - self.scene.append_to_caption('\n') - - camera_lock_checkbox = checkbox(bind=self.__camera_lock_checkbox, text="Camera Lock", - checked=self.__camera_lock) - self.__ui_controls.chkbox_cam = camera_lock_checkbox - self.scene.append_to_caption('\t') - - grid_relative_checkbox = checkbox( - bind=self.__grid_relative_checkbox, - text="Grid Relative", checked=self.__grid_relative) - self.__ui_controls.chkbox_rel = grid_relative_checkbox - - self.scene.append_to_caption('\t') - # Checkbox for grid visibility - checkbox_grid_visibility = checkbox( - bind=self.__grid_visibility_checkbox, text="Grid Visibility", - checked=self.__grid_visibility) - self.__ui_controls.chkbox_grid = checkbox_grid_visibility - self.scene.append_to_caption('\n') - - ################################################## - self.scene.append_to_caption('

Robot

\n') - # Drop down for robots / joints in frame - menu_robots_list = menu(bind=self.__menu_item_chosen, choices=list_of_names) - if not len(list_of_names) == 0: - menu_robots_list.index = self.__selected_robot - self.__ui_controls.menu_robots = menu_robots_list - self.scene.append_to_caption('\t') - - # Button to delete the selected robot - delete_button = button(bind=self.__del_robot, text="Delete Robot") - self.__ui_controls.btn_del = delete_button - self.scene.append_to_caption('\t') - - # Button to clear the robots in screen - clear_button = button(bind=self.clear_scene, text="Clear Scene") - self.__ui_controls.btn_clr = clear_button - self.scene.append_to_caption('\n') - - ################################################## - self.scene.append_to_caption('

Characteristics

\n') - # Checkbox for reference frame visibilities - if len(self.__robots) == 0: - reference_checkbox = checkbox( - bind=self.__reference_frame_checkbox, - text="Show Reference Frames", checked=True) - else: - chk = self.__robots[self.__selected_robot].ref_shown - reference_checkbox = checkbox( - bind=self.__reference_frame_checkbox, - text="Show Reference Frames", checked=chk) - self.__ui_controls.chkbox_ref = reference_checkbox - self.scene.append_to_caption('\t') - - # Checkbox for robot visibility - if len(self.__robots) == 0: - robot_vis_checkbox = checkbox( - bind=self.__robot_visibility_checkbox, - text="Show Robot", checked=True) - else: - chk = self.__robots[self.__selected_robot].rob_shown - robot_vis_checkbox = checkbox( - bind=self.__robot_visibility_checkbox, - text="Show Robot", checked=chk) - self.__ui_controls.chkbox_rob = robot_vis_checkbox - self.scene.append_to_caption('\n') - - # Slider for robot opacity - self.scene.append_to_caption('Robot Opacity:') - if len(self.__robots) == 0: - opacity_slider = slider(bind=self.__opacity_slider, value=1) - else: - opc = self.__robots[self.__selected_robot].opacity - opacity_slider = slider(bind=self.__opacity_slider, value=opc) - self.__ui_controls.sld_opc = opacity_slider - self.scene.append_to_caption('\n') - - ################################################## - # Control manual - controls_str = '

Controls


' \ - 'PAN
' \ - 'W , S | forward / backward
' \ - 'A , D | left / right
' \ - 'SPACE , SHIFT | up / down
' \ - 'ROTATE
' \ - 'CTRL + LMB | free spin
' \ - 'ARROWS KEYS | rotate direction
' \ - 'Q , E | roll left / right
' \ - 'ZOOM
' \ - 'MOUSEWHEEL | zoom in / out' - - self.scene.append_to_caption(controls_str) - - def __setup_joint_sliders(self): - """ - Display the Teachpanel mode of the UI - """ - self.scene.append_to_caption('\n') - if len(self.__teachpanel) == 0: - self.scene.append_to_caption("No robots available\n") - return - - # Update the robots to their current joint angles - for joint_idx, joint in enumerate(self.__teachpanel[self.__selected_robot]): - joint[self.__idx_theta] = self.__robots[self.__selected_robot].angles[joint_idx] - - i = 0 - for joint in self.__teachpanel[self.__selected_robot]: - if joint[self.__idx_qlim_min] == joint[self.__idx_qlim_max]: - # If a slider with (effectively) no values, skip it - i += 1 - continue - # Add a title - self.scene.append_to_caption('Joint {0}:\t'.format(i)) - # Add the slider, with the correct joint variables - s = slider( - bind=self.__joint_slider, - min=joint[self.__idx_qlim_min], - max=joint[self.__idx_qlim_max], - value=joint[self.__idx_theta], - id=i - ) - self.__teachpanel_sliders.append(s) - string = "{:.2f} rad ({:.2f} deg)".format( - joint[self.__idx_theta], degrees(joint[self.__idx_theta])) - joint[self.__idx_text] = wtext(text=string) - self.scene.append_to_caption('\n\n') - i += 1 - - def teach_mode(self, teach=False): - if teach: - self.__ui_mode = UImode.CANVASCONTROL - else: - self.__ui_mode = UImode.TEACHPANEL - self.__toggle_mode() - - ####################################### - # UI CALLBACKS - ####################################### - def __toggle_mode(self): - """ - Callback for when the toggle mode button is pressed - """ - # Update mode - # Update mode, default canvas controls - self.__ui_mode = { - UImode.CANVASCONTROL: UImode.TEACHPANEL, - UImode.TEACHPANEL: UImode.CANVASCONTROL - }.get(self.__ui_mode, UImode.CANVASCONTROL) - - # Update UI - # get list of robots - new_list = [] - for name in self.__ui_controls.get('menu_robots').choices: - new_list.append(name) - - self.__reload_caption(new_list) - - def __reset_camera(self): - """ - Reset the camera to a default position and orientation - """ - # Reset Camera - self.scene.up = z_axis_vector - self.scene.camera.pos = vector(10, 10, 10) - self.scene.camera.axis = -self.scene.camera.pos - - # Update grid - self.__graphics_grid.update_grid() - - def __menu_item_chosen(self, m): - """ - When a menu item is chosen, update the relevant checkboxes/options - - :param m: The menu object that has been used to select an item. - :type: class:`menu` - """ - # Get selected item - self.__selected_robot = m.index - - # Update the checkboxes/sliders for the selected robot - self.__ui_controls.get('chkbox_ref').checked = \ - self.__robots[self.__selected_robot].ref_shown - - self.__ui_controls.get('chkbox_rob').checked = \ - self.__robots[self.__selected_robot].rob_shown - - self.__ui_controls.get('sld_opc').value = \ - self.__robots[self.__selected_robot].opacity - - def __reference_frame_checkbox(self, c): - """ - When a checkbox is changed for the reference frame option, update the - graphics - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - if len(self.__robots) > 0: - self.__robots[self.__selected_robot].set_reference_visibility( - c.checked) - - def __robot_visibility_checkbox(self, c): - """ - When a checkbox is changed for the robot visibility, update the - graphics - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - if len(self.__robots) > 0: - self.__robots[self.__selected_robot].set_robot_visibility( - c.checked) - - def __grid_visibility_checkbox(self, c): - """ - When a checkbox is changed for the grid visibility, update the graphics - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - self.grid_visibility(c.checked) - self.__grid_visibility = c.checked - - def __camera_lock_checkbox(self, c): - """ - When a checkbox is changed for the camera lock, update the camera - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - # Update parameters - # True = locked - self.__camera_lock = c.checked - # True = enabled - self.scene.userspin = not c.checked - self.scene.userzoom = not c.checked - - def __grid_relative_checkbox(self, c): - """ - When a checkbox is changed for the grid lock, update the grid - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - self.__graphics_grid.set_relative(c.checked) - self.__grid_relative = c.checked - - def __opacity_slider(self, s): - """ - Update the opacity slider depending on the slider value - - :param s: The slider object that has been modified - :type s: class:`slider` - """ - if len(self.__robots) > 0: - self.__robots[self.__selected_robot].set_transparency(s.value) - - def __joint_slider(self, s): - """ - The callback for when a joint slider has changed value - - :param s: The slider object that has been modified - :type s: class:`slider` - """ - # Save the value - self.__teachpanel[self.__selected_robot][s.id][self.__idx_theta] = \ - s.value - - # Get all angles for the robot - angles = [] - for idx in range(len(self.__teachpanel_sliders)): - angles.append(self.__teachpanel_sliders[idx].value) - - # Run fkine - self.__robots[self.__selected_robot].fkine_and_set(angles) - - for joint in self.__teachpanel[self.__selected_robot]: - if joint[self.__idx_text] is None: - continue - string = "{:.2f} rad ({:.2f} deg)".format( - joint[self.__idx_theta], degrees(joint[self.__idx_theta])) - joint[self.__idx_text].text = string - - def __screenshot(self, b): - """ - Take a screencap - """ - filename = "vp_ss_{:04d}.png".format(self._ss_tally) - self.take_screenshot(filename) - self._ss_tally += 1 - - -class GraphicsCanvas2D: # pragma nocover - """ - Set up the scene with initial conditions. - - White background - - Width, height - - Title, caption - - Axes drawn (if applicable) - - :param height: Height of the canvas on screen (Pixels), defaults to 360. - :type height: `int`, optional - :param width: Width of the canvas on screen (Pixels), defaults to 640. - :type width: `int`, optional - :param title: Title of the plot. Gets displayed above canvas, defaults to ''. - :type title: `str`, optional - :param caption: Caption (subtitle) of the plot. Gets displayed below the canvas, defaults to ''. - :type caption: `str`, optional - :param grid: Whether a grid should be displayed in the plot, defaults to `True`. - :type grid: `bool`, optional - :param g_col: The RGB grid colour - :type g_col: `list`, optional - """ - - def __init__(self, height=500, width=888, title='', caption='', grid=True, g_type=GridType.XY2D, g_col=None): - - # Private lists - self.__line_styles = [ - '', # None - '-', # Solid (default) - '--', # Dashes - ':', # Dotted - '-.', # Dash-dot - ] - self.__marker_styles = [ - '+', # Plus - 'o', # Circle - '*', # Star - '.', # Dot - 'x', # Cross - 's', # Square - 'd', # Diamond - '^', # Up triangle - 'v', # Down triangle - '<', # Left triangle - '>', # Right triangle - 'p', # Pentagon - 'h', # Hexagon - ] - self.__colour_styles = [ - 'r', # Red - 'g', # Green - 'b', # Blue - 'y', # Yellow - 'c', # Cyan - 'm', # Magenta - 'k', # Black (default) - 'w', # White - ] - self.__colour_dictionary = { - 'r': color.red.value, - 'g': color.green.value, - 'b': color.blue.value, - 'c': color.cyan.value, - 'y': color.yellow.value, - 'm': color.magenta.value, - 'k': color.black.value, - 'w': color.white.value - } - - # Create a new independent scene - self.scene = canvas() - - # Apply the settings - self.scene.background = color.white - self.scene.width = width - self.scene.height = height - self.scene.autoscale = False - - # Disable default controls - self.scene.userpan = True # Keep shift+mouse panning (key overwritten) - self.scene.userzoom = True # Keep zoom controls (scrollwheel) - self.scene.userspin = False # Remove ctrl+mouse enabled to rotate - - self.__grid_visibility = grid - self.__camera_lock = False - self.__grid_relative = True - - # Apply HTML title/caption - if title != '': - self.scene.title = title - - self.__default_caption = caption - if caption != '': - self.scene.caption = caption - - self.__ui_controls = UIMMap() - self.__reload_caption() - - # Any time a key or mouse is held down, run the callback function - rate(30) # 30Hz - self.scene.bind('keydown', self.__handle_keyboard_inputs) - - # Create the grid, and display if wanted - self.__graphics_grid = GraphicsGrid(self.scene, colour=g_col) - self.__graphics_grid.set_mode(g_type) - # Toggle grid to 2D - # self.__graphics_grid.toggle_2d_3d() - # Lock the grid - # self.__graphics_grid.set_relative(False) - # Turn off grid if applicable - if not self.__grid_visibility: - self.__graphics_grid.set_visibility(False) - - # Reset the camera to known spot - self.__reset_camera() - self.__graphics_grid.update_grid() - self.__graphics_grid.toggle_2d_3d() - - ####################################### - # Canvas Management - ####################################### - def clear_scene(self): - """ - Clear the scene of all objects, keeping the grid visible if set on - """ - # Save grid visibility - restore = self.__grid_visibility - - # Set invis - if restore: - self.__graphics_grid.set_visibility(False) - - # Set all objects invis - for obj in self.scene.objects: - obj.visible = False - - # Restore grid (if needed) - if restore: - self.__graphics_grid.set_visibility(True) - - def grid_visibility(self, is_visible): - """ - Update the grid visibility in the scene - - :param is_visible: Whether the grid should be visible or not - :type is_visible: `bool` - """ - self.__graphics_grid.set_visibility(is_visible) - - ####################################### - # UI Management - ####################################### - def __handle_keyboard_inputs(self): - """ - Pans amount dependent on distance between camera and focus point. - Closer = smaller pan amount - - A = move left (pan) - D = move right (pan) - W = move up (pan) - S = move down (pan) - - <- = rotate left along camera axes (rotate) - -> = rotate right along camera axes (rotate) - ^ = rotate up along camera axes (rotate) - V = rotate down along camera axes (rotate) - - Q = roll left (rotate) - E = roll right (rotate) - - shift + LMB = pan (Default Vpython) - """ - # If camera lock, just skip the function - if self.__camera_lock: - return - - # Constants - pan_amount = 0.02 # units - rot_amount = 1.0 # deg - - # Current settings - cam_distance = self.scene.camera.axis.mag - cam_pos = vector(self.scene.camera.pos) - cam_focus = vector(self.scene.center) - - # Weird manipulation to get correct vector directions. - # (scene.camera.up always defaults to world up) - cam_axis = (vector(self.scene.camera.axis)) # X - cam_side_axis = self.scene.camera.up.cross(cam_axis) # Y - cam_up = cam_axis.cross(cam_side_axis) # Z - - cam_up.mag = cam_axis.mag - - # Get a list of keys - keys = keysdown() - - # Userpan uses ctrl, so skip this check to avoid changing camera pose - # while shift is held - if 'shift' in keys: - return - - ##################################################################### - # PANNING - # Check if the keys are pressed, update vectors as required - # Changing camera position updates the scene center to follow - # same changes - if 'w' in keys: - cam_pos = cam_pos + cam_up * pan_amount - if 's' in keys: - cam_pos = cam_pos - cam_up * pan_amount - if 'a' in keys: - cam_pos = cam_pos + cam_side_axis * pan_amount - if 'd' in keys: - cam_pos = cam_pos - cam_side_axis * pan_amount - - # Update camera position before rotation - # (to keep pan and rotate separate) - self.scene.camera.pos = cam_pos - - ##################################################################### - # Camera Roll - # If only one rotation key is pressed - if 'q' in keys and 'e' not in keys: - # Rotate camera up - cam_up = cam_up.rotate(angle=-radians(rot_amount), axis=cam_axis) - # Set magnitude as it went to inf - cam_up.mag = cam_axis.mag - # Set - self.scene.up = cam_up - - # If only one rotation key is pressed - if 'e' in keys and 'q' not in keys: - # Rotate camera up - cam_up = cam_up.rotate(angle=radians(rot_amount), axis=cam_axis) - # Set magnitude as it went to inf - cam_up.mag = cam_axis.mag - # Set - self.scene.up = cam_up - - ###################################################################### - # CAMERA ROTATION - d = cam_distance - move_dist = sqrt(d ** 2 + d ** 2 - 2 * d * d * cos( - radians(rot_amount))) # SAS Cosine - - # If only left not right key - if 'left' in keys and 'right' not in keys: - # Calculate distance to translate - cam_pos = cam_pos + norm(cam_side_axis) * move_dist - # Calculate new camera axis - cam_axis = -(cam_pos - cam_focus) - if 'right' in keys and 'left' not in keys: - cam_pos = cam_pos - norm(cam_side_axis) * move_dist - cam_axis = -(cam_pos - cam_focus) - if 'up' in keys and 'down' not in keys: - cam_pos = cam_pos + norm(cam_up) * move_dist - cam_axis = -(cam_pos - cam_focus) - if 'down' in keys and 'up' not in keys: - cam_pos = cam_pos - norm(cam_up) * move_dist - cam_axis = -(cam_pos - cam_focus) - - # Update camera position and axis - self.scene.camera.pos = cam_pos - self.scene.camera.axis = cam_axis - - def __reset_camera(self): - """ - Reset the camera to a known position - """ - # Reset Camera - self.scene.camera.pos = vector(5, 5, 12) # Hover above (5, 5, 0) - # Ever so slightly off focus, to ensure grid is rendered in the right - # region - # (if directly at, draws numbers wrong spots) - # Focus on (5, 5, 0) - self.scene.camera.axis = vector(-0.001, -0.001, -12) - self.scene.up = y_axis_vector - - def __reload_caption(self): - """ - Reload the UI with the new list of robot names - """ - # Remove all UI elements - for item in self.__ui_controls: - if self.__ui_controls.get(item) is None: - continue - self.__ui_controls.get(item).delete() - # Restore the caption - self.scene.caption = self.__default_caption - # Create the updated caption. - self.__setup_ui_controls() - - def __setup_ui_controls(self): - """ - The initial configuration of the user interface - """ - self.scene.append_to_caption('\n') - - # Button to reset camera - btn_reset = button( - bind=self.__reset_camera, text="Reset Camera") - self.__ui_controls.btn_reset = btn_reset - self.scene.append_to_caption('\t') - - chkbox_cam = checkbox( - bind=self.__camera_lock_checkbox, - text="Camera Lock", checked=self.__camera_lock) - self.__ui_controls.chkbox_cam = chkbox_cam - self.scene.append_to_caption('\t') - - chkbox_rel = checkbox( - bind=self.__grid_relative_checkbox, - text="Grid Relative", checked=self.__grid_relative) - self.__ui_controls.chkbox_rel = chkbox_rel - self.scene.append_to_caption('\n\n') - - # Button to clear the screen - btn_clr = button(bind=self.clear_scene, text="Clear Scene") - self.__ui_controls.btn_clear = btn_clr - self.scene.append_to_caption('\n\n') - - # Checkbox for grid visibility - chkbox_grid = checkbox( - bind=self.__grid_visibility_checkbox, text="Grid Visibility", - checked=self.__grid_visibility) - self.__ui_controls.chkbox_grid = chkbox_grid - self.scene.append_to_caption('\t') - - # Prevent the space bar from toggling the active checkbox/button/etc - # (default browser behaviour) - self.scene.append_to_caption(''' - ''') - # https://stackoverflow.com/questions/22280139/prevent-space-button-from-triggering-any-other-button-click-in-jquery - - # Control manual - controls_str = '
Controls
' \ - 'PAN
' \ - 'SHFT + LMB | free pan
' \ - 'W , S | up / down
' \ - 'A , D | left / right
' \ - 'ROTATE
' \ - 'ARROWS KEYS | rotate direction
' \ - 'Q , E | roll left / right
' \ - 'ZOOM
' \ - 'MOUSEWHEEL | zoom in / out
' \ - '' # noqa - # Disable the arrow keys from scrolling in the browser - # https://stackoverflow.com/questions/8916620/disable-arrow-key-scrolling-in-users-browser - self.scene.append_to_caption(controls_str) - - ####################################### - # UI CALLBACKS - ####################################### - def __camera_lock_checkbox(self, c): - """ - When a checkbox is changed for the camera lock, update the camera - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - # Update parameters - # True = locked - self.__camera_lock = c.checked - # True = enabled - self.scene.userspin = not c.checked - self.scene.userzoom = not c.checked - - def __grid_relative_checkbox(self, c): - """ - When a checkbox is changed for the grid lock, update the grid - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - self.__graphics_grid.set_relative(c.checked) - self.__grid_relative = c.checked - - def __grid_visibility_checkbox(self, c): - """ - When a checkbox is changed for the grid visibility, update the graphics - - :param c: The checkbox that has been toggled - :type c: class:`checkbox` - """ - self.grid_visibility(c.checked) - self.__grid_visibility = c.checked - - ####################################### - # Drawing Functions - ####################################### - def __draw_path( - self, x_path, y_path, opt_line, opt_marker, - opt_colour, thickness=0.05): - """ - Draw a line from point to point in the 2D path - - :param x_path: The x path to draw on the canvas - :type x_path: `list` - :param y_path: The y path to draw on the canvas - :type y_path: `list` - :param opt_line: The line option argument - :type opt_line: `str` - :param opt_marker: The marker option argument - :type opt_marker: `str` - :param opt_colour: The colour option argument - :type opt_colour: `str` - :param thickness: Thickness of the line - :type thickness: `float` - :raises ValueError: Invalid line type given - """ - # Get colour - colour = self.__get_colour_from_string(opt_colour) - - # For every point in the list, draw a line to the next one - # (excluding last point) - for point in range(0, len(x_path)): - # Get point 1 - x1 = x_path[point] - y1 = y_path[point] - p1 = vector(x1, y1, 0) - - # If at end / only coordinate - draw a marker - if point == len(x_path) - 1: - create_marker(self.scene, x1, y1, opt_marker, colour) - return - - # Get point 2 - x2 = x_path[point + 1] - y2 = y_path[point + 1] - p2 = vector(x2, y2, 0) - - if opt_line == '': - # Only one marker to avoid double-ups - create_marker(self.scene, x1, y1, opt_marker, colour) - elif opt_line == '-': - create_line( - p1, p2, self.scene, colour=colour, thickness=thickness) - # Only one marker to avoid double-ups - create_marker(self.scene, x1, y1, opt_marker, colour) - elif opt_line == '--': - create_segmented_line( - p1, p2, self.scene, 0.3, colour=colour, - thickness=thickness) - # Only one marker to avoid double-ups - create_marker(self.scene, x1, y1, opt_marker, colour) - elif opt_line == ':': - create_segmented_line( - p1, p2, self.scene, 0.05, colour=colour, - thickness=thickness) - # Only one marker to avoid double-ups - create_marker(self.scene, x1, y1, opt_marker, colour) - elif opt_line == '-.': - raise NotImplementedError("Other line types not implemented") - else: - raise ValueError("Invalid line type given") - - def plot(self, x_coords, y_coords=None, options=''): - """ - Same usage as MATLAB's plot. - - If given one list of coordinates, plots against index - If given two lists of coordinates, plots both (1st = x, 2nd = y) - - Options string is identical to MATLAB's input string - If you do not specify a marker type, plot uses no marker. - If you do not specify a line style, plot uses a solid line. - - b blue . point - solid - g green o circle : dotted - r red x x-mark -. dashdot - c cyan + plus -- dashed - m magenta * star (none) no line - y yellow s square - k black d diamond - w white v triangle (down) - ^ triangle (up) - < triangle (left) - > triangle (right) - p pentagram - h hexagram - - :param x_coords: The first plane of coordinates to plot - :type x_coords: `list` - :param y_coords: The second plane of coordinates to plot with. - :type y_coords: `list`, `str`, optional - :param options: A string of options to plot with - :type options: `str`, optional - :raises ValueError: Number of X and Y coordinates must be equal - """ - # TODO - # add options for line width, marker size - - # If y-vector is str, then only x vector given - if isinstance(y_coords, str): - options = y_coords - y_coords = None - - one_set_data = False - # Set y-vector to default if None - if y_coords is None: - one_set_data = True - y_coords = [*range(0, len(x_coords))] - - # Verify x, y coords have same length - if len(x_coords) != len(y_coords): - raise ValueError( - "Number of X coordinates does not equal " - "number of Y coordinates.") - - # Verify options given (and save settings to be applied) - verified_options = self.__verify_plot_options(options) - - if one_set_data: - # Draw plot for one list of data - self.__draw_path( - y_coords, # Y is default x-coords in one data set - x_coords, # User input - verified_options[0], # Line - verified_options[1], # Marker - verified_options[2], # Colour - ) - else: - # Draw plot for two lists of data - self.__draw_path( - x_coords, # User input - y_coords, # User input - verified_options[0], # Line - verified_options[1], # Marker - verified_options[2], # Colour - ) - - def __verify_plot_options(self, options_str): - """ - Verify that the given options are usable. - - :param options_str: The given options from the plot command to verify - user input - - :type options_str: `str` - :raises ValueError: Unknown character entered - :raises ValueError: Too many line segments used - :raises ValueError: Too many marker segments used - :raises ValueError: Too many colour segments used - :returns: List of options to plot with - :rtype: `list` - """ - default_line = '-' - default_marker = '' - default_colour = 'k' - - # Split str into chars list - options_split = list(options_str) - - # If 0, set defaults and return early - if len(options_split) == 0: - return [default_line, default_marker, default_colour] - - # If line_style given, join the first two options if applicable - # (some types have 2 characters) - for char in range(0, len(options_split) - 1): - # If char is '-' (only leading character in double length option) - if options_split[char] == '-' and len(options_split) > 1: - # If one of the leading characters is valid - if options_split[char + 1] == '-' or \ - options_split[char + 1] == '.': - # Join the two into the first - options_split[char] = options_split[char] \ - + options_split[char + 1] - # Shuffle down the rest - for idx in range(char + 2, len(options_split)): - options_split[idx - 1] = options_split[idx] - # Remove duplicate extra - options_split.pop() - - # If any unknown, throw error - for option in options_split: - if option not in self.__line_styles and \ - option not in self.__marker_styles and \ - option not in self.__colour_styles: - error_string = "Unknown character entered: '{0}'" - raise ValueError(error_string.format(option)) - - ############################## - # Verify Line Style - ############################## - line_style_count = 0 # Count of options used - # Index position of index used (only used when count == 1) - line_style_index = 0 - for option in options_split: - if option in self.__line_styles: - line_style_count = line_style_count + 1 - line_style_index = self.__line_styles.index(option) - - # If more than one, throw error - if line_style_count > 1: - raise ValueError( - "Too many line style arguments given. Only one allowed") - # If none, set as solid - elif line_style_count == 0 or not any( - item in options_split for item in self.__line_styles): - output_line = default_line - # If one, set as given - else: - output_line = self.__line_styles[line_style_index] - ############################## - - ############################## - # Verify Marker Style - ############################## - marker_style_count = 0 # Count of options used - # Index position of index used (only used when count == 1) - marker_style_index = 0 - for option in options_split: - if option in self.__marker_styles: - marker_style_count = marker_style_count + 1 - marker_style_index = self.__marker_styles.index(option) - - # If more than one, throw error - if marker_style_count > 1: - raise ValueError( - "Too many marker style arguments given. Only one allowed") - # If none, set as no-marker - elif marker_style_count == 0 or not any( - item in options_split for item in self.__marker_styles): - output_marker = default_marker - # If one, set as given - else: - output_marker = self.__marker_styles[marker_style_index] - # If marker set and no line given, turn line to no-line - if line_style_count == 0 or not any( - item in options_split for item in self.__line_styles): - output_line = '' - ############################## - - ############################## - # Verify Colour Style - ############################## - colour_style_count = 0 # Count of options used - # Index position of index used (only used when count == 1) - colour_style_index = 0 - for option in options_split: - if option in self.__colour_styles: - colour_style_count = colour_style_count + 1 - colour_style_index = self.__colour_styles.index(option) - - # If more than one, throw error - if colour_style_count > 1: - raise ValueError( - "Too many colour style arguments given. Only one allowed") - # If none, set as black - elif colour_style_count == 0 or not any( - item in options_split for item in self.__colour_styles): - output_colour = default_colour - # If one, set as given - else: - output_colour = self.__colour_styles[colour_style_index] - ############################## - - return [output_line, output_marker, output_colour] - - def __get_colour_from_string(self, colour_string): - """ - Using the colour plot string input, return an rgb array of the colour - selected - - :param colour_string: The colour string option - :type colour_string: `str` - :returns: List of RGB values for the representative colour - :rtype: `list` - """ - # Return the RGB list (black if not in dictionary) - return self.__colour_dictionary.get(colour_string, color.black.value) - - -def convert_grid_to_z_up(scene): # pragma nocover - """ - Rotate the camera so that +z is up - (Default vpython scene is +y up) - """ - - ''' - There is an interaction between up and forward, the direction that the - camera is pointing. By default, the camera points in the -z direction - vector(0,0,-1). In this case, you can make the x or y axes (or anything - between) be the up vector, but you cannot make the z axis be the up - vector, because this is the axis about which the camera rotates when - you set the up attribute. If you want the z axis to point up, first set - forward to something other than the -z axis, for example vector(1,0,0). - https://www.glowscript.org/docs/VPythonDocs/canvas.html - ''' - # First set the x-axis forward - scene.forward = x_axis_vector - scene.up = z_axis_vector - - # Place the camera in the + axes - scene.camera.pos = vector(10, 10, 10) - scene.camera.axis = -scene.camera.pos - return - - -def draw_reference_frame_axes(se3_pose, scene): # pragma nocover - """ - Draw x, y, z axes from the given point. - Each axis is represented in the objects reference frame. - - - :param se3_pose: SE3 pose representation of the reference frame - :type se3_pose: class:`spatialmath.pose3d.SE3` - :param scene: Which scene to put the graphics in - :type scene: class:`vpython.canvas` - :return: Compound object of the 3 axis arrows. - :rtype: class:`vpython.compound` - """ - - origin = get_pose_pos(se3_pose) - x_axis = get_pose_x_vec(se3_pose) - y_axis = get_pose_y_vec(se3_pose) - - # Create Basic Frame - # Draw X Axis - x_arrow = arrow( - canvas=scene, pos=origin, axis=x_axis_vector, - length=0.25, color=color.red) - - # Draw Y Axis - y_arrow = arrow( - canvas=scene, pos=origin, axis=y_axis_vector, - length=0.25, color=color.green) - - # Draw Z Axis - z_arrow = arrow( - canvas=scene, pos=origin, axis=z_axis_vector, - length=0.25, color=color.blue) - - # Combine all to manipulate together - # Set origin to where axis converge (instead of the middle of - # the resulting object bounding box) - frame_ref = compound( - [x_arrow, y_arrow, z_arrow], origin=origin, canvas=scene) - - # Set frame axes - frame_ref.axis = x_axis - frame_ref.up = y_axis - - return frame_ref diff --git a/roboticstoolbox/backends/VPython/common_functions.py b/roboticstoolbox/backends/VPython/common_functions.py deleted file mode 100644 index 92f8b663a..000000000 --- a/roboticstoolbox/backends/VPython/common_functions.py +++ /dev/null @@ -1,142 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - - -from vpython import radians, vector -from numpy import array -from spatialmath import SE3 - -""" -global variables that can be used to easily reference X, Y, and Z axis -directions. -""" -x_axis_vector = vector(1, 0, 0) -y_axis_vector = vector(0, 1, 0) -z_axis_vector = vector(0, 0, 1) - - -def get_pose_x_vec(se3_obj): # pragma nocover - """ - Convert SE3 details to VPython vector format - - :param se3_obj: SE3 pose and orientation object - :type se3_obj: class:`spatialmath.pose3d.SE3` - :return: VPython vector representation of the X orientation - :rtype: class:`vpython.vector` - """ - data = se3_obj.n - return vector(data[0], data[1], data[2]) - - -def get_pose_y_vec(se3_obj): # pragma nocover - """ - Convert SE3 details to VPython vector format - - :param se3_obj: SE3 pose and orientation object - :type se3_obj: class:`spatialmath.pose3d.SE3` - :return: VPython vector representation of the Y orientation - :rtype: class:`vpython.vector` - """ - data = se3_obj.o - return vector(data[0], data[1], data[2]) - - -def get_pose_z_vec(se3_obj): # pragma nocover - """ - Convert SE3 details to VPython vector format - - :param se3_obj: SE3 pose and orientation object - :type se3_obj: class:`spatialmath.pose3d.SE3` - :return: VPython vector representation of the Z orientation - :rtype: class:`vpython.vector` - """ - data = se3_obj.a - return vector(data[0], data[1], data[2]) - - -def get_pose_pos(se3_obj): # pragma nocover - """ - Convert SE3 details to VPython vector format - - :param se3_obj: SE3 pose and orientation object - :type se3_obj: `spatialmath.pose3d.SE3` - :return: VPython vector representation of the position - :rtype: class:`vpython.vector` - """ - data = se3_obj.t - return vector(data[0], data[1], data[2]) - - -def vpython_to_se3(graphic_object): # pragma nocover - """ - This function will take in a graphics object and output it's pose as an - SE3 object - - :param graphic_object: A VPython graphic object - :type graphic_object: class:`vpython.object` - :return: SE3 representation of the pose - :rtype: class:`spatialmath.pose3d.SE3` - """ - # Get the x, y, z axes and position - x_vec = graphic_object.axis - y_vec = graphic_object.up - z_vec = x_vec.cross(y_vec) - pos = graphic_object.pos - - # Form a numpy array - T = array([ - [x_vec.x, y_vec.x, z_vec.x, pos.x], - [x_vec.y, y_vec.y, z_vec.y, pos.y], - [x_vec.z, y_vec.z, z_vec.z, pos.z], - [0, 0, 0, 1] - ]) - - return SE3(T) - - -def wrap_to_pi(angle_type, angle): # pragma nocover - """ - Wrap the given angle (deg or rad) to [-pi pi] - - :param angle_type: String of whether the angle is deg or rad - :type angle_type: `str` - :param angle: The angle to wrap - :type angle: `float` - :raises ValueError: Throws the error if the given string is not "deg" - or "rad" - :return: The wrapped angle - :rtype: `float` - """ - if angle_type == "deg": - angle = angle % 360 - if angle > 180: - angle -= 360 - - elif angle_type == "rad": - angle = angle % radians(360) - if angle > radians(180): - angle -= radians(360) - - else: - raise ValueError('angle_type must be "deg" or "rad"') - - return angle - - -def close_localhost_session(canvas): # pragma nocover - """ - Terminate the local host session through JavaScript - - :param canvas: The scene to append the JS to the caption - :type canvas: - class:`roboticstoolbox.backends.VPython.graphics_canvas.GraphicsCanvas3D`, - class:`roboticstoolbox.backends.VPython.graphics_canvas.GraphicsCanvas2D` - """ - - canvas.scene.append_to_caption(''' - - ''') diff --git a/roboticstoolbox/backends/VPython/graphicalrobot.py b/roboticstoolbox/backends/VPython/graphicalrobot.py deleted file mode 100644 index 07e0c7347..000000000 --- a/roboticstoolbox/backends/VPython/graphicalrobot.py +++ /dev/null @@ -1,823 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -import numpy as np -from vpython import box, compound, color, sqrt -from roboticstoolbox.backends.VPython.canvas import draw_reference_frame_axes -from roboticstoolbox.backends.VPython.common_functions import \ - array, x_axis_vector, y_axis_vector, z_axis_vector, \ - get_pose_pos, get_pose_x_vec, get_pose_y_vec, get_pose_z_vec, \ - vector -from roboticstoolbox.backends.VPython.stl import set_stl_origin, \ - import_object_from_numpy_stl -from time import perf_counter -from spatialmath import SE3 -from pathlib import PurePath - - -class DefaultJoint: # pragma nocover - """ - This class forms a base for all the different types of joints - - Rotational - - Prismatic - - Static - - Gripper - - :param initial_se3: Pose to set the joint to initially - :type initial_se3: class:`spatialmath.pose3d.SE3` - :param structure: A variable representing the joint length (float) or - a file path to an STL (str) - :type structure: `float`, `str` - :param g_canvas: The canvas in which to add the link - :type g_canvas: class:`graphics.graphics_canvas.graphicscanvas3d` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - """ - - def __init__( - self, initial_se3, structure, g_canvas, - qlim, theta, axis_through=array([1, 0, 0])): - - if not isinstance( - structure, float) and not isinstance(structure, PurePath): - error_str = "structure must be of type {0} or {1}. Given {2}. " \ - "Either give a length (float)," \ - "or meshdata [filepath, scale, origindata]" - raise TypeError(error_str.format(float, str, type(structure))) - - self.qlim = qlim - self.theta = theta - - self.__scene = g_canvas.scene - self.__pose = initial_se3 - - # Set the graphic - # self.stl_offset = None - self.__graphic_obj = self.__set_graphic(structure, axis_through) - self.visible = True - - # Set the other reference frame vectors - self.__graphic_ref = draw_reference_frame_axes( - self.__pose, self.__scene) - - # Apply the initial pose - self.update_pose(self.__pose) - - def update_position(self, se_object): - """ - Given an SE object, update just the orientation of the joint. - - :param se_object: SE3 pose representation of the joint - :type se_object: class:`spatialmath.pose3d.SE3` - """ - new_position = get_pose_pos(se_object) - self.__graphic_obj.pos = new_position - - # Update the reference frame - self.__update_reference_frame() - self.draw_reference_frame(self.__graphic_ref.visible) - - def update_orientation(self, se_object): - """ - Given an SE object, update just the orientation of the joint. - - :param se_object: SE3 pose representation of the joint - :type se_object: class:`spatialmath.pose3d.SE3` - """ - # Get the new pose details - new_x_axis = get_pose_x_vec(se_object) - new_y_axis = get_pose_y_vec(se_object) - # new_z_axis = get_pose_z_vec(se_object) # not needed - - # Update the graphic object - self.__graphic_obj.axis = new_x_axis - self.__graphic_obj.up = new_y_axis - - # Update the reference frame - self.__update_reference_frame() - self.draw_reference_frame(self.__graphic_ref.visible) - - def update_pose(self, se_object): - """ - Given an SE object, update the pose of the joint. - - :param se_object: SE3 pose representation of the joint - :type se_object: class:`spatialmath.pose3d.SE3` - """ - # if self.stl_offset is not None: - # calc_se3 = se_object * self.stl_offset - # else: - # calc_se3 = se_object - - # Get the new pose details - new_x_axis = get_pose_x_vec(se_object) - new_y_axis = get_pose_y_vec(se_object) - # new_z_axis = get_pose_z_vec(se_object) # not needed - new_position = get_pose_pos(se_object) - - # Update the graphic object - self.__graphic_obj.pos = new_position - self.__graphic_obj.axis = new_x_axis - self.__graphic_obj.up = new_y_axis - - # Update the reference frame - self.__pose = se_object - self.__update_reference_frame() - self.draw_reference_frame(self.__graphic_ref.visible) - - def __update_reference_frame(self): - """ - Update the reference frame axis vectors - """ - self.__x_vector = get_pose_x_vec(self.__pose) - self.__y_vector = get_pose_y_vec(self.__pose) - self.__z_vector = get_pose_z_vec(self.__pose) - - def draw_reference_frame(self, is_visible): - """ - Draw a reference frame at the tool point position. - - :param is_visible: Whether the reference frame should be drawn or not - :type is_visible: `bool` - """ - self.__graphic_ref.pos = get_pose_pos(self.__pose) - self.__graphic_ref.axis = get_pose_x_vec(self.__pose) - self.__graphic_ref.up = get_pose_y_vec(self.__pose) - self.__graphic_ref.visible = is_visible - - def set_stl_joint_origin(self, current_location, required_location): - """ - Modify the origin position of the graphical object. - This is mainly used when loading STL objects. If the origin (point of - rotation/placement) does not align with - reality, it can be set. - It translates the object to place the origin at the required location, - and save the new origin to the object. - - :param current_location: 3D coordinate of where the real origin is in - space. - :type current_location: class:`vpython.vector` - :param required_location: 3D coordinate of where the real origin - should be in space - :type required_location: class:`vpython.vector` - """ - set_stl_origin( - self.__graphic_obj, current_location, - required_location, self.__scene) - - def set_joint_visibility(self, is_visible): - """ - Choose whether or not the joint is displayed in the canvas. - - :param is_visible: Whether the joint should be drawn or not - :type is_visible: `bool` - """ - # If the option is different to current setting - if is_visible is not self.visible: - # Update - self.__graphic_obj.visible = is_visible - # self.__graphic_ref.visible = is_visible - self.visible = is_visible - - def __set_graphic(self, structure, axis_through): - """ - Set the graphic object depending on if one was given. If no object was - given, create a box and return it - - :param structure: `float` or `str` representing the joint length or - STL path to load from - :type structure: `float`, `str` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - :raises ValueError: Joint length must be greater than 0 - :return: Graphical object for the joint - :rtype: class:`vpython.compound` - """ - if isinstance(structure, float): - length = structure - if length <= 0.0: - raise ValueError("Joint length must be greater than 0") - axis = vector(axis_through[0], axis_through[1], axis_through[2]) - axis.mag = length - - box_midpoint = axis / 2 - box_tooltip = axis - - # Create a box along the +x axis, with the origin - # (point of rotation) at (0, 0, 0) - graphic_obj = box( - canvas=self.__scene, - pos=vector(box_midpoint.x, box_midpoint.y, box_midpoint.z), - axis=axis, - size=vector(length, 0.1, 0.1), # L, W, H - # up=y_axis_vector - ) - - # Set the boxes new origin - graphic_obj = compound( - [graphic_obj], origin=box_tooltip, axis=axis) - - return graphic_obj - else: - # self.stl_offset = structure[2] - return import_object_from_numpy_stl(structure, self.__scene) - - def set_texture(self, colour=None, texture_link=None): - """ - Apply the texture/colour to the object. If both are given, both are - applied. - Texture link can either be a link to an online image, or local file. - - WARNING: If the texture can't be loaded, the object will have n - texture - (appear invisible, but not set as invisible). - - WARNING: If the image has a width or height that is not a power of 2 - (that is, not 2, 4, 8, 16, 32, 64, 128, 256, 512, 1024, etc.), - the image is stretched to the next larger width or height that is - a power of 2. - - :param colour: List of RGB values - :type colour: `list`, optional - :param texture_link: Path/website to a texture image - :type texture_link: `str`, optional - :raises ValueError: RGB values must be normalised between 0 and 1 - """ - # Apply the texture - if texture_link is not None: - self.__graphic_obj.texture = { - 'file': texture_link - # 'bumpmap', 'place', 'flipx', 'flipy', 'turn' - } - # Wait for the texture to load - self.__scene.waitfor("textures") - else: - # Remove any texture - self.__graphic_obj.texture = None - - # Apply the colour - if colour is not None: - if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \ - colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0: - raise ValueError( - "RGB values must be normalised between 0 and 1") - colour_vec = vector(colour[0], colour[1], colour[2]) - self.__graphic_obj.color = colour_vec - else: - # Set to white if none given - self.__graphic_obj.color = color.white - - def set_transparency(self, opacity): - """ - Sets the transparency of the joint. - - :param opacity: Normalised value (0 -> 1) to set the opacity. - 0 = transparent, 1 = opaque - :type opacity: `float` - :raises ValueError: Value must be between 0 and 1 inclusively - """ - if opacity < 0 or opacity > 1: - raise ValueError("Value must be between 0 and 1 inclusively") - self.__graphic_obj.opacity = opacity - - def get_axis_vector(self, axis): - """ - Get the current vector of a specified X, Y, or Z axis - - :param axis: Specified joint axis to get the angle of rotation of - :type axis: class:`vpython.vector` - :return: Current vector representation of the joints X, Y, or Z axis - :rtype: class:`vpython.vector` - :raise ValueError: The given axis_of_rotation must be one of the - default X, Y, Z vectors (e.g. x_axis_vector) - """ - # Return new vectors to avoid pass by reference - if axis.equals(x_axis_vector): - return self.__x_vector - elif axis.equals(y_axis_vector): - return self.__y_vector - elif axis.equals(z_axis_vector): - return self.__z_vector - else: - error_str = "Bad input vector given ({0}). " \ - "Must be either x_axis_vector ({1}), " \ - "y_axis_vector ({2})," \ - "or z_axis_vector ({3})." - raise ValueError( - error_str.format( - axis, x_axis_vector, y_axis_vector, z_axis_vector)) - - def get_pose(self): - """ - Return the current pose of the joint - - :return: SE3 representation of the current joint pose - :rtype: class:`spatialmath.pose3d.SE3` - """ - return self.__pose - - def get_joint_type(self): - """ - Return the type of joint (To Be Overridden by Child classes) - - :return: String representation of the joint type. - :rtype: `str` - """ - return "" - - def get_graphic_object(self): - """ - Getter function that returns the graphical object of the joint - - :return: VPython graphical entity of the joint - :rtype: class:`vpython.object` - """ - return self.__graphic_obj - - def get_scene(self): - """ - Getter function that returns the scene the object is in - - :return: The scene the object is in - :rtype: class:`vpython.canvas` - """ - return self.__scene - - -class RotationalJoint(DefaultJoint): # pragma nocover - """ - A rotational joint based off the default joint class - - :param g_canvas: The canvas in which to add the link - :type g_canvas: class:`graphics.graphics_canvas.graphicscanvas3d` - :param initial_se3: Pose to set the joint to initially - :type initial_se3: class:`spatialmath.pose3d.SE3` - :param structure: either a float of the length of the joint, or a list of - str of the filepath and scale - :type structure: `float`, `list` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - """ - - def __init__( - self, initial_se3, structure, g_canvas, qlim, - theta, axis_through=array([1, 0, 0])): - - # Call super init function - super().__init__( - initial_se3, structure, g_canvas, qlim, theta, axis_through) - - def get_joint_type(self): - """ - Return the type of joint (R for Rotational) - - :return: String representation of the joint type. - :rtype: `str` - """ - return "R" - - -class PrismaticJoint(DefaultJoint): # pragma nocover - """ - A prismatic joint based from the default joint class - - :param g_canvas: The canvas in which to add the link - :type g_canvas: class:`graphics.graphics_canvas.graphicscanvas3d` - :param initial_se3: Pose to set the joint to initially - :type initial_se3: class:`spatialmath.pose3d.SE3` - :param structure: either a float of the length of the joint, or a list of - str of the filepath and scale - :type structure: `float`, `list` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - """ - def __init__( - self, initial_se3, structure, g_canvas, qlim, theta, - axis_through=array([1, 0, 0])): - - super().__init__( - initial_se3, structure, g_canvas, qlim, theta, axis_through) - self.min_translation = None - self.max_translation = None - - def translate_joint(self, new_translation): - # TODO - pass - - def get_joint_type(self): - """ - Return the type of joint (P for Prismatic) - - :return: String representation of the joint type. - :rtype: `str` - """ - return "P" - - -class StaticJoint(DefaultJoint): # pragma nocover - """ - This class represents a static joint (one that doesn't translate or - rotate on it's own). - It has no extra functions to utilise. - - :param g_canvas: The canvas in which to add the link - :type g_canvas: class:`graphics.graphics_canvas.graphicscanvas3d` - :param initial_se3: Pose to set the joint to initially - :type initial_se3: class:`spatialmath.pose3d.SE3` - :param structure: either a float of the length of the joint, or a list of - str of the filepath and scale - :type structure: `float`, `list` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - """ - - def __init__( - self, initial_se3, structure, g_canvas, qlim, theta, - axis_through=array([1, 0, 0])): - - super().__init__( - initial_se3, structure, g_canvas, qlim, theta, axis_through) - - def get_joint_type(self): - """ - Return the type of joint (S for Static) - - :return: String representation of the joint type. - :rtype: `str` - """ - return "S" - - -class Gripper(DefaultJoint): # pragma nocover - """ - This class represents a gripper joint with a moving gripper - (To Be Implemented). - Usually the end joint of a robot. - - :param g_canvas: The canvas in which to add the link - :type g_canvas: class:`graphics.graphics_canvas.graphicscanvas3d` - :param initial_se3: Pose to set the joint to initially - :type initial_se3: class:`spatialmath.pose3d.SE3` - :param structure: either a float of the length of the joint, or a list - of str of the filepath and scale - :type structure: `float`, `list` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - """ - - def __init__( - self, initial_se3, structure, g_canvas, - qlim, theta, axis_through=array([1, 0, 0])): - - super().__init__( - initial_se3, structure, g_canvas, qlim, theta, axis_through) - - # TODO close/open gripper - - def get_joint_type(self): - """ - Return the type of joint (G for Gripper) - - :return: String representation of the joint type. - :rtype: `str` - """ - return "G" - - -class GraphicalRobot: # pragma nocover - """ - The GraphicalRobot class holds all of the different joints to easily - control the robot arm. - - :param graphics_canvas: The canvas to add the robot to - :type graphics_canvas: class:`GraphicsCanvas` - :param name: The name of the robot to identify it - :type name: `str` - :param robot: A serial link object to create a robot on - :type robot: class:`roboticstoolbox.robot.DHRobot` - """ - def __init__(self, graphics_canvas, name, robot=None): - self.joints = [] - self.num_joints = 0 - self.rob_shown = True - self.ref_shown = True - self.opacity = 1 - self.name = name - self.__scene = graphics_canvas - - self.angles = [] - self.robot = robot - - # If Robot given, create the robot - if self.robot is not None: - # Update name - self.name = self.robot.name - # Get initial poses - zero_angles = np.zeros((self.robot.n,)) - all_poses = self.robot.fkine_all(zero_angles, old=False) - # Create the base - try: - if robot.basemesh is not None: - self.append_link("s", all_poses[0], robot.basemesh, [0, 0], 0) - except: - pass - # else: assume no base joint - robot_colours = robot.linkcolormap() - # Create the joints - for i, link in enumerate(self.robot.links): - # Get info - if link.isprismatic: - j_type = 'p' - elif link.isrevolute: - j_type = 'r' - else: - j_type = 's' - pose = all_poses[i+1] # link frame pose - if link.mesh is None: - if i == 0: - x1, x2 = SE3().t[0], all_poses[i].t[0] - y1, y2 = SE3().t[1], all_poses[i].t[1] - z1, z2 = SE3().t[2], all_poses[i].t[2] - length = sqrt( - (x2 - x1) * (x2 - x1) + (y2 - y1) - * (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length - else: - x1, x2 = all_poses[i - 1].t[0], all_poses[i].t[0] - y1, y2 = all_poses[i - 1].t[1], all_poses[i].t[1] - z1, z2 = all_poses[i - 1].t[2], all_poses[i].t[2] - length = sqrt( - (x2 - x1) * (x2 - x1) + (y2 - y1) - * (y2 - y1) + (z2 - z1) * (z2 - z1)) # Length - else: - length = link.mesh - angle_lims = link.qlim # Angle limits - theta = link.theta # Current angle - self.append_link(j_type, pose, length, angle_lims, theta) - - # Apply colours - for i, joint in enumerate(self.joints): - link_colour = list(robot_colours(i))[:3] - joint.set_texture(colour=link_colour) - - # Add the robot to the canvas UI - graphics_canvas.add_robot(self) - - def append_made_link(self, joint): - """ - Append an already made joint to the end of the robot - (Useful for links manually created) - - :param joint: A joint object already constructed - :type joint: class:`graphics.graphics_robot.RotationalJoint`, - class:`graphics.graphics_robot.PrismaticJoint`, - class:`graphics.graphics_robot.StaticJoint`, - class:`graphics.graphics_robot.Gripper` - :raises RuntimeError: Ensure the link is in the same scene as the robot - """ - if joint.get_scene() != self.__scene.scene: - raise RuntimeError( - "The given made link is not in the same scene " - "as the robot is.") - - self.joints.append(joint) - self.num_joints += 1 - self.angles.append(joint.theta) - - def append_link( - self, typeof, pose, structure, qlim, theta, - axis_through=array([1, 0, 0])): - """ - Append a joint to the end of the robot. - - :param typeof: String character of the joint type. e.g. 'R', 'P', - 'S', 'G' - :type typeof: `str` - :param pose: SE3 object for the pose of the joint - :type pose: class:`spatialmath.pose3d.SE3` - :param structure: either a float of the length of the joint, or a list - of str of the filepath and scale - :type structure: `float`, `list` - :param qlim: A list of the angle limits for the joint - :type qlim: `list` - :param theta: The current angle of the joint in radians - :type theta: `float` - :param axis_through: The axis that the longest side goes through - :type axis_through: class:`numpy.ndarray` - :raises ValueError: typeof must be a valid character - """ - # Capitalise the type for case-insensitive use - typeof = typeof.upper() - - if typeof == 'R': - link = RotationalJoint( - pose, structure, self.__scene, qlim, theta, axis_through) - elif typeof == 'P': - link = PrismaticJoint( - pose, structure, self.__scene, qlim, theta, axis_through) - elif typeof == 'S': - link = StaticJoint( - pose, structure, self.__scene, qlim, theta, axis_through) - elif typeof == 'G': - link = Gripper( - pose, structure, self.__scene, qlim, theta, axis_through) - else: - raise ValueError( - "typeof should be (case-insensitive) either 'R' (Rotational)" - ", 'P' (Prismatic), 'S' (Static), or 'G' (Gripper)") - - # Append the joint to the robot - self.joints.append(link) - self.num_joints += 1 - self.angles.append(theta) - - def detach_link(self): - """ - Detach the end link of the robot. - - :raises UserWarning: Must have a joint available to detach - """ - # Check if no joints to detach - if self.num_joints == 0: - raise UserWarning("No robot joints to detach") - - # Turn off the graphics in the canvas - self.joints[self.num_joints - 1].set_joint_visibility(False) - self.joints[self.num_joints - 1].draw_reference_frame(False) - # Ensure deletion - self.joints[self.num_joints - 1] = None - - # Resize lists - self.joints = self.joints[0:self.num_joints - 1] - self.angles = self.angles[0:self.num_joints - 1] - self.num_joints -= 1 - - def set_robot_visibility(self, is_visible): - """ - Set the entire robots visibility inside the canvas. - - :param is_visible: Whether the robot should be visible or not. - :type is_visible: `bool` - """ - if is_visible is not self.rob_shown: - for joint in self.joints: - joint.set_joint_visibility(is_visible) - self.rob_shown = is_visible - - def set_reference_visibility(self, is_visible): - """ - Set the visibility of the reference frame for all joints. - - :param is_visible: Whether the reference frames should be - visible or not. - :type is_visible: `bool` - """ - if is_visible is not self.ref_shown: - for joint in self.joints: - joint.draw_reference_frame(is_visible) - self.ref_shown = is_visible - - def set_transparency(self, opacity): - """ - Set the transparency of the robot. - Allows for easier identification of the reference frames - (if hidden by the robot itself) - - :param opacity: Normalised value (0 -> 1) to set the opacity. - 0 = transparent, 1 = opaque - :type opacity: `float` - """ - if opacity is not self.opacity: - for joint in self.joints: - joint.set_transparency(opacity) - self.opacity = opacity - - def set_joint_poses(self, all_poses): - """ - Set the joint poses. - - :param all_poses: List of all the new poses to set - :type all_poses: class:`spatialmath.pose3d.SE3` list - :raises UserWarning: Robot must not have 0 joints, and given poses - length must equal number of joints. - """ - # Sanity checks - if self.num_joints == 0: - raise UserWarning( - "Robot has 0 joints. Create some using append_link()") - - # If given a base pose, when there isn't one - if self.num_joints == len(all_poses) - 1 and all_poses[0] == SE3(): - # Update the joints - for idx in range(self.num_joints): - self.joints[idx].update_pose(all_poses[idx+1]) - return - - # If given all poses excluding a base pose - if self.num_joints - 1 == len(all_poses): - for idx in range(1, self.num_joints): - self.joints[idx].update_pose(all_poses[idx-1]) - return - - # If incorrect number of joints - if self.num_joints != len(all_poses): - err = "Number of given poses {0} does not equal number " \ - "of joints {1}" - raise UserWarning(err.format(len(all_poses), self.num_joints)) - - # Update the joints - for idx in range(self.num_joints): - self.joints[idx].update_pose(all_poses[idx]) - - def animate(self, frame_poses, fps): - """ - Calling this function will animate the robot through its frames. - - :param frame_poses: A 2D list of each joint pose for each frame. - :type frame_poses: `list` - :param fps: Number of frames per second to render at - (limited by number of graphics trying to update) - :type fps: `int` - :raises ValueError: Number of frames and fps must be greater than 0 - """ - num_frames = len(frame_poses) - # Validate num_frames - if num_frames == 0: - raise ValueError( - "0 frames were given. Supply at least 1 iteration of poses.") - - if fps <= 0: - raise ValueError( - "fps must be greater than 0.") - f = 1 / fps - - for poses in frame_poses: - # Get current time - t_start = perf_counter() - - self.set_joint_poses(poses) # Validation done in set_joint_poses - # Wait for scene to finish drawing - self.__scene.scene.waitfor("draw_complete") - - # Get current time - t_stop = perf_counter() - - # Wait for time of frame to finish - # If drawing takes longer than frame frequency, - # this while is skipped - while t_stop - t_start < f: - t_stop = perf_counter() - - def fkine_and_set(self, joint_angles): - """ - Call fkine for the robot. If it is based on a seriallink object, - run it's fkine function. - - :param joint_angles: List of the joint angles - :type joint_angles: `list` - """ - # If seriallink object, run it's fkine - if self.robot is not None: - poses = self.robot.fkine_all(joint_angles) - if joint_angles is None: - joint_angles = self.robot.q - for a_idx in range(len(joint_angles)): - # Ignore the base's angle (idx == 0) - self.angles[a_idx+1] = joint_angles[a_idx] - self.set_joint_poses(poses) - # Else TODO - else: - pass - - def print_joint_poses(self): - """ - Print all of the current joint poses - """ - # For each joint - num = 0 - print(self.name) - for joint in self.joints: - print("Joint", num, "| Type:", joint.get_joint_type(), "| Pose:") - print(joint.get_pose(), "\n") - num += 1 diff --git a/roboticstoolbox/backends/VPython/grid.py b/roboticstoolbox/backends/VPython/grid.py deleted file mode 100644 index f3ce44faf..000000000 --- a/roboticstoolbox/backends/VPython/grid.py +++ /dev/null @@ -1,624 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -from vpython import vector, compound, mag, box, curve -from numpy import sign, ceil, arange -from roboticstoolbox.backends.VPython.text import update_grid_numbers, GridType -from roboticstoolbox.backends.VPython.object2d import Marker2D -from spatialmath import SE2 -from collections.abc import MutableMapping - - -class GridMMap(MutableMapping): # pragma nocover - def __init__(self, *args, **kwargs): - self.__dict__.update(*args, **kwargs) - - def __setitem__(self, key, value): - self.__dict__[key] = value - - def __getitem__(self, key): - return self.__dict__[key] - - def __delitem__(self, key): - del self.__dict__[key] - - def __iter__(self): - return iter(self.__dict__) - - def __len__(self): - return len(self.__dict__) - - -class GraphicsGrid: # pragma nocover - """ - This class holds the current grid displayed in the canvas - - :param scene: The scene in which to add the grid - :type scene: class:`vpython.canvas` - """ - - def __init__(self, scene, colour=None): - - # Save the scene the grid is placed in - self.__scene = scene - self._mode = GridType.XYZ - - self.__relative_cam = True - self.__num_squares = 10 - self.__scale = 1 - - # Save the current camera settings - self.camera_pos = self.__scene.camera.pos - self.camera_axes = self.__scene.camera.axis - self.__focal_point = [ - round(self.__scene.center.x), - round(self.__scene.center.y), - round(self.__scene.center.z) - ] - - if colour is None: - colour = [0, 0, 0] - self._colour = colour - - # Initialise a grid object - line_thickness = min(max(self.__scale / 25, 0.01), 5) # 0.01 -> 5 - - self.grid_object = GridMMap({ - 'xy_plane': curve(vector(0, 0, 0), - color=vector(self._colour[0], self._colour[1], self._colour[2]), - radius=line_thickness, - origin=vector(0, 0, 0)), - 'xz_plane': curve(vector(0, 0, 0), - color=vector(self._colour[0], self._colour[1], self._colour[2]), - radius=line_thickness, - origin=vector(0, 0, 0)), - 'yz_plane': curve(vector(0, 0, 0), - color=vector(self._colour[0], self._colour[1], self._colour[2]), - radius=line_thickness, - origin=vector(0, 0, 0)), - 'labels': [], - }) - self.__init_grid() - - # Bind mouse releases to the update_grid function - self.__scene.bind('mouseup keyup', self.update_grid) - - def __init_grid(self): - """ - Initialise the grid along the x, y, z axes. - """ - # self.grid_object[self.__planes_idx] = self.__create_grid_objects() - self.__create_grid_objects() - - # Update the labels instead of recreating them - update_grid_numbers( - self.__focal_point, self.grid_object.get('labels'), - self.__num_squares, self.__scale, self._mode, self.__scene) - - self.__move_grid_objects() - - self._set_mode_plane_visibility() - - def __create_grid_objects(self): - """ - Draw a grid along each 3D plane, that is closest to the camera. - """ - # Create grid from 0,0,0 to positive directions with step sizes of the scale - min_x_coord = 0 - max_x_coord = min_x_coord + round(self.__num_squares * self.__scale, 2) - min_y_coord = 0 - max_y_coord = min_y_coord + round(self.__num_squares * self.__scale, 2) - min_z_coord = 0 - max_z_coord = min_z_coord + round(self.__num_squares * self.__scale, 2) - - # Get all coords - x_coords = arange( - min_x_coord, max_x_coord + self.__scale, self.__scale) - y_coords = arange( - min_y_coord, max_y_coord + self.__scale, self.__scale) - z_coords = arange( - min_z_coord, max_z_coord + self.__scale, self.__scale) - - # If the grid has given too many objects - if len(x_coords) > self.__num_squares + 1: - x_coords = x_coords[0:self.__num_squares + 1] - if len(y_coords) > self.__num_squares + 1: - y_coords = y_coords[0:self.__num_squares + 1] - if len(z_coords) > self.__num_squares + 1: - z_coords = z_coords[0:self.__num_squares + 1] - - # As curve objects cannot be compounded, so must be a single entity - - line_thickness = min(max(self.__scale / 25, 0.01), 2) # 0.01 -> 5 - - # Update curve objects - self.grid_object.get('xy_plane').radius = line_thickness - self.grid_object.get('xy_plane').append(vector(0, 0, 0)) - - self.grid_object.get('xz_plane').radius = line_thickness - self.grid_object.get('xz_plane').append(vector(0, 0, 0)) - - self.grid_object.get('yz_plane').radius = line_thickness - self.grid_object.get('yz_plane').append(vector(0, 0, 0)) - - # Zig-Zag through all of the points - # XY plane - for idx, x_point in enumerate(x_coords): - if idx % 2 == 0: - y_vals = y_coords - else: - y_vals = y_coords[::-1] - for y_point in y_vals: - self.grid_object.get('xy_plane'). \ - append(vector(x_point, y_point, 0)) - - for idx, y_point in enumerate(y_coords[::-1]): - if idx % 2 == 0: - x_vals = x_coords[::-1] - else: - x_vals = x_coords - for x_point in x_vals: - self.grid_object.get('xy_plane'). \ - append(vector(x_point, y_point, 0)) - - # XZ Plane - for idx, x_point in enumerate(x_coords): - if idx % 2 == 0: - z_vals = z_coords - else: - z_vals = z_coords[::-1] - for z_point in z_vals: - self.grid_object.get('xz_plane'). \ - append(vector(x_point, 0, z_point)) - - for idx, z_point in enumerate(z_coords[::-1]): - if idx % 2 == 0: - x_vals = x_coords[::-1] - else: - x_vals = x_coords - for x_point in x_vals: - self.grid_object.get('xz_plane'). \ - append(vector(x_point, 0, z_point)) - - # YZ Plane - for idx, y_point in enumerate(y_coords): - if idx % 2 == 0: - z_vals = z_coords - else: - z_vals = z_coords[::-1] - for z_point in z_vals: - self.grid_object.get('yz_plane'). \ - append(vector(0, y_point, z_point)) - - for idx, z_point in enumerate(z_coords[::-1]): - if idx % 2 == 0: - y_vals = y_coords[::-1] - else: - y_vals = y_coords - for y_point in y_vals: - self.grid_object.get('yz_plane'). \ - append(vector(0, y_point, z_point)) - - def __move_grid_objects(self): - """ - Reusing the current assets, move the planes to the new origins. - """ - camera_axes = self.camera_axes - # Locate centre of axes - if self.__relative_cam: - if self._mode == GridType.XYZ: - x_origin, y_origin, z_origin = round(self.__scene.center.x, 2), \ - round(self.__scene.center.y, 2), \ - round(self.__scene.center.z, 2) - self.__focal_point = [x_origin, y_origin, z_origin] - # Convert focal point for 2D rendering. - # Puts focus point in centre of the view - elif self._mode == GridType.XY2D: - self.__focal_point = [ - val - int(self.__num_squares / 2) - for val in self.__focal_point - ] - x_origin = self.__focal_point[0] - y_origin = self.__focal_point[1] - z_origin = 0 - self.__focal_point[2] = z_origin - elif self._mode == GridType.XY3D: - x_origin = round(self.__scene.center.x, 2) - y_origin = round(self.__scene.center.y, 2) - z_origin = 0 - self.__focal_point = [x_origin, y_origin, z_origin] - else: - x_origin, y_origin, z_origin = round(self.__scene.center.x, 2), \ - round(self.__scene.center.y, 2), \ - round(self.__scene.center.z, 2) - self.__focal_point = [x_origin, y_origin, z_origin] - else: - x_origin, y_origin, z_origin = self.__focal_point[0], \ - self.__focal_point[1], \ - self.__focal_point[2] - - # CAMERA AXES | DISPLAYED GRID | XZ PLANE | XY PLANE | YZ PLANE - # x,y,z | x,y,z | x,z | x,y | y,z - # -------------+-----------------+----------+----------+---------- - # -,-,- | +,+,+ | +,+ | +,+ | +,+ - # -,-,+ | +,+,- | +,- | +,+ | +,- - # -,+,- | +,-,+ | +,+ | +,- | -,+ - # -,+,+ | +,-,- | +,- | +,- | -,- - # +,-,- | -,+,+ | -,+ | -,+ | +,+ - # +,-,+ | -,+,- | -,- | -,+ | +,- - # +,+,- | -,-,+ | -,+ | -,- | -,+ - # +,+,+ | -,-,- | -,- | -,- | -,- - # min = -num_squares or 0, around the default position - # max = +num_squares or 0, around the default position - # e.g. at the origin, for negative axes: -10 -> 0, - # positive axes: 0 -> 10 - min_x_coord = round( - x_origin + (-(self.__num_squares / 2) - + (sign(camera_axes.x) * -1) * ( - self.__num_squares / 2)) * self.__scale, 2) - - min_y_coord = round( - y_origin + (-(self.__num_squares / 2) - + (sign(camera_axes.y) * -1) * ( - self.__num_squares / 2)) * self.__scale, 2) - - min_z_coord = round( - z_origin + (-(self.__num_squares / 2) - + (sign(camera_axes.z) * -1) * ( - self.__num_squares / 2)) * self.__scale, 2) - - if camera_axes.x < 0: - x_pos = x_origin - else: - x_pos = min_x_coord - if camera_axes.y < 0: - y_pos = y_origin - else: - y_pos = min_y_coord - if camera_axes.z < 0: - z_pos = z_origin - else: - z_pos = min_z_coord - - if self._mode == GridType.XY3D: - x_pos = x_pos + sign(camera_axes.x) * (self.__scale * (self.__num_squares / 2)) - y_pos = y_pos + sign(camera_axes.y) * (self.__scale * (self.__num_squares / 2)) - - self.grid_object.get('xy_plane').origin = \ - vector(x_pos, y_pos, z_origin) - self.grid_object.get('xz_plane').origin = \ - vector(x_pos, y_origin, z_pos) - self.grid_object.get('yz_plane').origin = \ - vector(x_origin, y_pos, z_pos) - - def update_grid(self): - """ - Update the grid axes and numbers if the camera position/rotation - has changed. - """ - # If invisible, skip the updating (Unnecessary) - if not self.grid_object.get('xy_plane'). \ - visible: - return - - # Obtain the new camera settings - new_camera_pos = vector(self.__scene.camera.pos) - new_camera_axes = vector(self.__scene.camera.axis) - - old_camera_pos = vector(self.camera_pos) - old_camera_axes = vector(self.camera_axes) - - # Update old positions - self.camera_pos = new_camera_pos - self.camera_axes = new_camera_axes - - distance_from_center = mag( - self.__scene.center - self.__scene.camera.pos) - - if self._mode == GridType.XYZ: - new_scale = round(distance_from_center / 30.0, 1) - else: - new_scale = round(distance_from_center / 15.0, 1) - - new_scale = round(round(new_scale / 0.2) * 0.2, 1) # Round to nearest 0.2 multiple - new_scale = max(min(new_scale, 100), 0.2) # Between 0.2 and 100 - - if not new_scale == self.__scale: - self.set_scale(new_scale) - self._set_mode_plane_visibility() - - # If camera is different to previous: update - if (not new_camera_axes.equals(old_camera_axes)) or ( - not new_camera_pos.equals(old_camera_pos)): - # Update grid - self.__move_grid_objects() - update_grid_numbers(self.__focal_point, - self.grid_object.get('labels'), - self.__num_squares, - self.__scale, - self._mode, - self.__scene) - - def _set_mode_plane_visibility(self): - """ - Turn off Z-planes if 2D mode selected - """ - # Turn visibility False if XY only - z_visibility = not (self._mode == GridType.XY2D or self._mode == GridType.XY3D) - - # Toggle it for XZ, YZ planes - self.grid_object.get('xz_plane').visible = z_visibility - self.grid_object.get('yz_plane').visible = z_visibility - - # Toggle it for Z plane numbers - # Index start = (num_squares + 1) (11 numbers shown for 10 squares) * - # 2 axes + 2 letters for axes - z_label_start = (self.__num_squares + 1) * 2 + 2 - # Index end = end of labels array - z_label_end = len(self.grid_object.get('labels')) - # Toggle - for idx in range(z_label_start, z_label_end): - self.grid_object.get('labels')[idx].visible = z_visibility - - self.update_grid() - - def set_visibility(self, is_visible): - """ - Set the visibility of the grid - - :param is_visible: Boolean of whether to display the grid - :type is_visible: `bool` - """ - # Modify all graphics - self.grid_object.get('xy_plane').visible = is_visible - self.grid_object.get('xz_plane').visible = is_visible - self.grid_object.get('yz_plane').visible = is_visible - for number in self.grid_object.get('labels'): - number.visible = is_visible - - # Update for 2D if required - if is_visible: - self._set_mode_plane_visibility() - - def set_relative(self, is_relative): - """ - Set whether the grid should be locked to (0, 0, 0) or relative to - camera focus point - - :param is_relative: Whether the camera is dynamic (True) or - static (False) - :type is_relative: `bool` - """ - self.__relative_cam = is_relative - self.update_grid() - - def set_scale(self, value): - """ - Set the scale and redraw the grid - - :param value: The value to set the scale to - :type value: `float` - """ - # If invisible, skip the updating (Unnecessary) - if not self.grid_object.get('xy_plane').visible: - return - - value = round(round(value / 0.2) * 0.2, 1) # Round to nearest 0.2 multiple - value = max(min(value, 100), 0.2) # Between 0.2 and 100 - self.__scale = value - - self._refresh_grid() - - def _refresh_grid(self): - # Turn off grid then delete - self.grid_object.get('xy_plane').clear() - self.grid_object.get('xz_plane').clear() - self.grid_object.get('yz_plane').clear() - for text in self.grid_object.get('labels'): - text.visible = False - - # Don't del the curve objects - self.grid_object.labels = [] - self.__init_grid() - - def set_mode(self, mode): - """ - - """ - # Sanitise input - if not type(mode) == GridType: - err = "Invalid parameter type given. Expected {0}, given {1}." - raise UserWarning(err.format(GridType, type(mode))) - # Apply - self._mode = mode - self._refresh_grid() - - -def create_line(pos1, pos2, scene, - colour=None, thickness=0.01, opacity=1): # pragma nocover - """ - Create a line from position 1 to position 2. - - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :param pos1: 3D position of one end of the line. - :type pos1: class:`vpython.vector` - :param pos2: 3D position of the other end of the line. - :type pos2: class:`vpython.vector` - :param colour: RGB list to colour the line to - :type colour: `list` - :param thickness: Thickness of the line - :type thickness: `float` - :param opacity: Opacity of the line - :type opacity: `float` - :raises ValueError: RGB colour must be normalised between 0->1 - :raises ValueError: Thickness must be greater than 0 - :return: A box resembling a line - :rtype: class:`vpython.box` - """ - # Set default colour - # Stops a warning about mutable parameter - if colour is None: - colour = [0, 0, 0] - - if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \ - colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0: - raise ValueError("RGB values must be normalised between 0 and 1") - - if thickness < 0.0: - raise ValueError("Thickness must be greater than 0") - - # Length of the line using the magnitude - line_len = mag(pos2 - pos1) - - # Position of the line is the midpoint (centre) between the ends - position = (pos1 + pos2) / 2 - - # Axis direction of the line (to align the box (line) to intersect the - # two points) - axis_dir = pos2 - pos1 - - # Return a box of thin width and height to resemble a line - # thickness = 0.01 - return box(canvas=scene, - pos=position, - axis=axis_dir, - length=line_len, - width=thickness, - height=thickness, - color=vector(colour[0], colour[1], colour[2]), - opacity=opacity) - - -def create_segmented_line( - pos1, pos2, scene, segment_len, - colour=None, thickness=0.01): # pragma nocover - """ - Create a dashed line from position 1 to position 2. - - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :param pos1: 3D position of one end of the line. - :type pos1: class:`vpython.vector` - :param pos2: 3D position of the other end of the line. - :type pos2: class:`vpython.vector` - :param colour: RGB list to colour the line to - :type colour: `list` - :param thickness: Thickness of the line - :type thickness: `float` - :param segment_len: The length of the segment, and gap between segments - :type segment_len: `float` - :raises ValueError: RGB colour must be normalised between 0->1 - :raises ValueError: Thickness must be greater than 0 - :return: A box resembling a line - :rtype: class:`vpython.box` - """ - # Set default colour - # Stops a warning about mutable parameter - if colour is None: - colour = [0, 0, 0] - - if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \ - colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0: - raise ValueError("RGB values must be normalised between 0 and 1") - - if thickness < 0.0: - raise ValueError("Thickness must be greater than 0") - - # Length of the line using the magnitude - line_len = mag(pos2 - pos1) - - # Axis direction of the line - # (to align the box (line) to intersect the two points) - axis_dir = pos2 - pos1 - axis_dir.mag = 1.0 - - # Return a compound of boxes of thin width and height to - # resemble a dashed line - dash_positions = [] - boxes = [] - # Translate centre pos to centre of where dashes will originate from - pos1 = pos1 + (axis_dir * segment_len / 2) - - # Range = number of dashes (vis and invis) - for idx in range(0, int(ceil(line_len / (segment_len / axis_dir.mag)))): - # Add every even point (zeroth, second...) to skip gaps between boxes - if idx % 2 == 0: - dash_positions.append(pos1) - pos1 = (pos1 + axis_dir * segment_len) - # If the axis between points changes, then the line has surpassed - # the end point. The line is done - check_dir = pos2 - pos1 - check_dir.mag = 1.0 - if not vectors_approx_equal(axis_dir, check_dir): - break - - for xyz in dash_positions: - length = segment_len - # If the box will surpass the end point - len_to_end = (pos2 - xyz).mag - if len_to_end < segment_len / 2: - # Length is equal to dist to the end * 2 (as pos is middle of box) - length = len_to_end * 2 - - boxes.append( - box( - canvas=scene, - pos=xyz, - axis=axis_dir, - length=length, - width=thickness, - height=thickness, - color=vector(colour[0], colour[1], colour[2]) - ) - ) - - return compound(boxes) - - -def create_marker(scene, x, y, shape, colour=None): # pragma nocover - """ - Draw the shape at the given position - - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :param x: The x location to draw the object at - :type x: `float` - :param y: The y location to draw the object at - :type y: `float` - :param shape: The shape of the object to draw - :type shape: `str` - :param colour: The colour of the object - :type colour: `list` - :returns: A 2D object that has been drawn - :rtype: class:`graphics.graphics_object2d.Object2D` - """ - # Set default colour - # Stops a warning about mutable parameter - if colour is None: - colour = [0, 0, 0] - - # Create an SE2 for the object - obj_se2 = SE2(x=x, y=y, theta=0) - - # Create the object and return it - return Marker2D(obj_se2, scene, shape, colour) - - -def vectors_approx_equal(v1, v2): # pragma nocover - """ - Check whether the vectors are approximately equal. - This is used where there is VERY minor floating point differences - that can occur in VPython. - - :param v1: Vector 1 - :type v1: class:`vpython.vector` - :param v2: Vector 2 - :type v2: class:`vpython.vector` - :returns: True if vectors are within tolerance - :rtype: `bool` - """ - return abs(v1.x - v2.x < 0.001) and abs(v1.y - v2.y < 0.001) and \ - abs(v1.z - v2.z < 0.001) diff --git a/roboticstoolbox/backends/VPython/object2d.py b/roboticstoolbox/backends/VPython/object2d.py deleted file mode 100644 index 31107e1b1..000000000 --- a/roboticstoolbox/backends/VPython/object2d.py +++ /dev/null @@ -1,247 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -from vpython import shapes, radians, extrusion, vector -from roboticstoolbox.backends.VPython.stl import import_object_from_numpy_stl - - -class Object2D: # pragma nocover - """ - This object will allow the ability to update it's pose in the scene. - For example, it could resemble a car or drone that moves around in - 2D space. - - :param se2: The SE2 object representing position and orientation - :type se2: class:`spatialmath.se2` - :param scene: The scene in which to add the link - :type scene: class:`vpython.canvas` - :param shape: The shape of the object - :type shape: `str` - :param colour: The colour of the shape - :type colour: `list` - :raises ValueError: The shape must be in the list of possible shapes - """ - def __init__(self, se2, scene, shape, colour): - # Save inputs - self.se2 = se2 - self.scene = scene - self.shape = shape - self.size = 0 - if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \ - colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0: - raise ValueError("RGB values must be normalised between 0 and 1") - self.colourVec = vector(colour[0], colour[1], colour[2]) - self.graphic = None - - def __create_object(self): - """ - To be overridden by child classes - """ - pass - - def update_pose(self, new_se2): - """ - Update the pose of the object - - :param new_se2: The SE2 representation of the pose - :type new_se2: class:`spatialmath.se2` - """ - self.se2 = new_se2 - x = self.se2.t[0] - y = self.se2.t[1] - t = self.se2.theta - self.graphic.pos = vector(x, y, 0) - self.graphic.axis = vector(0, 1, 0).rotate(t) - - def update_colour(self, colour): - """ - Update the colour of the object - - :param colour: The RGB colour - :type colour: `list` - """ - if colour[0] > 1.0 or colour[1] > 1.0 or colour[2] > 1.0 or \ - colour[0] < 0.0 or colour[1] < 0.0 or colour[2] < 0.0: - raise ValueError("RGB values must be normalised between 0 and 1") - self.graphic.color = vector(colour[0], colour[1], colour[2]) - self.colourVec = vector(colour[0], colour[1], colour[2]) - - def update_visibility(self, is_visible): - """ - Update the visibility of the object - - :param is_visible: Whether to draw or not - :type is_visible: `bool` - """ - self.graphic.visible = is_visible - - def update_size(self, multiply): - """ - Update the size of the object by a multiple of the original size. - - :param multiply: A number to multiply the original size by - :type multiply: `float`, `int` - """ - self.graphic.size = self.size * multiply - - -class STL2D(Object2D): # pragma nocover - """ - This object is for 2D objects that contain an STL. - - :param se2: The SE2 object representing position and orientation - :type se2: class:`spatialmath.se2` - :param scene: The scene in which to add the link - :type scene: class:`vpython.canvas` - :param stl_path: The file path to the STL to apply - :type stl_path: `str` - :param colour: The colour of the shape - :type colour: `list` - """ - def __init__(self, se2, scene, stl_path, colour): - super().__init__(se2, scene, stl_path, colour) - self.graphic = self.__create_object() - self.size = self.graphic.size - self.graphic.color = self.colourVec - - def __create_object(self): - """ - Return a compound of the loaded STL - - :return: A compound object of the triangles in the STL - :rtype: class:`vpython.compound` - """ - return import_object_from_numpy_stl(self.shape, self.scene) - - -class Marker2D(Object2D): # pragma nocover - """ - This class will place a marker in the given location based on the given - marker inputs - - :param se2: The SE2 object representing position and orientation - :type se2: class:`spatialmath.se2` - :param scene: The scene in which to add the link - :type scene: class:`vpython.canvas` - :param shape: The shape of the object - :type shape: `str` - :param colour: The colour of the shape - :type colour: `list` - :raises ValueError: The shape must be in the list of possible shapes - """ - def __init__(self, se2, scene, shape, colour): - super().__init__(se2, scene, shape, colour) - - self.__marker_size = 0.2 - - marker_styles = [ - '', # None - '+', # Plus - 'o', # Circle - '*', # Star - '.', # Dot - 'x', # Cross - 's', # Square - 'd', # Diamond - '^', # Up triangle - 'v', # Down triangle - '<', # Left triangle - '>', # Right triangle - 'p', # Pentagon - 'h', # Hexagon - ] - - if shape not in marker_styles: - raise ValueError( - "The shape must be in the list of possible shapes") - - # Create the object - self.graphic = self.__create_object() - - def __create_object(self): - """ - Create the physical graphical object - - :returns: The graphical entity - :rtype: class:`vpython.baseobj` - """ - if self.shape == '': - # 2D coords of the circle boundary - shape_path = shapes.circle(radius=self.__marker_size / 2) - elif self.shape == '+': - # 2D coords of the cross boundary - shape_path = shapes.cross( - width=self.__marker_size, thickness=self.__marker_size / 5) - elif self.shape == 'o': - # 2D coords of the circle boundary - shape_path = shapes.circle(radius=self.__marker_size / 2) - elif self.shape == '*': - # 2D coords of the star boundary - shape_path = shapes.star(radius=self.__marker_size / 2, n=6) - elif self.shape == '.': - # 2D coords of the square boundary - shape_path = shapes.rectangle( - width=self.__marker_size, height=self.__marker_size) - elif self.shape == 'x': - # 2D coords of the cross boundary - shape_path = shapes.cross( - width=self.__marker_size, thickness=self.__marker_size / 5, - rotate=radians(45)) - elif self.shape == 's': - # 2D coords of the square boundary - shape_path = shapes.rectangle( - width=self.__marker_size, height=self.__marker_size, - thickness=self.__marker_size) - elif self.shape == 'd': - # 2D coords of the diamond boundary - shape_path = shapes.rectangle( - width=self.__marker_size, height=self.__marker_size, - thickness=self.__marker_size, rotate=radians(45)) - elif self.shape == '^': - # 2D coords of the triangle boundary - shape_path = shapes.triangle( - length=self.__marker_size) - elif self.shape == 'v': - # 2D coords of the triangle boundary - shape_path = shapes.triangle( - length=self.__marker_size, rotate=radians(180)) - elif self.shape == '<': - # 2D coords of the triangle boundary - shape_path = shapes.triangle( - length=self.__marker_size, rotate=radians(90)) - elif self.shape == '>': - # 2D coords of the triangle boundary - shape_path = shapes.triangle( - length=self.__marker_size, rotate=radians(-90)) - elif self.shape == 'p': - # 2D coords of the pentagon boundary - shape_path = shapes.pentagon( - length=self.__marker_size) - elif self.shape == 'h': - # 2D coords of the hexagon boundary - shape_path = shapes.hexagon( - length=self.__marker_size) - # CURRENTLY UNUSED - # elif self.__shape == 'o': - # # 2D coords of the octagon boundary - # shape_path = shapes.octagon(length=1) - # elif self.__shape == 'r': - # # 2D coords of the ring boundary (with thickness = 10%) - # shape_path = shapes.circle(radius=0.5, thickness=0.1) - else: - raise ValueError("Invalid shape given") - - # Create the shape - x = self.se2.t[0] - y = self.se2.t[1] - obj = extrusion(scene=self.scene, - path=[vector(x, y, 0.001), vector(x, y, -0.001)], - shape=shape_path, - color=self.colourVec, - shininess=0) - self.size = obj.size - if self.shape == '': - obj.visible = False - return obj diff --git a/roboticstoolbox/backends/VPython/stl.py b/roboticstoolbox/backends/VPython/stl.py deleted file mode 100644 index e961a99aa..000000000 --- a/roboticstoolbox/backends/VPython/stl.py +++ /dev/null @@ -1,130 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - -from vpython import vec, vertex, color, triangle, compound -from roboticstoolbox.backends.VPython.common_functions import \ - vector, x_axis_vector -from io import StringIO - -try: - from spatialgeometry.tools.stdout_supress import pipes -except Exception: # pragma nocover - from contextlib import contextmanager - - @contextmanager - def pipes(stdout=None, stderr=None): - pass - -_out = StringIO() -_err = StringIO() -try: - with pipes(stdout=_out, stderr=_err): - from stl import mesh -except Exception: # pragma nocover - from stl import mesh - - -def import_object_from_numpy_stl(filename, scene): # pragma nocover - """ - Import either an ASCII or BINARY file format of an STL file. - The triangles will be combined into a single compound entity. - - :param filename: Path of the stl file to import. - :type filename: `str` - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :return: Compound object of a collection of triangles formed from an - stl file. - :rtype: class:`vpython.compound` - """ - # filename = meshdata[0] - # scale = meshdata[1] - # origin = meshdata[2] - # origin_pos = get_pose_pos(origin) - # origin_x = get_pose_x_vec(origin) - # origin_y = get_pose_y_vec(origin) - # origin_z = get_pose_z_vec(origin) - - # Load the mesh using NumPy-STL - the_mesh = mesh.Mesh.from_file(filename) - - num_faces = len(the_mesh.vectors) - triangles = [] - - # For every face in the model - for face in range(0, num_faces): - # Get the (3) 3D points - point0 = the_mesh.vectors[face][0] - point1 = the_mesh.vectors[face][1] - point2 = the_mesh.vectors[face][2] - - # Get the normal direction for the face - normal0 = the_mesh.normals[face][0] - normal1 = the_mesh.normals[face][1] - normal2 = the_mesh.normals[face][2] - normal = vec(normal0, normal1, normal2) - - # Create the VPython 3D points - vertex0 = vertex( - pos=vec(point0[0], point0[1], point0[2]), - normal=normal, - color=color.white - ) - vertex1 = vertex( - pos=vec(point1[0], point1[1], point1[2]), - normal=normal, - color=color.white - ) - vertex2 = vertex( - pos=vec(point2[0], point2[1], point2[2]), - normal=normal, - color=color.white - ) - - # Combine them in a list - vertices = [vertex0, vertex1, vertex2] - - # Create a triangle using the points, and add it to the list - triangles.append(triangle(canvas=scene, vs=vertices)) - - # Return a compound of the triangles - visual_mesh = compound(triangles, origin=vec(0, 0, 0), canvas=scene) - # visual_mesh.length *= scale[0] - # visual_mesh.height *= scale[1] - # visual_mesh.width *= scale[2] - - return visual_mesh - - -def set_stl_origin( - stl_obj, current_obj_origin, - required_obj_origin, scene): # pragma nocover - """ - Move the object so the required origin is at (0, 0, 0). Then set the - origin for the generated stl object. Origin can't be changed, so creating - a compound of itself allows setting an origin location - - :param stl_obj: The generated stl object. - :type stl_obj: class:`vpython.compound` - :param current_obj_origin: Current coordinates of the origin of the model - :type current_obj_origin: class:`vpython.vector` - :param required_obj_origin: Required coordinates to place the origin - at (0, 0, 0) - :type required_obj_origin: class:`vpython.vector` - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :return: Compound object of itself, with origin set respective to the joint - :rtype: class:`vpython.compound` - """ - # Move the object to put the origin at 0, 0, 0 - movement = required_obj_origin - current_obj_origin - stl_obj.pos += movement - - # Set invisible to return an overwritten copy - stl_obj.visible = False - - # Return a compound of itself with the origin at (0, 0, 0) - return compound( - [stl_obj], origin=vector(0, 0, 0), vector=x_axis_vector, canvas=scene) diff --git a/roboticstoolbox/backends/VPython/text.py b/roboticstoolbox/backends/VPython/text.py deleted file mode 100644 index aaf6d0a6e..000000000 --- a/roboticstoolbox/backends/VPython/text.py +++ /dev/null @@ -1,347 +0,0 @@ -#!/usr/bin/env python -""" -@author Micah Huth -""" - - -from vpython import color, label, mag, vector -from numpy import sign, arange -from enum import Enum - - -class GridType(Enum): # pragma nocover - XYZ = 1 - XY3D = 2 - XY2D = 3 - - -def get_text_size(scene): # pragma nocover - """ - Determine the text size based on zoom distance - - :param scene: The scene in which the camera to be used resides. - :type scene: `vpython.canvas` - """ - # Distance of camera from focus point to determine text size - distance_from_center = mag(scene.center - scene.camera.pos) - - # Eq generated from data (Using 3rd order polynomial) - # D | size - # 0 | 5 - # 0.5 | 8 - # 1 | 10 - # 2 | 12 - # 3 | 14 - # 5 | 15 - val = 0.1114 * distance_from_center**3 - 1.336 * distance_from_center**2 \ - + 5.8666 * distance_from_center + 5.1711 - - return min(max(val, 10), 15) # Return val between 10 and 15 - - -def draw_label(label_text, label_position, scene): # pragma nocover - """ - Display a label at a given position, with borders and lines - - :param label_text: String of text to be written on the label. - :type label_text: `str` - :param label_position: 3D vector position to draw the label at. - :type label_position: class:`vpython.vector` - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :return: The created label object. - :rtype: class:`vpython.label` - """ - - # Custom settings for the label - label_height = 10 - label_xoffset = 0 - label_yoffset = 50 - label_space = 20 - label_font = 'serif' - label_text_colour = color.black - label_line_color = color.black - - the_label = label( - canvas=scene, - pos=label_position, - text=label_text, - height=label_height, - xoffset=label_xoffset, - yoffset=label_yoffset, - space=label_space, - font=label_font, - color=label_text_colour, - linecolor=label_line_color - ) - - return the_label - - -def draw_text(label_text, label_position, scene): # pragma nocover - """ - Display a label at a given position, without borders or lines. - - :param label_text: String of text to be written on the label. - :type label_text: `str` - :param label_position: 3D vector position to draw the label at. - :type label_position: class:`vpython.vector` - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - :return: The created label object. - :rtype: class:`vpython.label` - """ - label_height = get_text_size(scene) - label_xoffset = 0 - label_yoffset = 0 - label_space = 0 - label_font = 'serif' - label_text_colour = color.black - label_line_color = color.white - label_bg_opacity = 0 - label_linewidth = 0.1 - - the_label = label( - canvas=scene, - pos=label_position, - text=label_text, - height=label_height, - xoffset=label_xoffset, - yoffset=label_yoffset, - space=label_space, - font=label_font, - color=label_text_colour, - linecolor=label_line_color, - opacity=label_bg_opacity, - linewidth=label_linewidth - ) - - return the_label - - -def update_grid_numbers( - focal_point, numbers_list, num_squares, - scale, grid_mode, scene): # pragma nocover - """ - Draw the grid numbers along the xyz axes. - - :param focal_point: The focus point of the camera to draw the grid about - :type focal_point: `list` - :param numbers_list: A reference to a list of the labels that gets updated. - :type numbers_list: `list` - :param num_squares: How many unit squares to draw along the axis. - :type num_squares: `int` - :param scale: The scaled length of 1 square unit - :type scale: `float` - :param grid_mode: Whether the grid is 3D or not - :type grid_mode: `backends.vpython.text.GridType` - :param scene: The scene in which to draw the object - :type scene: class:`vpython.canvas` - """ - - # Initial conditions - padding = 0.25 # Padding to not draw numbers on top of lines. - camera_axes = scene.camera.axis - z_visible = grid_mode == GridType.XYZ - - # Locate center of the axes - x_origin, y_origin, z_origin = focal_point[0], focal_point[1], \ - focal_point[2] - - # CAMERA AXES | DISPLAYED GRID | XZ PLANE | XY PLANE | YZ PLANE - # x,y,z | x,y,z | x,z | x,y | y,z - # -------------+-----------------+----------+----------+---------- - # -,-,- | +,+,+ | +,+ | +,+ | +,+ - # -,-,+ | +,+,- | +,- | +,+ | +,- - # -,+,- | +,-,+ | +,+ | +,- | -,+ - # -,+,+ | +,-,- | +,- | +,- | -,- - # +,-,- | -,+,+ | -,+ | -,+ | +,+ - # +,-,+ | -,+,- | -,- | -,+ | +,- - # +,+,- | -,-,+ | -,+ | -,- | -,+ - # +,+,+ | -,-,- | -,- | -,- | -,- - # min = -num_squares or 0, around the default position - # max = +num_squares or 0, around the default position - # e.g. at the origin, for negative axes: -10 -> 0, positive axes: 0 -> 10 - min_x_coord = round(x_origin + (-(num_squares / 2) + - (sign(camera_axes.x) * -1) - * (num_squares / 2)) * scale, 2) - max_x_coord = round(x_origin + ((num_squares / 2) + - (sign(camera_axes.x) * -1) - * (num_squares / 2)) * scale, 2) - - min_y_coord = round(y_origin + (-(num_squares / 2) + - (sign(camera_axes.y) * -1) - * (num_squares / 2)) * scale, 2) - max_y_coord = round(y_origin + ((num_squares / 2) + - (sign(camera_axes.y) * -1) - * (num_squares / 2)) * scale, 2) - - min_z_coord = round(z_origin + (-(num_squares / 2) + - (sign(camera_axes.z) * -1) - * (num_squares / 2)) * scale, 2) - max_z_coord = round(z_origin + ((num_squares / 2) + - (sign(camera_axes.z) * -1) - * (num_squares / 2)) * scale, 2) - - # Modify to have centre of grid around object - # i.e. XYZ would go from 0->10, XY3D would go from 5->-5 - if grid_mode == GridType.XY3D: - min_x_coord = min_x_coord + sign(camera_axes.x) * (scale * (num_squares / 2)) - max_x_coord = max_x_coord + sign(camera_axes.x) * (scale * (num_squares / 2)) - min_y_coord = min_y_coord + sign(camera_axes.y) * (scale * (num_squares / 2)) - max_y_coord = max_y_coord + sign(camera_axes.y) * (scale * (num_squares / 2)) - min_z_coord = min_z_coord + sign(camera_axes.z) * (scale * (num_squares / 2)) - max_z_coord = max_z_coord + sign(camera_axes.z) * (scale * (num_squares / 2)) - - x_coords = arange(min_x_coord, max_x_coord + scale, scale) - y_coords = arange(min_y_coord, max_y_coord + scale, scale) - z_coords = arange(min_z_coord, max_z_coord + scale, scale) - - # If the grid has given too many objects - if len(x_coords) > num_squares + 1: - x_coords = x_coords[0:num_squares + 1] - if len(y_coords) > num_squares + 1: - y_coords = y_coords[0:num_squares + 1] - if len(z_coords) > num_squares + 1: - z_coords = z_coords[0:num_squares + 1] - - # Compound origins are in the middle of the bounding boxes. - # Thus new pos will be between max and min. - x_middle = x_coords.mean() - y_middle = y_coords.mean() - z_middle = z_coords.mean() - - # If input is empty, append new, otherwise update current - append = len(numbers_list) == 0 - # Dimensions don't change between updates, so indexing shall - # remain the same - index = 0 - offset = scale * (num_squares / 2) - - # X plane - for x_pos in x_coords: - # Draw the corresponding unit number at each x coordinate - txt = "{:.2f}".format(x_pos) - if z_visible: - if (sign(camera_axes.y) * -1) > 0: - pos = vector(x_pos, max_y_coord + padding, z_origin) - else: - pos = vector(x_pos, min_y_coord - padding, z_origin) - else: - if grid_mode == GridType.XY3D: - pos = vector(x_pos, y_origin - padding - offset, z_origin) - else: - pos = vector(x_pos, y_origin - padding, z_origin) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list)-1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 - # Draw the axis label at the centre of the axes numbers - txt = "X" - if grid_mode == GridType.XY3D: - x = x_middle - y = y_origin - offset - scale * 2 - pos = vector(x, y, z_origin) - else: - if (sign(camera_axes.y) * -1) > 0: - x = x_middle - y = max_y_coord + scale * 2 - pos = vector(x, y, z_origin) - else: - x = x_middle - y = min_y_coord - scale * 2 - pos = vector(x, y, z_origin) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list) - 1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 - - # Y plane - for y_pos in y_coords: - # Draw the corresponding unit number at each x coordinate - txt = "{:.2f}".format(y_pos) - if z_visible: - if (sign(camera_axes.x) * -1) > 0: - pos = vector(max_x_coord + padding, y_pos, z_origin) - else: - pos = vector(min_x_coord - padding, y_pos, z_origin) - else: - if grid_mode == GridType.XY3D: - pos = vector(x_origin - padding - offset, y_pos, z_origin) - else: - pos = vector(x_origin - padding, y_pos, z_origin) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list) - 1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 - # Draw the axis label at the centre of the axes numbers - txt = "Y" - if grid_mode == GridType.XY3D: - x = x_origin - offset - scale * 2 - y = y_middle - pos = vector(x, y, z_origin) - else: - if (sign(camera_axes.x) * -1) > 0: - x = max_x_coord + scale * 2 - y = y_middle - pos = vector(x, y, z_origin) - else: - x = min_x_coord - scale * 2 - y = y_middle - pos = vector(x, y, z_origin) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list) - 1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 - - if not z_visible: - return - - # Z plane - for z_pos in z_coords: - # Draw the corresponding unit number at each x coordinate - txt = "{:.2f}".format(z_pos) - if (sign(camera_axes.x) * -1) > 0: - pos = vector(max_x_coord + padding, y_origin, z_pos) - else: - pos = vector(min_x_coord - padding, y_origin, z_pos) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list) - 1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 - # Draw the axis label at either the positive or negative side away from - # center - # If sign = -1, draw off max side, if sign = 0 or 1, draw off negative side - txt = "Z" - if (sign(camera_axes.x) * -1) > 0: - pos = vector(max_x_coord + scale * 2, y_origin, z_middle) - else: - pos = vector(min_x_coord - scale * 2, y_origin, z_middle) - if append: - numbers_list.append(draw_text(txt, pos, scene)) - numbers_list[len(numbers_list) - 1].height = get_text_size(scene) - else: - numbers_list[index].text = txt - numbers_list[index].pos = pos - numbers_list[index].height = get_text_size(scene) - index += 1 diff --git a/roboticstoolbox/backends/__init__.py b/roboticstoolbox/backends/__init__.py index 2077bc29e..e69de29bb 100644 --- a/roboticstoolbox/backends/__init__.py +++ b/roboticstoolbox/backends/__init__.py @@ -1,10 +0,0 @@ -# from roboticstoolbox.backends.PyPlot import * # noqa -# from roboticstoolbox.backends.Swift import * # noqa -# from roboticstoolbox.backends.ROS import * # noqa -# print("about to import VP") -# try: -# from roboticstoolbox.backends.VPython import * # noqa -# except ImportError as e: # pragma nocover -# print("failed to import VPython", e) -# pass -# print("VP imported") \ No newline at end of file diff --git a/roboticstoolbox/bin/rtbtool.py b/roboticstoolbox/bin/rtbtool.py index 1ac7579a2..27c26f07a 100755 --- a/roboticstoolbox/bin/rtbtool.py +++ b/roboticstoolbox/bin/rtbtool.py @@ -19,20 +19,12 @@ from traitlets.config import Config import IPython import argparse -from math import pi # lgtm [py/unused-import] -import numpy as np -import scipy as sp -import matplotlib.pyplot as plt # lgtm [py/unused-import] -from roboticstoolbox import * # lgtm [py/unused-import] -from spatialmath import * # lgtm [py/polluting-import] -from spatialgeometry import * # lgtm [py/polluting-import] -import spatialmath.base as smb -from spatialmath.base import sym -import matplotlib as mpl from pathlib import Path import sys from importlib.metadata import version + + try: from colored import fg, bg, attr @@ -42,16 +34,31 @@ # print('colored not found') _colored = False -def main(): - # setup defaults - np.set_printoptions( - linewidth=120, - formatter={"float": lambda x: f"{0:8.4g}" if abs(x) < 1e-10 else f"{x:8.4g}"}, - ) +# imports for use by IPython and user +import math +from math import pi # lgtm [py/unused-import] +import numpy as np +from scipy import linalg, optimize +import matplotlib.pyplot as plt # lgtm [py/unused-import] + +from spatialmath import * # lgtm [py/polluting-import] +from spatialmath.base import * +import spatialmath.base as smb +from spatialmath.base import sym +from spatialgeometry import * # lgtm [py/polluting-import] + +from roboticstoolbox import * # lgtm [py/unused-import] +# load some robot models +puma = models.DH.Puma560() +panda = models.DH.Panda() + +def parse_arguments(): parser = argparse.ArgumentParser("Robotics Toolbox shell") parser.add_argument("script", default=None, nargs="?", help="specify script to run") - parser.add_argument("--backend", "-b", default=None, help="specify Matplotlib backend") + parser.add_argument( + "--backend", "-B", default=None, help="specify graphics backend" + ) parser.add_argument( "--color", "-c", @@ -68,6 +75,14 @@ def main(): default=None, help="execution result prefix, include {} for execution count number", ) + parser.add_argument( + "-b", + "--no-banner", + dest="banner", + default=True, + action="store_false", + help="suppress startup banner", + ) parser.add_argument( "--showassign", "-a", @@ -79,10 +94,6 @@ def main(): "--book", default=False, action="store_true", help="use defaults as per RVC book" ) - parser.add_argument( - "--vision", default=False, action="store_true", - help="import vision toolbox (MVTB)" - ) parser.add_argument( "--ansi", default=False, @@ -103,13 +114,79 @@ def main(): action="store_true", help="use Swift as default backend", ) - args = parser.parse_args() + args, rest = parser.parse_known_args() + + # remove the arguments we've just parsed from sys.argv so that IPython can have a + # go at them later + sys.argv = [sys.argv[0]] + rest # TODO more options # color scheme, light/dark # silent startup - sys.argv = [sys.argv[0]] + if args.script is not None: + args.banner = False + + return args + +def make_banner(): + # banner template + # https://patorjk.com/software/taag/#p=display&f=Cybermedium&t=Robotics%20Toolbox%0A + + banner = f"""\ + ____ ____ ___ ____ ___ _ ____ ____ ___ ____ ____ _ ___ ____ _ _ + |__/ | | |__] | | | | | [__ | | | | | | |__] | | \/ + | \ |__| |__] |__| | | |___ ___] | |__| |__| |___ |__] |__| _/\_ + + for Python""" + + versions = [] + versions.append(f"RTB=={version('roboticstoolbox-python')}") + versions.append(f"SMTB=={version('spatialmath-python')}") + versions.append(f"SG=={version('spatialmath-python')}") + versions.append(f"NumPy=={version('numpy')}") + versions.append(f"SciPy=={version('scipy')}") + versions.append(f"Matplotlib=={version('matplotlib')}") + + # create banner + banner += " (" + ", ".join(versions) + ")" + banner += r""" + + import math + import numpy as np + from scipy import linalg, optimize + import matplotlib.pyplot as plt + from spatialmath import * + from spatialmath.base import * + from spatialmath.base import sym + from roboticstoolbox import *" + + # useful variables + from math import pi + puma = models.DH.Puma560() + panda = models.DH.Panda() + + func/object? - show brief help + help(func/object) - show detailed help + func/object?? - show source code + + """ + + print(fg("yellow") + banner + attr(0)) + +def startup(): + plt.ion() + +def main(): + + args = parse_arguments() + + + # setup defaults + np.set_printoptions( + linewidth=120, + formatter={"float": lambda x: f"{0:8.4g}" if abs(x) < 1e-10 else f"{x:8.4g}"}, + ) if args.book: # set book options @@ -131,48 +208,19 @@ def main(): print(f"Using matplotlb backend {args.backend}") mpl.use(args.backend) - # load some robot models - puma = models.DH.Puma560() - panda = models.DH.Panda() - # build the banner, import * packages and their versions - versions = f"(RTB=={version('roboticstoolbox-python')}, SMTB=={version('spatialmath-python')}" - imports = ["roboticstoolbox", "spatialmath"] - if args.vision: - versions += f", MVTB=={version('machinevision-toolbox-python')}" - imports.append("machinevisiontoolbox") - versions += ")" - imports = "\n".join([f" from {x} import *" for x in imports]) - # banner template - # https://patorjk.com/software/taag/#p=display&f=Cybermedium&t=Robotics%20Toolbox%0A - banner = f"""\ - ____ ____ ___ ____ ___ _ ____ ____ ___ ____ ____ _ ___ ____ _ _ - |__/ | | |__] | | | | | [__ | | | | | | |__] | | \/ - | \ |__| |__] |__| | | |___ ___] | |__| |__| |___ |__] |__| _/\_ - - for Python {versions} - -{imports} - import numpy as np - import scipy as sp - - func/object? - show brief help - help(func/object) - show detailed help - func/object?? - show source code + if args.banner: + banner = make_banner() + print(banner) - """ - - print(fg("yellow") + banner + attr(0)) - - if args.showassign: + if args.showassign and args.banner: print( fg("red") - + """Results of assignments will be displayed, use trailing ; to suppress - - """, - attr(0), + + "Results of assignments will be displayed, use trailing ; to suppress" + + attr(0) + + "\n" ) # drop into IPython @@ -202,7 +250,7 @@ def out_prompt_tokens(self, cli=None): c.InteractiveShell.prompts_class = MyPrompt if args.showassign: c.InteractiveShell.ast_node_interactivity = "last_expr_or_assign" - + c.TerminalIPythonApp.force_interact = False # set precision, same as %precision c.PlainTextFormatter.float_precision = "%.3f" @@ -215,18 +263,16 @@ def out_prompt_tokens(self, cli=None): code = path.open("r").readlines() if code is None: code = [ + "startup()", "%precision %.3g", - "plt.ion()", ] - else: code.append("plt.ion()") - if args.vision: - code.append("from machinevisiontoolbox import *") + + c.InteractiveShellApp.exec_lines = code IPython.start_ipython(config=c, user_ns=globals()) - if __name__ == "__main__": main() diff --git a/roboticstoolbox/blocks/__init__.py b/roboticstoolbox/blocks/__init__.py index 1ca1e18d8..121429907 100644 --- a/roboticstoolbox/blocks/__init__.py +++ b/roboticstoolbox/blocks/__init__.py @@ -1,5 +1,6 @@ from .arm import * from .mobile import * from .uav import * +from .spatial import * -url = "https://petercorke.github.io/bdsim/" + __package__ \ No newline at end of file +url = "https://petercorke.github.io/robotics-toolbox-python/" + __package__ diff --git a/roboticstoolbox/blocks/arm.py b/roboticstoolbox/blocks/arm.py index fcb9e1d3c..e89238eb7 100644 --- a/roboticstoolbox/blocks/arm.py +++ b/roboticstoolbox/blocks/arm.py @@ -3,7 +3,8 @@ # import matplotlib.pyplot as plt import time -from spatialmath import base, SE3 +from spatialmath import SE3 +import spatialmath.base as smb from bdsim.components import TransferBlock, FunctionBlock, SourceBlock from bdsim.graphics import GraphicsBlock @@ -19,27 +20,44 @@ """ # The constructor of each class ``MyClass`` with a ``@block`` decorator becomes a method ``MYCLASS()`` of the BlockDiagram instance. + # ------------------------------------------------------------------------ # class FKine(FunctionBlock): - """ + r""" :blockname:`FKINE` - .. table:: - :align: left + Robot arm forward kinematics. + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | SE3 | | - +------------+---------+---------+ + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{q}` + * - Output + - 0 + - SE3 + - :math:`\mathbf{T}` + + Compute the end-effector pose as an SE(3) object as a function of the input joint + configuration. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.fkine` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('T',) + inlabels = ("q",) + outlabels = ("T",) def __init__(self, robot=None, args={}, **blockargs): """ @@ -51,22 +69,12 @@ def __init__(self, robot=None, args={}, **blockargs): :type args: dict, optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a FORWARD_KINEMATICS block - :rtype: Foward_Kinematics instance - - Robot arm forward kinematic model. - - **Block ports** - - :input q: Joint configuration vector as an ndarray. - - :output T: End-effector pose as an SE(3) object """ if robot is None: - raise ValueError('robot is not defined') + raise ValueError("robot is not defined") super().__init__(**blockargs) - self.type = "forward-kinematics" + # self.type = "forward-kinematics" self.robot = robot self.args = args @@ -74,34 +82,63 @@ def __init__(self, robot=None, args={}, **blockargs): self.inport_names(("q",)) self.outport_names(("T",)) - def output(self, t=None): - q = self.inputs[0] - return [self.robot.fkine(self.inputs[0], **self.args)] + def output(self, t, inports, x): + q = inports[0] + return [self.robot.fkine(q, **self.args)] + + +# ------------------------------------------------------------------------ # class IKine(FunctionBlock): - """ + r""" :blockname:`IKINE` - .. table:: - :align: left + Robot arm inverse kinematics. + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | SE3 | ndarray | | - +------------+---------+---------+ + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - SE3 + - :math:`\mathbf{T}` + * - Output + - 0 + - ndarray(N) + - :math:`\mathit{q}` + + Compute joint configuration required to achieve end-effector pose input as + an SE(3) object. + + :note: The solution may not exist and is not unique. The solution will depend + on the initial joint configuration ``q0``. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.ik_LM` """ nin = 1 nout = 1 - inlabels = ('T',) - outlabels = ('q',) + inlabels = ("T",) + outlabels = ("q",) def __init__( - self, robot=None, q0=None, useprevious=True, ik=None, **blockargs + self, + robot=None, + q0=None, + useprevious=True, + ik=None, + args={}, + seed=None, + **blockargs, ): """ :param robot: Robot model, defaults to None @@ -110,36 +147,30 @@ def __init__( :type q0: array_like(n), optional :param useprevious: Use previous IK solution as q0, defaults to True :type useprevious: bool, optional - :param ik: Specify an IK function, defaults to 'ikine_LM' - :type ik: callable f(T) + :param ik: Specify an IK function, defaults to ``Robot.ikine_LM`` + :type ik: callable + :param args: Options passed to IK function + :type args: dict + :param seed: random seed for solution + :type seed: int :param blockargs: |BlockOptions| :type blockargs: dict - :return: an INVERSE_KINEMATICS block - :rtype: Inverse_Kinematics instance - - Robot arm inverse kinematic model. - - The block has one input port: - - 1. End-effector pose as an SE(3) object - - and one output port: - - 1. Joint configuration vector as an ndarray. - - """ if robot is None: - raise ValueError('robot is not defined') + raise ValueError("robot is not defined") super().__init__(**blockargs) - self.type = "inverse-kinematics" + # self.type = "inverse-kinematics" self.robot = robot self.q0 = q0 self.qprev = q0 self.useprevious = useprevious - self.ik = None + if ik is None: + ik = robot.ikine_LM + self.ik = ik + self.args = args + self.seed = 0 self.inport_names(("T",)) self.outport_names(("q",)) @@ -149,19 +180,16 @@ def start(self): if self.useprevious: self.qprev = self.q0 - def output(self, t=None): + def output(self, t, inports, x): if self.useprevious: q0 = self.qprev else: q0 = self.q0 - if self.ik is None: - sol = self.robot.ikine_LM(self.inputs[0], q0=q0) - else: - sol = self.ik(self.inputs[0]) + sol = self.ik(inports[0], q0=q0, seed=self.seed, **self.args) if not sol.success: - raise RuntimeError("inverse kinematic failure for pose", self.inputs[0]) + raise RuntimeError("inverse kinematic failure for pose", inports[0]) if self.useprevious: self.qprev = sol.q @@ -171,63 +199,81 @@ def output(self, t=None): # ------------------------------------------------------------------------ # + class Jacobian(FunctionBlock): - """ + r""" :blockname:`JACOBIAN` - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | ndarray | | - +------------+---------+---------+ + Robot arm Jacobian matrix. + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{q}` + * - Output + - 0 + - ndarray(N,N) + - :math:`\mathbf{J}` + + Compute the Jacobian matrix as a function of the input joint configuration. The + Jacobian can be computed in the world or end-effector frame, for spatial or + analytical velocity, and its inverse, damped inverse or transpose can be returned. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.jacob0` + :meth:`~roboticstoolbox.robot.Robot.Robot.jacobe` + :meth:`~roboticstoolbox.robot.Robot.Robot.jacob0_analytical` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('J',) + inlabels = ("q",) + outlabels = ("J",) def __init__( self, robot, frame="0", + representation=None, inverse=False, pinv=False, + damping=None, transpose=False, - **blockargs + **blockargs, ): """ :param robot: Robot model :type robot: Robot subclass - :param frame: Frame to compute Jacobian for, one of: '0' [default], 'e' + :param frame: Frame to compute Jacobian for, one of: "0" [default], "e" :type frame: str, optional + :param representation: representation for analytical Jacobian + :type representation: str, optional :param inverse: output inverse of Jacobian, defaults to False :type inverse: bool, optional :param pinv: output pseudo-inverse of Jacobian, defaults to False :type pinv: bool, optional + :param damping: damping term for inverse, defaults to None + :type damping: float or array_like(N) :param transpose: output transpose of Jacobian, defaults to False :type transpose: bool, optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a JACOBIAN block - :rtype: Jacobian instance - Robot arm Jacobian. + If an inverse is requested and ``damping`` is not None it is added to the + diagonal of the Jacobian prior to the inversion. If a scalar is provided it is + added to each element of the diagonal, otherwise an N-vector is assumed. - The block has one input port: - - 1. Joint configuration vector as an ndarray. - - and one output port: - - 1. Jacobian matrix as an ndarray(6,n) - - .. notes:: + .. note:: - Only one of ``inverse`` or ``pinv`` can be True - ``inverse`` or ``pinv`` can be used in conjunction with ``transpose`` - ``inverse`` requires that the Jacobian is square @@ -235,16 +281,24 @@ def __init__( error will occur. """ if robot is None: - raise ValueError('robot is not defined') - + raise ValueError("robot is not defined") + super().__init__(**blockargs) self.robot = robot if frame in (0, "0"): - self.jfunc = robot.jacob0 + if representation is None: + self.jfunc = robot.jacob0 + else: + self.jfunc = lambda q: robot.jacob0_analytical( + q, representation=representation + ) elif frame == "e": - self.jfunc = robot.jacobe + if representation is None: + self.jfunc = robot.jacobe + else: + raise ValueError("cannot compute analytical Jacobian in EE frame") else: raise ValueError("unknown frame") @@ -254,373 +308,733 @@ def __init__( raise ValueError("can only set one of inverse and pinv") self.inverse = inverse self.pinv = pinv + self.damping = damping self.transpose = transpose + self.representation = representation self.inport_names(("q",)) self.outport_names(("J",)) - def output(self, t=None): - J = self.jfunc(self.inputs[0]) + def output(self, t, inports, x): + q = inports[0] + + J = self.jfunc(q) + + # add damping term if given + if (self.inverse or self.pinv) and self.damping is not None: + D = np.zeros(J.shape) + np.fill_diagonal(D, self.damping) + J = J + D + + # optionally invert the Jacobian if self.inverse: J = np.linalg.inv(J) if self.pinv: J = np.linalg.pinv(J) + + # optionally transpose the Jacobian if self.transpose: J = J.T return [J] -class Tr2Delta(FunctionBlock): - """ - :blockname:`TR2DELTA` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 2 | 1 | 0 | - +------------+------------+---------+ - | SE3, SE3 | ndarray(6) | | - +------------+------------+---------+ - """ - - nin = 2 - nout = 1 - inlabels = ('T1', 'T2') - outlabels = ('Δ',) - - def __init__(self, **blockargs): - """ - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: a TR2DELTA block - :rtype: Tr2Delta instance - - Difference between T1 and T2 as a 6-vector - - The block has two input port: - - 1. T1 as an SE3. - 2. T2 as an SE3. +# ------------------------------------------------------------------------ # - and one output port: - 1. delta as an ndarray(6,n) +class ArmPlot(GraphicsBlock): + r""" + :blockname:`ARMPLOT` - :seealso: :func:`spatialmath.base.tr2delta` - """ - super().__init__(**blockargs) + Plot robot arm. - self.inport_names(("T1", "T2")) - self.outport_names(("$\delta$",)) + :inputs: 1 [ndarray(N)] + :outputs: 0 + :states: 0 - def output(self, t=None): - return [base.tr2delta(self.inputs[0].A, self.inputs[1].A)] + :inputs: 1 + :outputs: 0 + :states: 0 + .. list-table:: + :header-rows: 1 -# ------------------------------------------------------------------------ # + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{q}`, joint configuration + * - Output + Create a robot animation using the robot's ``plot`` method. -class Delta2Tr(FunctionBlock): - """ - :blockname:`DELTA2TR` - - .. table:: - :align: left - - +------------+----------+---------+ - | inputs | outputs | states | - +------------+----------+---------+ - | 1 | 1 | 0 | - +------------+----------+---------+ - | ndarray(6) | SE3 | | - +------------+----------+---------+ + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.plot` """ nin = 1 - nout = 1 - outlabels = ('T',) - inlabels = ('Δ',) + nout = 0 + inlabels = ("q",) - def __init__(self, **blockargs): + def __init__(self, robot=None, q0=None, backend=None, **blockargs): """ + :param robot: Robot model + :type robot: Robot subclass + :param q0: initial joint angles, defaults to None + :type q0: ndarray(N) + :param backend: RTB backend name, defaults to 'pyplot' + :type backend: str, optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a DELTA2TR block - :rtype: Delta2Tr instance - - Delta to SE(3) - - The block has one input port: + """ + if robot is None: + raise ValueError("robot is not defined") - 1. delta as an ndarray(6,n) + super().__init__(**blockargs) + self.inport_names(("q",)) - and one output port: + if q0 is None: + q0 = np.zeros((robot.n,)) + self.robot = robot + self.backend = backend + self.q0 = q0 + self.env = None - 1. T as an SE3 + def start(self, simstate): + # create the plot + # super().reset() + # if state.options.graphics: + super().start(simstate) + self.fig = self.create_figure(simstate) + self.env = self.robot.plot( + self.q0, backend=self.backend, fig=self.fig, block=False + ) - :seealso: :func:`spatialmath.base.delta2tr` - """ - super().__init__(**blockargs) + def step(self, t, inports): + # update the robot plot + self.robot.q = inports[0] + self.env.step() - self.inport_names(("$\delta$",)) - self.outport_names(("T",)) + super().step(t, inports) - def output(self, t=None): - return [SE3.Delta(self.inputs[0])] +# ======================================================================== # -# ------------------------------------------------------------------------ # -class Point2Tr(FunctionBlock): +class JTraj(SourceBlock): """ - :blockname:`POINT2TR` - - .. table:: - :align: left - - +------------+----------+---------+ - | inputs | outputs | states | - +------------+----------+---------+ - | 1 | 1 | 0 | - +------------+----------+---------+ - | ndarray(3) | SE3 | | - +------------+----------+---------+ + :blockname:`JTRAJ` + + Joint-space trajectory + + :inputs: 0 + :outputs: 3 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Output + - 0 + - ndarray + - :math:`q(s)` + * - Output + - 1 + - ndarray + - :math:`\dot{q}(s)` + * - Output + - 2 + - ndarray + - :math:`\ddot{q}(s)` + + Outputs a joint space trajectory where the joint coordinates vary from ``q0`` to + ``qf`` over the course of the simulation. A quintic (5th order) polynomial is used + with default zero boundary conditions for velocity and acceleration. + + :seealso: :func:`~roboticstoolbox.tools.trajectory.ctraj` + :func:`~roboticstoolbox.tools.trajectory.xplot` + :func:`~roboticstoolbox.tools.trajectory.jtraj` """ - nin = 1 - nout = 1 + nin = 0 + nout = 3 + outlabels = ("q", "qd", "qdd") - def __init__(self, T, **blockargs): + def __init__(self, q0, qf, qd0=None, qdf=None, T=None, **blockargs): """ - :param T: the transform - :type T: SE3 + + :param q0: initial joint coordinate + :type q0: array_like(n) + :param qf: final joint coordinate + :type qf: array_like(n) + :param T: time vector or number of steps, defaults to None + :type T: array_like or int, optional + :param qd0: initial velocity, defaults to None + :type qd0: array_like(n), optional + :param qdf: final velocity, defaults to None + :type qdf: array_like(n), optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a POINT2TR block - :rtype: Point2Tr instance + """ + super().__init__(**blockargs) + self.outport_names( + ( + "q", + "qd", + "qdd", + ) + ) + + q0 = smb.getvector(q0) + qf = smb.getvector(qf) + + if not len(q0) == len(qf): + raise ValueError("q0 and q1 must be same size") + + if qd0 is None: + qd0 = np.zeros(q0.shape) + else: + qd0 = getvector(qd0) + if not len(qd0) == len(q0): + raise ValueError("qd0 has wrong size") + if qdf is None: + qdf = np.zeros(q0.shape) + else: + qd1 = getvector(qdf) + if not len(qd1) == len(q0): + raise ValueError("qd1 has wrong size") - The block has one input port: + self.q0 = q0 + self.qf = qf + self.qd0 = qd0 + self.qdf = qf - 1. a 3D point as an ndarray(3) + # call start now, so that output works when called by compile + # set T to 1 just for now + if T is None: + self.T = 1 + self.T = T - and one output port: + # valid value, to allow compile to call output() before start() + self.start(None) - 1. T as an SE3 with its position part replaced by the input + def start(self, simstate): + if self.T is None: + # use simulation tmax + self.T = simstate.T - :seealso: :func:`spatialmath.base.delta2tr` - """ - super().__init__(**blockargs) + tscal = self.T + self.tscal = tscal - self.inport_names(("t",)) - self.outport_names(("T",)) - self.pose = T + q0 = self.q0 + qf = self.qf + qd0 = self.qd0 + qdf = self.qdf - def output(self, t=None): - T = SE3.SO3(self.pose.R, t=self.inputs[0]) - return [T] + # compute the polynomial coefficients + A = 6 * (qf - q0) - 3 * (qdf + qd0) * tscal + B = -15 * (qf - q0) + (8 * qd0 + 7 * qdf) * tscal + C = 10 * (qf - q0) - (6 * qd0 + 4 * qdf) * tscal + E = qd0 * tscal + F = q0 + self.coeffs = np.array([A, B, C, np.zeros(A.shape), E, F]) + self.dcoeffs = np.array( + [np.zeros(A.shape), 5 * A, 4 * B, 3 * C, np.zeros(A.shape), E] + ) + self.ddcoeffs = np.array( + [ + np.zeros(A.shape), + np.zeros(A.shape), + 20 * A, + 12 * B, + 6 * C, + np.zeros(A.shape), + ] + ) -# ------------------------------------------------------------------------ # + def output(self, t, inports, x): + tscal = self.tscal + ts = t / tscal + tt = np.array([ts**5, ts**4, ts**3, ts**2, ts, 1]).T + qt = tt @ self.coeffs -class TR2T(FunctionBlock): - """ - :blockname:`TR2T` - - .. table:: - :align: left - - +------------+----------+---------+ - | inputs | outputs | states | - +------------+----------+---------+ - | 1 | 3 | 0 | - +------------+----------+---------+ - | SE3 | float | | - +------------+----------+---------+ - """ + # compute velocity + qdt = tt @ self.dcoeffs / tscal - nin = 1 - nout = 3 - inlabels = ('T',) - outlabels = ('x', 'y', 'z') + # compute acceleration + qddt = tt @ self.ddcoeffs / tscal**2 - def __init__(self, **blockargs): - """ - :param T: the transform - :type T: SE3 - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: a POINT2TR block - :rtype: Point2Tr instance + return [qt, qdt, qddt] - The block has one input port: - 1. a 3D point as an ndarray(3) +# ------------------------------------------------------------------------ # - and one output port: - 1. T as an SE3 with its position part replaced by the input +class CTraj(SourceBlock): + r""" + :blockname:`CTRAJ` - :seealso: :func:`spatialmath.base.delta2tr` - """ - super().__init__(**blockargs) + Task space trajectory - self.inport_names(("T",)) - self.outport_names(("x", "y", "z")) + :inputs: 0 + :outputs: 1 + :states: 0 - def output(self, t=None): - t = self.inputs[0].t - return list(t) + .. list-table:: + :header-rows: 1 + * - Port type + - Port number + - Types + - Description + * - Output + - 0 + - SE3 + - :math:`\mathbf{T}(t)` -# ------------------------------------------------------------------------ # + The block outputs a pose that varies smoothly from ``T1`` to ``T2`` over + the course of ``T`` seconds. -class FDyn(TransferBlock): - """ - :blockname:`FDYN` + If ``T`` is not given it defaults to the simulation time. + + If ``trapezoidal`` is True then a trapezoidal motion profile is used along the path + to provide initial acceleration and final deceleration. Otherwise, + motion is at constant velocity. - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 3 | 0 | - +------------+---------+---------+ - | ndarray | ndarray,| | - | | ndarray,| | - | | ndarray | | - +------------+---------+---------+ + :seealso: :meth:`~spatialmath.pose3d.SE3.interp` + :func:`~roboticstoolbox.tools.trajectory.ctraj` + :func:`~roboticstoolbox.tools.trajectory.xplot` + :func:`~roboticstoolbox.tools.trajectory.jtraj` """ - nin = 1 - nout = 3 - outlabels = ('q', 'qd', 'qdd') - inlabels = ('τ') + nin = 0 + nout = 1 + outlabels = ("T",) - def __init__(self, robot, q0=None, **blockargs): + def __init__(self, T1, T2, T, trapezoidal=True, **blockargs): """ - :param robot: Robot model - :type robot: Robot subclass - :param q0: Initial joint configuration - :type q0: array_like(n) + :param T1: initial pose + :type T1: SE3 + :param T2: final pose + :type T2: SE3 + :param T: motion time + :type T: float + :param trapezoidal: Use LSPB motion profile along the path + :type trapezoidal: bool :param blockargs: |BlockOptions| :type blockargs: dict - :return: a FORWARD_DYNAMICS block - :rtype: Foward_Dynamics instance + """ - Robot arm forward dynamics model. + # TODO + # flag to rotate the frame rather than just translate it + super().__init__(**blockargs) - The block has one input port: + self.T1 = T1 + self.T2 = T2 + self.T = T + self.trapezoidal = trapezoidal - 1. Joint force/torque as an ndarray. + def start(self, simstate): + if self.T is None: + self.T = simstate.T + if self.trapezoidal: + self.trapezoidalfunc = trapezoidal_func(0.0, 1.0, self.T) - and three output ports: + def output(self, t, inports, x): + if self.trapezoidal: + s = self.trapezoidalfunc(t) + else: + s = np.min(t / self.T, 1.0) - 1. joint configuration - 2. joint velocity - 3. joint acceleration + return [self.T1.interp(self.T2, s)] - """ - if robot is None: - raise ValueError('robot is not defined') - - super().__init__(**blockargs) - self.type = "forward-dynamics" +# ------------------------------------------------------------------------ # - self.robot = robot - self.nstates = robot.n * 2 - # state vector is [q qd] +class CirclePath(SourceBlock): + """ + :blockname:`CIRCLEPATH` - self.inport_names(("$\tau$",)) - self.outport_names(("q", "qd", "qdd")) + Circular motion. - if q0 is None: - q0 = np.zeros((robot.n,)) - else: - q0 = base.getvector(q0, robot.n) - self._x0 = np.r_[q0, np.zeros((robot.n,))] - self._qdd = None + :inputs: 0 or 1 + :outputs: 1 + :states: 0 - def output(self, t=None): - n = self.robot.n - q = self._x[:n] - qd = self._x[n:] - qdd = self._qdd # from last deriv - return [q, qd, qdd] + .. list-table:: + :header-rows: 1 - def deriv(self): - # return [qd qdd] - Q = self.inputs[0] - n = self.robot.n - assert len(Q) == n, "torque vector wrong size" + * - Port type + - Port number + - Types + - Description + * - Output + - 0 + - ndarray(3) or SE3 + - :math:`\mathit{p}(t)` or :math:`\mathbf{T}(t)` - q = self._x[:n] - qd = self._x[n:] - qdd = self.robot.accel(q, qd, Q) - self._qdd = qdd - return np.r_[qd, qdd] + The block outputs the coordinates of a point moving in a circle of + radius ``r`` centred at ``centre`` and parallel to the xy-plane. + By default the output is a 3-vector :math:`(x, y, z)` but if + ``pose`` is an ``SE3`` instance the output is a copy of that pose with + its translation set to the coordinate of the moving point. This is the + motion of a frame with fixed orientation following a circular path. + """ -class IDyn(FunctionBlock): + nin = 0 + nout = 1 + + def __init__( + self, + radius=1, + centre=(0, 0, 0), + pose=None, + frequency=1, + unit="rps", + phase=0, + **blockargs, + ): + """ + :param radius: radius of circle, defaults to 1 + :type radius: float + :param centre: center of circle, defaults to [0,0,0] + :type centre: array_like(3) + :param pose: SE3 pose of output, defaults to None + :type pose: SE3 + :param frequency: rotational frequency, defaults to 1 + :type frequency: float + :param unit: unit for frequency, one of: 'rps' [default], 'rad' + :type unit: str + :param phase: phase + :type phase: float + :param blockargs: |BlockOptions| + :type blockargs: dict + """ + + # TODO + # flag to rotate the frame rather than just translate it + super().__init__(**blockargs) + + if unit == "rps": + omega = frequency * 2 * pi + phase = phase * 2 * pi + elif unit == "rad": + omega = frequency + + # Redundant assignment, commented for LGTM + # phase = phase + else: + raise ValueError("bad units: rps or rad") + + self.radius = radius + assert len(centre) == 3, "centre must be a 3 vector" + self.centre = centre + self.pose = pose + self.omega = omega + self.phase = phase + + self.outport_names(("y",)) + + def output(self, t, inports, x): + theta = t * self.omega + self.phase + x = self.radius * cos(theta) + self.centre[0] + y = self.radius * sin(theta) + self.centre[1] + p = (x, y, self.centre[2]) + + if self.pose is not None: + pp = SE3.Rt(self.pose.R, p) + p = pp + + return [p] + + +class Trapezoidal(SourceBlock): + r""" + :blockname:`Trapezoidal` + + Trapezoidal scalar trajectory + + :inputs: 0 + :outputs: 3 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Output + - 0 + - float + - :math:`q(t)` + * - Output + - 1 + - float + - :math:`\dot{q}(t)` + * - Output + - 2 + - float + - :math:`\ddot{q}(t)` + + + Scalar trapezoidal trajectory that varies from ``q0`` to ``qf`` over the + simulation period. + + :seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj` + """ + + nin = 0 + nout = 3 + outlabels = ("q", "qd", "qdd") + + # TODO: change name to Trapezoidal, check if used anywhere + + def __init__(self, q0, qf, V=None, T=None, **blockargs): + """ + Compute a joint-space trajectory + + :param q0: initial joint coordinate + :type q0: float + :param qf: final joint coordinate + :type qf: float + :param T: maximum time, defaults to None + :type T: float, optional + :param blockargs: |BlockOptions| + :type blockargs: dict + + If ``T`` is given the value ``qf`` is reached at this time. This can be + less or greater than the simulation time. + """ + super().__init__(nout=3, **blockargs) + self.T = T + self.q0 = q0 + self.qf = qf + + def start(self, simstate): + if self.T is None: + self.T = simstate.T + self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, self.T) + + def output(self, t, inports, x): + return list(self.trapezoidalfunc(t)) + + +# ------------------------------------------------------------------------ # + + +class Traj(FunctionBlock): + """ + :blockname:`TRAJ` + + Vector trajectory + + :inputs: 0 or 1 + :outputs: 3 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`s \in [0, 1]` distance along trajectory. + * - Output + - 0 + - ndarray + - :math:`y(s)` + * - Output + - 1 + - ndarray + - :math:`\dot{y}(s)` + * - Output + - 2 + - ndarray + - :math:`\ddot{y}(s)` + + Generates a vector trajectory using a trapezoidal or quintic + polynomial profile that varies from ``y0`` to ``yf`` + + The distance along the trajectory is either: + + - a linear function from 0 to ``T`` or maximum simulation time + - the value [0, 1] given on inport port. + + :seealso: :func:`spatialmath.base.mtraj` """ + + nin = -1 + nout = 3 + outlabels = ("q",) + + # TODO: this needs work, need better description of what this does for + # time-based case + + def __init__(self, y0=0, yf=1, T=None, time=False, traj="trapezoidal", **blockargs): + """ + :param y0: initial value, defaults to 0 + :type y0: array_like(m), optional + :param yf: final value, defaults to 1 + :type yf: array_like(m), optional + :param T: maximum time, defaults to None + :type T: float, optional + :param time: x is simulation time, defaults to False + :type time: bool, optional + :param traj: trajectory type, one of: 'trapezoidal' [default], 'quintic' + :type traj: str, optional + :param blockargs: |BlockOptions| + :type blockargs: dict + """ + self.time = time + if time: + # function of time in simulation + nin = 0 + blockclass = "source" + else: + # function of input port + nin = 1 + blockclass = "function" + + super().__init__(nin=nin, blockclass=blockclass, **blockargs) + + y0 = smb.getvector(y0) + yf = smb.getvector(yf) + assert len(y0) == len(yf), "y0 and yf must have same length" + + self.y0 = y0 + self.yf = yf + self.time = time + self.T = T + self.traj = traj + + self.outport_names(("y", "yd", "ydd")) + + def start(self, simstate): + # if self.time: + # assert self.x[0] <= 0, "interpolation not defined for t=0" + # assert self.x[-1] >= simstate.T, "interpolation not defined for t=T" + + if self.traj == "trapezoidal": + trajfunc = trapezoidal_func + elif self.traj == "quintic": + trajfunc = quintic_func + + self.trajfuncs = [] + + if self.time: + # time based + if self.T is not None: + xmax = self.T + else: + xmax = simstate.T + else: + # input based + xmax = 1 + self.xmax = xmax + + for i in range(len(self.y0)): + self.trajfuncs.append(trajfunc(self.y0[i], self.yf[i], xmax)) + + def output(self, t, inports, x): + if not self.time: + t = inports[0] + + assert t >= 0, "interpolation not defined for x<0" + assert t <= self.xmax, "interpolation not defined for x>" + str(self.xmax) + + out = [] + for i in range(len(self.y0)): + out.append(self.trajfuncs[i](t)) + + # we have a list of tuples out[i][j] + # i is the timestep, j is y/yd/ydd + y = [o[0] for o in out] + yd = [o[1] for o in out] + ydd = [o[2] for o in out] + + return [np.hstack(y), np.hstack(yd), np.hstack(ydd)] + + +# ======================================================================== # + + +class IDyn(FunctionBlock): + r""" :blockname:`IDYN` - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 3 | 1 | 0 | - +------------+---------+---------+ - | ndarray, | ndarray | | - | ndarray, | | | - | ndarray | | | - +------------+---------+---------+ + Robot arm forward dynamics model. + + :inputs: 3 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{q}`, joint configuration + * - Input + - 1 + - ndarray(N) + - :math:`\dot{\mathit{q}}`, joint velocity + * - Input + - 2 + - ndarray(N) + - :math:`\ddot{\mathit{q}}`, joint acceleration + * - Output + - 0 + - ndarray(N) + - :math:`\mathit{Q}`, generalized joint force + + Compute the generalized joint torques required to achieve the input joint + configuration, velocity and acceleration. This uses the recursive Newton-Euler + (RNE) algorithm. + + .. todo:: end-effector wrench input, base wrench output, payload input + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.rne` """ nin = 3 nout = 1 - inlabels = ('q', 'qd', 'qdd') - outlabels = ('τ') + inlabels = ("q", "qd", "qdd") + outlabels = "τ" def __init__(self, robot, gravity=None, **blockargs): """ - :param robot: Robot model :type robot: Robot subclass :param gravity: gravitational acceleration :type gravity: float :param blockargs: |BlockOptions| :type blockargs: dict - :return: an INVERSE_DYNAMICS block - :rtype: Inverse_Dynamics instance - - Robot arm forward dynamics model. - - The block has three input port: - - 1. Joint configuration vector as an ndarray. - 2. Joint velocity vector as an ndarray. - 3. Joint acceleration vector as an ndarray. - - and one output port: - - 1. joint torque/force - - .. TODO:: end-effector wrench input, base wrench output, payload input """ if robot is None: - raise ValueError('robot is not defined') + raise ValueError("robot is not defined") super().__init__(**blockargs) - self.type = "inverse-dynamics" + # self.type = "inverse-dynamics" self.robot = robot self.gravity = gravity @@ -630,238 +1044,263 @@ def __init__(self, robot, gravity=None, **blockargs): self.inport_names(("q", "qd", "qdd")) self.outport_names(("$\tau$",)) - def output(self, t=None): - tau = self.robot.rne(self.inputs[0], self.inputs[1], self.inputs[2], gravity=gravity) + def output(self, t, inports, x): + tau = self.robot.rne(inports[0], inports[1], inports[2], gravity=self.gravity) return [tau] +# ------------------------------------------------------------------------ # + + class Gravload(FunctionBlock): - """ + r""" :blockname:`GRAVLOAD` - .. table:: - :align: left + Robot arm gravity load. + + :inputs: 1 + :outputs: 1 + :states: 0 - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | ndarray | | - +------------+---------+---------+ + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{q}`, joint configuration + * - Output + - 0 + - ndarray(N) + - :math:`\mathit{g}`, generalized joint force + + Compute generalized joint forces due to gravity for the input joint + configuration. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.gravload` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('τ') + inlabels = ("q",) + outlabels = "τ" def __init__(self, robot, gravity=None, **blockargs): """ - :param robot: Robot model :type robot: Robot subclass :param gravity: gravitational acceleration :type gravity: float :param blockargs: |BlockOptions| :type blockargs: dict - :return: a GRAVLOAD block - :rtype: Gravload instance - - Robot arm gravity torque. - - The block has one input port: - - 1. Joint configuration vector as an ndarray. - - and one output port: - - 1. joint torque/force due to gravity - """ if robot is None: - raise ValueError('robot is not defined') - + raise ValueError("robot is not defined") + super().__init__(**blockargs) - self.type = "gravload" + # self.type = "gravload" - self.robot = robots + self.robot = robot self.gravity = gravity self.inport_names(("q",)) self.outport_names(("$\tau$",)) - def output(self, t=None): - tau = self.robot.gravload(self.inputs[0], gravity=self.gravity) + def output(self, t, inports, x): + tau = self.robot.gravload(inports[0], gravity=self.gravity) return [tau] + class Gravload_X(FunctionBlock): - """ + r""" :blockname:`GRAVLOAD_X` - .. table:: - :align: left + Task-space robot arm gravity wrench. + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | ndarray | | - +------------+---------+---------+ + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(6) + - :math:`\mathit{x}`, end-effector pose + * - Output + - 0 + - ndarray(6) + - :math:`\mathit{g}_x`, generalized joint force + + Compute end-effector wrench due to gravity for the input end-effector pose. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.gravload_x` + :meth:`~roboticstoolbox.robot.Robot.Robot.gravload` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('w') + inlabels = ("q",) + outlabels = "w" - def __init__(self, robot, gravity=None, **blockargs): + def __init__(self, robot, representation="rpy/xyz", gravity=None, **blockargs): """ - :param robot: Robot model :type robot: Robot subclass + :param representation: task-space representation, defaults to "rpy/xyz" + :type representation: str :param gravity: gravitational acceleration :type gravity: float :param blockargs: |BlockOptions| :type blockargs: dict - :return: a GRAVLOAD block - :rtype: Gravload instance - - Robot arm gravity torque. - - The block has one input port: - - 1. Joint configuration vector as an ndarray. - - and one output port: - - 1. joint torque/force due to gravity - """ if robot is None: - raise ValueError('robot is not defined') - + raise ValueError("robot is not defined") + super().__init__(**blockargs) - self.type = "gravload-x" + # self.type = "gravload-x" - self.robot = robots + self.robot = robot self.gravity = gravity self.inport_names(("q",)) self.outport_names(("$\tau$",)) + self.representation = representation - def output(self, t=None): - q = self.inputs[0] - tau = self.robot.gravload(q, gravity=self.gravity) - J = self.robot.jacob0(q) - if J.shape[0] == J.shape[1]: - w = np.linalg.inv(J).T * tau - else: - w = np.linalg.pinv(J).T * tau + def output(self, t, inports, x): + q = inports[0] + w = self.robot.gravload_x( + q, representation=self.representation, gravity=self.gravity + ) return [w] + +# ------------------------------------------------------------------------ # + + class Inertia(FunctionBlock): - """ + r""" :blockname:`INERTIA` - .. table:: - :align: left + Robot arm inertia matrix. + + :inputs: 1 [ndarray(N)] + :outputs: 3 [ndarray(N,N)] + :states: 0 + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray + - :math:`\mathit{q}`, joint configuration + * - Output + - 0 + - ndarray(N,N) + - :math:`\mathbf{M}`, mass matrix - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | ndarray | | - +------------+---------+---------+ + Joint-space inertia matrix (mass matrix) as a function of joint configuration. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.inertia` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('M') + inlabels = ("q",) + outlabels = "M" - def __init__(self, robot, gravity=None, **blockargs): + def __init__(self, robot, **blockargs): """ - :param robot: Robot model :type robot: Robot subclass :param blockargs: |BlockOptions| :type blockargs: dict - :return: an INERTIA block - :rtype: Inertia instance - - Robot arm inertia matrix. - - The block has one input port: - - 1. Joint configuration vector as an ndarray. - - and one output port: - - 1. Joint-space inertia matrix :math:`\mat{M}(q)` - """ if robot is None: - raise ValueError('robot is not defined') - + raise ValueError("robot is not defined") + super().__init__(**blockargs) - self.type = "inertia" + # self.type = "inertia" - self.robot = robots + self.robot = robot self.inport_names(("q",)) self.outport_names(("M",)) - def output(self, t=None): - M = self.robot.inertia(self.inputs[0]) + def output(self, t, inports, x): + q = inports[0] + M = self.robot.inertia(q) return [M] + +# ------------------------------------------------------------------------ # + + class Inertia_X(FunctionBlock): - """ + r""" :blockname:`INERTIA_X` - .. table:: - :align: left + Task-space robot arm inertia matrix. - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 0 | - +------------+---------+---------+ - | ndarray | ndarray | | - +------------+---------+---------+ + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(6) + - :math:`\mathit{x}`, end-effector pose + * - Output + - 0 + - ndarray(6,6) + - :math:`\mathbf{M_x}`, task-space mass matrix + + Task-space inertia matrix as a function of end-effector pose. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.inertia_x` """ nin = 1 nout = 1 - inlabels = ('q',) - outlabels = ('M') + inlabels = ("q",) + outlabels = "M" - def __init__(self, robot, representation=None, pinv=False, **blockargs): + def __init__(self, robot, representation="rpy/xyz", pinv=False, **blockargs): """ - :param robot: Robot model :type robot: Robot subclass + :param representation: task-space representation, defaults to "rpy/xyz" + :type representation: str :param blockargs: |BlockOptions| :type blockargs: dict - :return: an INERTIA_X block - :rtype: Inertia_X instance + """ + if robot is None: + raise ValueError("robot is not defined") - Robot arm task-space inertia matrix. - - The block has one input port: - - 1. Joint configuration vector as an ndarray. - - and one output port: - - 1. Task-space inertia matrix :math:`\mat{M}_x(q)` - - """ - if robot is None: - raise ValueError('robot is not defined') - - super().__init__(**blockargs) - self.type = "inertia-x" + super().__init__(**blockargs) + # self.type = "inertia-x" self.robot = robot self.representation = representation @@ -869,65 +1308,189 @@ def __init__(self, robot, representation=None, pinv=False, **blockargs): self.inport_names(("q",)) self.outport_names(("M",)) - def output(self, t=None): - q = self.inputs[0] + def output(self, t, inports, x): + q = inports[0] Mx = self.robot.inertia_x(q, pinv=self.pinv, representation=self.representation) return [Mx] + + # ------------------------------------------------------------------------ # -class FDyn_X(TransferBlock): - """ - :blockname:`FDYN_X` - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 3 | 0 | - +------------+---------+---------+ - | ndarray | ndarray,| | - | | ndarray,| | - | | ndarray | | - +------------+---------+---------+ +class FDyn(TransferBlock): + r""" + :blockname:`FDYN` + + Robot arm forward dynamics. + + :inputs: 1 + :outputs: 3 + :states: 2N + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\mathit{Q}`, generalized joint force + * - Output + - 0 + - ndarray(N) + - :math:`\mathit{q}`, joint configuration + * - Output + - 1 + - ndarray(N) + - :math:`\dot{\mathit{q}}`, joint velocity + * - Output + - 2 + - ndarray(N) + - :math:`\ddot{\mathit{q}}`, joint acceleration + + Compute the manipulator arm forward dynamics in joint space, the joint acceleration + for the input configuration and applied joint forces. The acceleration is + integrated to obtain joint velocity and joint configuration. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.fdyn` """ nin = 1 - nout = 5 - outlabels = ('q', 'qd', 'x', 'xd', 'xdd') - inlabels = ('w') + nout = 3 + outlabels = ("q", "qd", "qdd") + inlabels = "τ" - def __init__(self, robot, q0=None, gravcomp=False, velcomp=False, representation='rpy/xyz', **blockargs): + def __init__(self, robot, q0=None, **blockargs): """ :param robot: Robot model :type robot: Robot subclass - :param end: Link to compute pose of, defaults to end-effector - :type end: Link or str + :param q0: Initial joint configuration + :type q0: array_like(n) :param blockargs: |BlockOptions| :type blockargs: dict - :return: a FDYN_X block - :rtype: FDyn_X instance + """ + if robot is None: + raise ValueError("robot is not defined") - Robot arm forward dynamics model. + super().__init__(**blockargs) + # self.type = "forward-dynamics" - The block has one input port: + self.robot = robot + self.nstates = robot.n * 2 - 1. Applied end-effector wrench as an ndarray. + # state vector is [q qd] - and three output ports: + self.inport_names(("$\tau$",)) + self.outport_names(("q", "qd", "qdd")) - 1. task space pose - 2. task space velocity - 3. task space acceleration + if q0 is None: + q0 = np.zeros((robot.n,)) + else: + q0 = smb.getvector(q0, robot.n) + self._x0 = np.r_[q0, np.zeros((robot.n,))] + self._qdd = None + + def output(self, t, inports, x): + n = self.robot.n + q = x[:n] + qd = x[n:] + qdd = self._qdd # from last deriv + return [q, qd, qdd] + + def deriv(self, t, inports, x): + # return [qd qdd] + Q = inports[0] + n = self.robot.n + assert len(Q) == n, "torque vector wrong size" + + q = x[:n] + qd = x[n:] + qdd = self.robot.accel(q, qd, Q) + self._qdd = qdd + return np.r_[qd, qdd] +# ------------------------------------------------------------------------ # + + +class FDyn_X(TransferBlock): + r""" + :blockname:`FDYN_X` + + Task-space robot arm forward dynamics. + + :inputs: 1 + :outputs: 3 + :states: 12 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(6) + - :math:`\mathit{\tau}`, end-effector wrench + * - Output + - 0 + - ndarray(6) + - :math:`\mathit{x}`, end-effector pose + * - Output + - 1 + - ndarray(6) + - :math:`\dot{\mathit{x}}`, end-effector velocity + * - Output + - 2 + - ndarray(6) + - :math:`\dot{\mathit{x}}``, end-effector acceleration + + Compute the manipulator arm forward dynamics in task space, the end-effector + acceleration for the input end-effector pose and applied end-effector wrench. The + acceleration is integrated to obtain task-space velocity and task-space pose. + + :seealso: :meth:`~roboticstoolbox.robot.Robot.Robot.fdyn_x` + """ + + nin = 1 + nout = 5 + outlabels = ("q", "qd", "x", "xd", "xdd") + inlabels = "w" + + def __init__( + self, + robot, + q0=None, + gravcomp=False, + velcomp=False, + representation="rpy/xyz", + **blockargs, + ): + """ + :param robot: Robot model + :type robot: Robot subclass + :param q0: Initial joint configuration + :type q0: array_like(n) + :param gravcomp: perform gravity compensation + :type gravcomp: bool + :param velcomp: perform velocity term compensation + :type velcomp: bool + :param representation: task-space representation, defaults to "rpy/xyz" + :type representation: str + + :param blockargs: |BlockOptions| + :type blockargs: dict """ if robot is None: - raise ValueError('robot is not defined') - + raise ValueError("robot is not defined") + super().__init__(**blockargs) - self.type = "forward-dynamics-x" + # self.type = "forward-dynamics-x" self.robot = robot self.nstates = robot.n * 2 @@ -943,19 +1506,19 @@ def __init__(self, robot, q0=None, gravcomp=False, velcomp=False, representation if q0 is None: q0 = np.zeros((robot.n,)) else: - q0 = base.getvector(q0, robot.n) + q0 = smb.getvector(q0, robot.n) # append qd0, assumed to be zero self._x0 = np.r_[q0, np.zeros((robot.n,))] self._qdd = None - def output(self, t=None): + def output(self, t, inports, x): n = self.robot.n - q = self._x[:n] - qd = self._x[n:] + q = x[:n] + qd = x[n:] qdd = self._qdd # from last deriv T = self.robot.fkine(q) - x = base.tr2x(T.A) + x = smb.tr2x(T.A) Ja = self.robot.jacob0_analytical(q, self.representation) xd = Ja @ qd @@ -968,23 +1531,23 @@ def output(self, t=None): if qdd is None: xdd = None else: - Ja_dot = self.robot.jacob_dot(q, qd, J0=Ja) + Ja_dot = self.robot.jacob0_dot(q, qd, J0=Ja) xdd = Ja @ qdd + Ja_dot @ qd return [q, qd, x, xd, xdd] - def deriv(self): + def deriv(self, t, inports, x): # return [qd qdd] # get current joint space state n = self.robot.n - q = self._x[:n] - qd = self._x[n:] + q = x[:n] + qd = x[n:] # compute joint forces - w = self.inputs[0] + w = inports[0] assert len(w) == 6, "wrench vector wrong size" - Q = self.robot.jacob0_analytical(q, self.representation).T @ w + Q = self.robot.jacob0_analytical(q, representation=self.representation).T @ w if self.gravcomp or self.velcomp: if self.velcomp: @@ -992,602 +1555,18 @@ def deriv(self): else: qd_rne = np.zeros((n,)) Q_rne = self.robot.rne(q, qd_rne, np.zeros((n,))) - - qdd = self.robot.accel(q, qd, Q + Q_rne) + Q += Q_rne + qdd = self.robot.accel(q, qd, Q) self._qdd = qdd return np.r_[qd, qdd] -# ------------------------------------------------------------------------ # - -class ArmPlot(GraphicsBlock): - """ - :blockname:`ARMPLOT` - - .. table:: - :align: left - - +--------+---------+---------+ - | inputs | outputs | states | - +--------+---------+---------+ - | 1 | 0 | 0 | - +--------+---------+---------+ - | ndarray| | | - +--------+---------+---------+ - """ - - nin = 1 - nout = 0 - inlabels = ('q',) - - def __init__(self, robot=None, q0=None, backend=None, **blockargs): - """ - :param robot: Robot model - :type robot: Robot subclass - :param backend: RTB backend name, defaults to 'pyplot' - :type backend: str, optional - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: An ARMPLOT block - :rtype: ArmPlot instance - - - Create a robot animation. - - Notes: - - - Uses RTB ``plot`` method - - Example of vehicle display (animated). The label at the top is the - block name. - """ - if robot is None: - raise ValueError('robot is not defined') - - super().__init__(**blockargs) - self.inport_names(("q",)) - - if q0 is None: - q0 = np.zeros((robot.n,)) - self.robot = robot - self.backend = backend - self.q0 = q0 - self.env = None - print('ARMPLOT init') - - def start(self, state): - # create the plot - # super().reset() - # if state.options.graphics: - print('ARMPLOT init') - self.fig = self.create_figure(state) - self.env = self.robot.plot( - self.q0, backend=self.backend, fig=self.fig, block=False - ) - super().start() - - def step(self, state): - - # update the robot plot - self.robot.q = self.inputs[0] - self.env.step() - - super().step(state) - -# ------------------------------------------------------------------------ # - - -class Traj(FunctionBlock): - """ - :blockname:`TRAJ` - - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 0 or 1 | 1 | 0 | - +------------+---------+---------+ - | float | float | | - +------------+---------+---------+ - """ - - nin = -1 - nout = 3 - outlabels = ('q',) - - def __init__(self, y0=0, yf=1, T=None, time=False, traj="trapezoidal", **blockargs): - """ - - :param y0: initial value, defaults to 0 - :type y0: array_like(m), optional - :param yf: final value, defaults to 1 - :type yf: array_like(m), optional - :param T: time vector or number of steps, defaults to None - :type T: array_like or int, optional - :param time: x is simulation time, defaults to False - :type time: bool, optional - :param traj: trajectory type, one of: 'trapezoidal' [default], 'quintic' - :type traj: str, optional - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: TRAJ block - :rtype: Traj instance - - Create a trajectory block. - - A block that generates a trajectory using a trapezoidal or quintic - polynomial profile. - - """ - self.time = time - if time: - nin = 1 - blockclass = "function" - else: - nin = 0 - blockclass = "source" - - super().__init__(nin=nin, blockclass=blockclass, **blockargs) - - y0 = base.getvector(y0) - yf = base.getvector(yf) - assert len(y0) == len(yf), "y0 and yf must have same length" - - self.y0 = y0 - self.yf = yf - self.time = time - self.T = T - self.traj = traj - - self.outport_names(("y", "yd", "ydd")) - - def start(self, **blockargs): - if self.time: - assert self.x[0] <= 0, "interpolation not defined for t=0" - assert self.x[-1] >= self.bd.T, "interpolation not defined for t=T" - - if self.traj == "trapezoidal": - trajfunc = trapezoidal_func - elif self.traj == "quintic": - trajfunc = quintic_func - - self.trajfuncs = [] - for i in range(len(self.y0)): - self.trajfuncs.append(trajfunc(self.y0[i], self.yf[i], self.T)) - - def output(self, t=None): - if self.time: - t = self.inputs[0] - - out = [] - for i in range(len(self.y0)): - out.append(self.trajfuncs[i](t)) - - # we have a list of tuples out[i][j] - # i is the timestep, j is y/yd/ydd - y = [o[0] for o in out] - yd = [o[1] for o in out] - ydd = [o[2] for o in out] - - return [np.hstack(y), np.hstack(yd), np.hstack(ydd)] - -# ------------------------------------------------------------------------ # - -class JTraj(SourceBlock): - """ - :blockname:`JTRAJ` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 0 | 3 | 0 | - +------------+------------+---------+ - | | ndarray(n) | | - +------------+------------+---------+ - """ - - nin = 0 - nout = 3 - outlabels = ('q', 'qd', 'qdd') - - def __init__(self, q0, qf, qd0=None, qdf=None, T=None, **blockargs): - """ - Compute a joint-space trajectory - - :param q0: initial joint coordinate - :type q0: array_like(n) - :param qf: final joint coordinate - :type qf: array_like(n) - :param T: time vector or number of steps, defaults to None - :type T: array_like or int, optional - :param qd0: initial velocity, defaults to None - :type qd0: array_like(n), optional - :param qdf: final velocity, defaults to None - :type qdf: array_like(n), optional - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: TRAJ block - :rtype: Traj instance - - - ``tg = jtraj(q0, qf, N)`` is a joint space trajectory where the joint - coordinates vary from ``q0`` (M) to ``qf`` (M). A quintic (5th order) - polynomial is used with default zero boundary conditions for velocity and - acceleration. Time is assumed to vary from 0 to 1 in ``N`` steps. - - - ``tg = jtraj(q0, qf, t)`` as above but ``t`` is a uniformly-spaced time - vector - - The return value is an object that contains position, velocity and - acceleration data. - - Notes: - - - The time vector, if given, scales the velocity and acceleration outputs - assuming that the time vector starts at zero and increases - linearly. - - :seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj` - """ - super().__init__(**blockargs) - self.type = "source" - self.outport_names( - ( - "q", - "qd", - "qdd", - ) - ) - - q0 = base.getvector(q0) - qf = base.getvector(qf) - - if not len(q0) == len(qf): - raise ValueError("q0 and q1 must be same size") - - if qd0 is None: - qd0 = np.zeros(q0.shape) - else: - qd0 = getvector(qd0) - if not len(qd0) == len(q0): - raise ValueError("qd0 has wrong size") - if qdf is None: - qdf = np.zeros(q0.shape) - else: - qd1 = getvector(qdf) - if not len(qd1) == len(q0): - raise ValueError("qd1 has wrong size") - - self.q0 = q0 - self.qf = qf - self.qd0 = qd0 - self.qdf = qf - - # call start now, so that output works when called by compile - # set T to 1 just for now - if T is None: - self.T = 1 - self.start() - self.T = T - - - def start(self, state=None): - - if self.T is None: - # use simulation tmax - self.T = state.T - - tscal = self.T - self.tscal = tscal - - q0 = self.q0 - qf = self.qf - qd0 = self.qd0 - qdf = self.qdf - - # compute the polynomial coefficients - A = 6 * (qf - q0) - 3 * (qdf + qd0) * tscal - B = -15 * (qf - q0) + (8 * qd0 + 7 * qdf) * tscal - C = 10 * (qf - q0) - (6 * qd0 + 4 * qdf) * tscal - E = qd0 * tscal - F = q0 - - self.coeffs = np.array([A, B, C, np.zeros(A.shape), E, F]) - self.dcoeffs = np.array( - [np.zeros(A.shape), 5 * A, 4 * B, 3 * C, np.zeros(A.shape), E] - ) - self.ddcoeffs = np.array( - [ - np.zeros(A.shape), - np.zeros(A.shape), - 20 * A, - 12 * B, - 6 * C, - np.zeros(A.shape), - ] - ) - - def output(self, t=None): - - tscal = self.tscal - ts = t / tscal - tt = np.array([ts ** 5, ts ** 4, ts ** 3, ts ** 2, ts, 1]).T - - qt = tt @ self.coeffs - - # compute velocity - qdt = tt @ self.dcoeffs / tscal - - # compute acceleration - qddt = tt @ self.ddcoeffs / tscal ** 2 - - return [qt, qdt, qddt] - -# ------------------------------------------------------------------------ # - -class LSPB(SourceBlock): - """ - :blockname:`LSPB` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 0 | 3 | 0 | - +------------+------------+---------+ - | | float | | - +------------+------------+---------+ - """ - - nin = 0 - nout = 3 - outlabels = ('q', 'qd', 'qdd') - - def __init__(self, q0, qf, V=None, T=None, **blockargs): - """ - Compute a joint-space trajectory - - :param q0: initial joint coordinate - :type q0: array_like(n) - :param qf: final joint coordinate - :type qf: array_like(n) - :param T: time vector or number of steps, defaults to None - :type T: array_like or int, optional - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: LSPB block - :rtype: LSPB instance - - - ``tg = jtraj(q0, qf, N)`` is a joint space trajectory where the joint - coordinates vary from ``q0`` (M) to ``qf`` (M). A quintic (5th order) - polynomial is used with default zero boundary conditions for velocity and - acceleration. Time is assumed to vary from 0 to 1 in ``N`` steps. - - - ``tg = jtraj(q0, qf, t)`` as above but ``t`` is a uniformly-spaced time - vector - - The return value is an object that contains position, velocity and - acceleration data. - - Notes: - - - The time vector, if given, scales the velocity and acceleration outputs - assuming that the time vector starts at zero and increases - linearly. - - :seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj` - """ - super().__init__(nout=3, **blockargs) - self.type = "source" - self.T = T - self.q0 = q0 - self.qf = qf - - def start(self): - - if self.T is None: - self.T = self.bd.state.T - self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, self.T) - - def output(self, t=None): - return self.trapezoidalfunc(t) - -# ------------------------------------------------------------------------ # - -class CTraj(SourceBlock): - """ - :blockname:`CTRAJ` - - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 0 | 1 | 0 | - +------------+---------+---------+ - | | float | | - +------------+---------+---------+ - """ - - nin = 0 - nout = 1 - outlabels = ('T',) - - def __init__( - self, - T1, - T2, - T, - trapezoidal=True, - **blockargs - ): - """ - [summary] - - :param T1: initial pose - :type T1: SE3 - :param T2: final pose - :type T2: SE3 - :param T: motion time - :type T: float - :param trapezoidal: Use LSPB motion profile along the path - :type trapezoidal: bool - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: CTRAJ block - :rtype: CTraj instance - - Create a Cartesian motion block. - - The block outputs a pose that varies smoothly from ``T1`` to ``T2`` over - the course of ``T`` seconds. - - If ``T`` is not given it defaults to the simulation time. - - If ``trapezoidal`` is True then a trapezoidal motion profile is used along the path - to provide initial acceleration and final deceleration. Otherwise, - motion is at constant velocity. - - :seealso: :method:`SE3.interp` - - """ - - # TODO - # flag to rotate the frame rather than just translate it - super().__init__(**blockargs) - - self.T1 = T1 - self.T2 = T2 - self.T = T - - def start(self, state): - if self.T is None: - self.T = self.bd.state.T - if self.trapezoidal: - self.trapezoidalfunc = trapezoidal_func(self.q0, self.qf, self.T) - - def output(self, t=None): - if trapezoidal: - s = self.trapezoidalfunc(t) - else: - s = np.min(t / self.T, 1.0) - - return self.T1.interp(self.T2, s) - - -# ------------------------------------------------------------------------ # - -class CirclePath(SourceBlock): - """ - :blockname:`CIRCLEPATH` - - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 0 or 1 | 1 | 0 | - +------------+---------+---------+ - | float | float | | - +------------+---------+---------+ - """ - - nin = 0 - nout = 1 - - def __init__( - self, - radius=1, - centre=(0, 0, 0), - pose=None, - frequency=1, - unit="rps", - phase=0, - **blockargs - ): - """ - - :param radius: radius of circle, defaults to 1 - :type radius: float - :param centre: center of circle, defaults to [0,0,0] - :type centre: array_like(3) - :param pose: SE3 pose of output, defaults to None - :type pose: SE3 - :param frequency: rotational frequency, defaults to 1 - :type frequency: float - :param unit: unit for frequency, one of: 'rps' [default], 'rad' - :type unit: str - :param phase: phase - :type phase: float - :param blockargs: |BlockOptions| - :type blockargs: dict - :return: TRAJ block - :rtype: Traj instance - - Create a circular motion block. - - The block outputs the coordinates of a point moving in a circle of - radius ``r`` centred at ``centre`` and parallel to the xy-plane. - - By default the output is a 3-vector :math:`(x, y, z)` but if - ``pose`` is an ``SE3`` instance the output is a copy of that pose with - its translation set to the coordinate of the moving point. This is the - motion of a frame with fixed orientation following a circular path. - - """ - - # TODO - # flag to rotate the frame rather than just translate it - super().__init__(**blockargs) - - if unit == "rps": - omega = frequency * 2 * pi - phase = frequency * 2 * pi - elif unit == "rad": - omega = frequency - - # Redundant assignment, commented for LGTM - # phase = phase - else: - raise ValueError("bad units: rps or rad") - - self.radius = radius - assert len(centre) == 3, "centre must be a 3 vector" - self.centre = centre - self.pose = pose - self.omega = omega - self.phase = phase - - self.outport_names(("y",)) - - def output(self, t=None): - theta = t * self.omega + self.phase - x = self.radius * cos(theta) + self.centre[0] - y = self.radius * sin(theta) + self.centre[1] - p = (x, y, self.centre[2]) - - if self.pose is not None: - pp = SE3.Rt(self.pose.R, p) - p = pp - - return [p] - if __name__ == "__main__": - - import pathlib - import os.path + from pathlib import Path exec( open( - os.path.join(pathlib.Path(__file__).parent.absolute(), "test_robots.py") + Path(__file__).parent.parent.parent.absolute() / "tests" / "test_blocks.py" ).read() ) diff --git a/roboticstoolbox/blocks/mobile.py b/roboticstoolbox/blocks/mobile.py index 9def35187..557027750 100644 --- a/roboticstoolbox/blocks/mobile.py +++ b/roboticstoolbox/blocks/mobile.py @@ -9,36 +9,66 @@ from bdsim.components import TransferBlock from bdsim.graphics import GraphicsBlock -from spatialmath import base from roboticstoolbox import mobile # ------------------------------------------------------------------------ # class Bicycle(TransferBlock): - """ + r""" :blockname:`BICYCLE` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 2 | 1 | 3 | - +------------+------------+---------+ - | float | ndarray(3) | | - +------------+------------+---------+ + + Vehicle model with Bicycle kinematics. + + :inputs: 2 + :outputs: 1 + :states: 3 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`v`, longitudinal velocity + * - Input + - 1 + - float + - :math:`\gamma`, steer angle + * - Output + - 0 + - ndarray(3) + - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration + + Bicycle kinematic model with velocity and steering inputs and vehicle configuration + as output. + + Various limits are applied to the inputs: + + * velocity limit ``speed_max`` + * acceleration limit ``accel_max`` + * steering limit ``steer_max`` + + :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Bicycle` :class:`Unicycle` :class:`DiffSteer` """ nin = 2 nout = 1 - inlabels = ('v', 'γ') - outlabels = ('q',) - - def __init__(self, L=1, speed_max=np.inf, accel_max=np.inf, steer_max=0.45 * pi, - x0=None, **blockargs): + inlabels = ("v", "γ") + outlabels = ("q",) + + def __init__( + self, + L=1, + speed_max=np.inf, + accel_max=np.inf, + steer_max=0.45 * pi, + x0=None, + **blockargs, + ): r""" - Create a vehicle model with Bicycle kinematics. - :param L: Wheelbase, defaults to 1 :type L: float, optional :param speed_max: Velocity limit, defaults to math.inf @@ -51,73 +81,95 @@ def __init__(self, L=1, speed_max=np.inf, accel_max=np.inf, steer_max=0.45 * pi, :type x0: array_like(3), optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a BICYCLE block - :rtype: Bicycle instance - - - Bicycle kinematic model with state/configuration :math:`[x, y, \theta]`. - - **Block ports** - - :input v: Vehicle speed (metres/sec). The velocity limit ``speed_max`` - and acceleration limit ``accel_max`` is - applied to this input. - :input γ: Steered wheel angle (radians). The steering limit ``steer_max`` - is applied to this input. - - :output q: configuration (x, y, θ) - - :seealso: :class:`roboticstoolbox.mobile.Bicycle` :class:`Unicycle` :class:`DiffSteer` """ # TODO: add option to model the effect of steering arms, responds to # gamma dot - + super().__init__(nstates=3, **blockargs) + self.type = "bicycle" - self.vehicle = mobile.Bicycle(L=L, - steer_max=steer_max, speed_max=speed_max, accel_max=accel_max) + self.vehicle = mobile.Bicycle( + L=L, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max + ) if x0 is None: self._x0 = np.zeros((self.nstates,)) else: - assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) + assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format( + len(x0), self.nstates + ) self._x0 = x0 - - self.inport_names(('v', '$\gamma$')) - self.outport_names(('q',)) - self.state_names(('x', 'y', r'$\theta$')) - - def output(self, t): - return [self._x] # one output which is ndarray(3) - - def deriv(self): - return self.vehicle.deriv(self._x, self.inputs) - + + self.inport_names(("v", "$\gamma$")) + self.outport_names(("q",)) + self.state_names(("x", "y", r"$\theta$")) + + def output(self, t, inports, x): + return [x] # one output which is ndarray(3) + + def deriv(self, t, inports, x): + return self.vehicle.deriv(x, inports) + + # ------------------------------------------------------------------------ # class Unicycle(TransferBlock): r""" :blockname:`UNICYCLE` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 2 | 1 | 3 | - +------------+------------+---------+ - | float | ndarray(3) | | - +------------+------------+---------+ + + Vehicle model with Unicycle kinematics. + + :inputs: 2 + :outputs: 1 + :states: 3 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`v`, longitudinal velocity + * - Input + - 1 + - float + - :math:`\omega`, turn rate + * - Output + - 0 + - ndarray(3) + - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration + + Unicycle kinematic model with velocity and steering inputs and vehicle configuration + as output. + + Various limits are applied to the inputs: + + * velocity limit ``speed_max`` + * acceleration limit ``accel_max`` + * steering limit ``steer_max`` + + :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Unicycle` :class:`Bicycle` :class:`DiffSteer` + """ nin = 2 nout = 1 - inlabels = ('v', 'ω') - outlabels = ('q',) - - def __init__(self, w=1, speed_max=np.inf, accel_max=np.inf, steer_max=np.inf, - a=0, x0=None, **blockargs): + inlabels = ("v", "ω") + outlabels = ("q",) + + def __init__( + self, + w=1, + speed_max=np.inf, + accel_max=np.inf, + steer_max=np.inf, + # a=0, implement this, RVC2 p111, change output and deriv + x0=None, + **blockargs, + ): r""" - Create a vehicle model with Unicycle kinematics. :param w: vehicle width, defaults to 1 :type w: float, optional @@ -131,82 +183,107 @@ def __init__(self, w=1, speed_max=np.inf, accel_max=np.inf, steer_max=np.inf, :type x0: array_like(3), optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a UNICYCLE block - :rtype: Unicycle instance - - Unicycle kinematic model with state/configuration :math:`[x, y, \theta]`. - - **Block ports** - - :input v: Vehicle speed (metres/sec). The velocity limit ``speed_max`` and - acceleration limit ``accel_max`` is - applied to this input. - :input ω: Angular velocity (radians/sec). The steering limit ``steer_max`` - is applied to this input. - - :output q: configuration (x, y, θ) - - :seealso: :class:`roboticstoolbox.mobile.Unicycle` :class:`Bicycle` :class:`DiffSteer` - """ + """ super().__init__(nstates=3, **blockargs) - + self.type = "unicycle" + if x0 is None: self._x0 = np.zeros((self.nstates,)) else: - assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) + assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format( + len(x0), self.nstates + ) self._x0 = x0 - self.vehicle = mobile.Unicycle(w=w, - steer_max=steer_max, speed_max=speed_max, accel_max=accel_max) + self.vehicle = mobile.Unicycle( + W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max + ) - #TODO, add support for origin shift + # TODO, add support for origin shift # If ``a`` is non-zero then the planar velocity of that point $x=a$ # can be controlled by # .. math:: - # \begin{pmatrix} v \\ \omega \end{pmatrix} = + # \begin{pmatrix} v \\ \omega \end{pmatrix} = # \begin{pmatrix} - # \cos \theta & \sin \theta \\ + # \cos \theta & \sin \theta \\ # -\frac{1}{a}\sin \theta & \frac{1}{a}\cos \theta # \end{pmatrix}\begin{pmatrix} # \dot{x} \\ \dot{y} # \end{pmatrix} - - def output(self, t): - return self._x - - def deriv(self): - return self.vehicle.deriv(self._x, self.inputs) - + + def output(self, t, inports, x): + return [x] + + def deriv(self, t, inports, x): + return self.vehicle.deriv(x, inports) + + # ------------------------------------------------------------------------ # class DiffSteer(TransferBlock): """ :blockname:`DIFFSTEER` - - .. table:: - :align: left - - +------------+------------+---------+ - | inputs | outputs | states | - +------------+------------+---------+ - | 2 | 1 | 3 | - +------------+------------+---------+ - | float | ndarray(3) | | - +------------+------------+---------+ + + Differential steer vehicle model + + :inputs: 2 + :outputs: 1 + :states: 3 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`\omega_L`, left-wheel angular velocity (radians/sec). + * - Input + - 1 + - float + - :math:`\omega_R`, right-wheel angular velocity (radians/sec). + * - Output + - 0 + - ndarray(3) + - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration + + Differential steering kinematic model with wheel velocity inputs and vehicle + configuration as output. + + Various limits are applied to the inputs: + + * velocity limit ``speed_max`` + * acceleration limit ``accel_max`` + * steering limit ``steer_max`` + + .. note:: Wheel velocity is defined such that if both are positive the vehicle + moves forward. + + :seealso: :class:`~roboticstoolbox.mobile.Vehicle.Diffsteer` :class:`Bicycle` :class:`Unicycle` """ nin = 2 nout = 1 - inlabels = ('ωL', 'ωR') - outlabels = ('q',) - - def __init__(self, w=1, R=1, speed_max=np.inf, accel_max=np.inf, steer_max=None, - a=0, x0=None, **blockargs): + inlabels = ("ωL", "ωR") + outlabels = ("q",) + + def __init__( + self, + w=1, + R=1, + speed_max=np.inf, + accel_max=np.inf, + steer_max=None, + a=0, + x0=None, + **blockargs, + ): r""" - Create a differential steer vehicle model - :param w: vehicle width, defaults to 1 :type w: float, optional :param R: Wheel radius, defaults to 1 @@ -221,79 +298,84 @@ def __init__(self, w=1, R=1, speed_max=np.inf, accel_max=np.inf, steer_max=None, :type x0: array_like, optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a DIFFSTEER block - :rtype: DiffSteer instance - - Unicycle kinematic model with state :math:`[x, y, \theta]`, with - inputs given as wheel angular velocity. - - **Block ports** - - :input ωL: Left-wheel angular velocity (radians/sec). - :input ωR: Right-wheel angular velocity (radians/sec). - - :output q: configuration (x, y, θ) - - The resulting forward velocity and turning rate from ωL and ωR have - the velocity limit ``speed_max`` and acceleration limit ``accel_max`` - applied, as well as the turning rate limit ``steer_max``. - - .. note:: Wheel velocity is defined such that if both are positive the vehicle - moves forward. - - :seealso: :class:`roboticstoolbox.mobile.Unicycle` :class:`Bicycle` :class:`Unicycle` """ super().__init__(nstates=3, **blockargs) - self.type = 'diffsteer' + self.type = "diffsteer" self.R = R - + if x0 is None: self._x0 = np.zeros((slef.nstates,)) else: - assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format(len(x0), self.nstates) + assert len(x0) == self.nstates, "x0 is {:d} long, should be {:d}".format( + len(x0), self.nstates + ) self._x0 = x0 - self.vehicle = mobile.Unicycle(w=w, - steer_max=steer_max, speed_max=speed_max, accel_max=accel_max) + self.vehicle = mobile.DiffSteer( + W=w, steer_max=steer_max, speed_max=speed_max, accel_max=accel_max + ) + + def output(self, t, inports, x): + return [x] + + def deriv(self, t, inports, x): + return self.vehicle.deriv(x, inports) - def output(self, t): - return self._x - - def deriv(self): - # compute (v, omega) from left/right wheel speeds - v = self.R * (self.inputs[0] + self.inputs[1]) / 2 - omega = (self.inputs[1] + self.inputs[0]) / self.W - return self.vehicle.deriv(self._x, (v, omega)) # ------------------------------------------------------------------------ # + class VehiclePlot(GraphicsBlock): - """ + r""" :blockname:`VEHICLEPLOT` - - .. table:: - :align: left - - +--------+---------+---------+ - | inputs | outputs | states | - +--------+---------+---------+ - | 1 | 0 | 0 | - +--------+---------+---------+ - | ndarray| | | - +--------+---------+---------+ + + Vehicle animation + + :inputs: 1 + :outputs: 0 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`\mathit{q} = (x, y, \theta)`, vehicle configuration + + Create a vehicle animation similar to the figure below. + + .. figure:: ../figs/rvc4_4.gif + :width: 500px + :alt: example of generated graphic + + Example of vehicle display (animated). The label at the top is the + block name. """ nin = 1 nout = 0 - inlabels = ('q',) + inlabels = ("q",) # TODO add ability to render an image instead of an outline - - def __init__(self, animation=None, path=None, labels=['X', 'Y'], square=True, init=None, scale=True, **blockargs): - """ - Create a vehicle animation + def __init__( + self, + animation=None, + path=None, + labels=["X", "Y"], + square=True, + init=None, + scale="auto", + polyargs={}, + **blockargs, + ): + """ :param animation: Graphical animation of vehicle, defaults to None :type animation: VehicleAnimation subclass, optional :param path: linestyle to plot path taken by vehicle, defaults to None @@ -306,111 +388,91 @@ def __init__(self, animation=None, path=None, labels=['X', 'Y'], square=True, in :type init: callable, optional :param scale: scale of plot, defaults to "auto" :type scale: list or str, optional + :param polyargs: arguments passed to :meth:`Animation.Polygon` + :type polyargs: dict :param blockargs: |BlockOptions| :type blockargs: dict - :return: A VEHICLEPLOT block - :rtype: VehiclePlot instance - - Create a vehicle animation similar to the figure below. - **Block ports** + .. note:: - :input q: configuration (x, y, θ) - - Notes: - - The ``init`` function is called after the axes are initialized and can be used to draw application specific detail on the plot. In the example below, this is the dot and star. - A dynamic trail, showing path to date can be animated if the option ``path`` is set to a linestyle. - - .. figure:: ../../figs/rvc4_4.gif - :width: 500px - :alt: example of generated graphic - - Example of vehicle display (animated). The label at the top is the - block name. """ super().__init__(**blockargs) self.xdata = [] self.ydata = [] - self.type = 'vehicleplot' + # self.type = "vehicleplot" if init is not None: - assert callable(init), 'graphics init function must be callable' + assert callable(init), "graphics init function must be callable" self.init = init self.square = square self.pathstyle = path - - if scale != 'auto': + + if scale != "auto": if len(scale) == 2: scale = scale * 2 self.scale = scale self.labels = labels - + if animation is None: - animation = mobile.VehiclePolygon() + animation = mobile.VehiclePolygon(**polyargs) elif not isinstance(animation, mobile.VehicleAnimationBase): - raise TypeError('animation object must be VehicleAnimationBase subclass') + raise TypeError("animation object must be VehicleAnimationBase subclass") self.animation = animation - - def start(self, state=None): + def start(self, simstate): + super().start(simstate) + # create the plot # super().reset() # create the figures - self.fig = self.create_figure(state) + self.fig = self.create_figure(simstate) self.ax = self.fig.add_subplot(111) - + if self.square: - self.ax.set_aspect('equal') - print('done') + self.ax.set_aspect("equal") + print("done") self.ax.grid(True) self.ax.set_xlabel(self.labels[0]) self.ax.set_ylabel(self.labels[1]) self.ax.set_title(self.name) - if self.scale != 'auto': + if self.scale != "auto": self.ax.set_xlim(*self.scale[0:2]) self.ax.set_ylim(*self.scale[2:4]) if self.init is not None: self.init(self.ax) if isinstance(self.pathstyle, str): - self.line, = plt.plot(0, 0, self.pathstyle) + (self.line,) = plt.plot(0, 0, self.pathstyle) elif isinstance(self.pathstyle, dict): - self.line, = plt.plot(0, 0, **self.pathstyle) + (self.line,) = plt.plot(0, 0, **self.pathstyle) self.animation.add() - - plt.draw() - plt.show(block=False) - super().start() - - def step(self, state=None, **kwargs): + # plt.draw() + # plt.show(block=False) + + def step(self, t, inports): # inputs are set - xyt = self.inputs[0] + xyt = inports[0] # update the path line self.xdata.append(xyt[0]) self.ydata.append(xyt[1]) - #plt.figure(self.fig.number) + # plt.figure(self.fig.number) if self.pathstyle is not None: self.line.set_data(self.xdata, self.ydata) # update the vehicle pose self.animation.update(xyt) - - if self.scale == 'auto': + + if isinstance(self.scale, str) and self.scale == "auto": self.ax.relim() self.ax.autoscale_view() - super().step(state=state) - - def done(self, block=False, **kwargs): - if self.bd.options.graphics: - plt.show(block=block) - - super().done() + super().step(t, inports) diff --git a/roboticstoolbox/blocks/quad_model.py b/roboticstoolbox/blocks/quad_model.py new file mode 100644 index 000000000..bbc63e857 --- /dev/null +++ b/roboticstoolbox/blocks/quad_model.py @@ -0,0 +1,132 @@ +# MDL_QUADCOPTER Dynamic parameters for a quadrotor. +# +# MDL_QUADCOPTER is a script creates the workspace variable quad which +# describes the dynamic characterstics of a quadrotor flying robot. +# +# Properties:: +# +# This is a structure with the following elements: +# +# nrotors Number of rotors (1x1) +# J Flyer rotational inertia matrix (3x3) +# h Height of rotors above CoG (1x1) +# d Length of flyer arms (1x1) +# nb Number of blades per rotor (1x1) +# r Rotor radius (1x1) +# c Blade chord (1x1) +# e Flapping hinge offset (1x1) +# Mb Rotor blade mass (1x1) +# Mc Estimated hub clamp mass (1x1) +# ec Blade root clamp displacement (1x1) +# Ib Rotor blade rotational inertia (1x1) +# Ic Estimated root clamp inertia (1x1) +# mb Static blade moment (1x1) +# Ir Total rotor inertia (1x1) +# Ct Non-dim. thrust coefficient (1x1) +# Cq Non-dim. torque coefficient (1x1) +# sigma Rotor solidity ratio (1x1) +# thetat Blade tip angle (1x1) +# theta0 Blade root angle (1x1) +# theta1 Blade twist angle (1x1) +# theta75 3/4 blade angle (1x1) +# thetai Blade ideal root approximation (1x1) +# a Lift slope gradient (1x1) +# A Rotor disc area (1x1) +# gamma Lock number (1x1) +# +# +# Notes:: +# - SI units are used. +# +# References:: +# - Design, Construction and Control of a Large Quadrotor micro air vehicle. +# P.Pounds, PhD thesis, +# Australian National University, 2007. +# http://www.eng.yale.edu/pep5/P_Pounds_Thesis_2008.pdf +# - This is a heavy lift quadrotor +# + +import numpy as np +from math import pi, sqrt, inf + +quadrotor = {} +quadrotor["nrotors"] = 4 # 4 rotors +quadrotor["g"] = 9.81 # g Gravity +quadrotor["rho"] = 1.184 # rho Density of air +quadrotor["muv"] = 1.5e-5 # muv Viscosity of air + +# Airframe +quadrotor["M"] = 4 # M Mass +Ixx = 0.082 +Iyy = 0.082 +Izz = 0.149 # 0.160 +quadrotor["J"] = np.diag([Ixx, Iyy, Izz]) # I Flyer rotational inertia matrix 3x3 + +quadrotor["h"] = -0.007 # h Height of rotors above CoG +quadrotor["d"] = 0.315 # d Length of flyer arms + +# Rotor +quadrotor["nb"] = 2 # b Number of blades per rotor +quadrotor["r"] = 0.165 # r Rotor radius + +quadrotor["c"] = 0.018 # c Blade chord + +quadrotor["e"] = 0.0 # e Flapping hinge offset +quadrotor["Mb"] = 0.005 # Mb Rotor blade mass +quadrotor["Mc"] = 0.010 # Mc Estimated hub clamp mass +quadrotor["ec"] = 0.004 # ec Blade root clamp displacement +quadrotor["Ib"] = ( + quadrotor["Mb"] * (quadrotor["r"] - quadrotor["ec"]) ** 2 / 4 +) # Ib Rotor blade rotational inertia +quadrotor["Ic"] = ( + quadrotor["Mc"] * (quadrotor["ec"]) ** 2 / 4 +) # Ic Estimated root clamp inertia +quadrotor["mb"] = quadrotor["g"] * ( + quadrotor["Mc"] * quadrotor["ec"] / 2 + quadrotor["Mb"] * quadrotor["r"] / 2 +) # mb Static blade moment +quadrotor["Ir"] = quadrotor["nb"] * ( + quadrotor["Ib"] + quadrotor["Ic"] +) # Ir Total rotor inertia + +quadrotor["Ct"] = 0.0048 # Ct Non-dim. thrust coefficient +quadrotor["Cq"] = quadrotor["Ct"] * sqrt( + quadrotor["Ct"] / 2 +) # Cq Non-dim. torque coefficient + +quadrotor["sigma"] = ( + quadrotor["c"] * quadrotor["nb"] / (pi * quadrotor["r"]) +) # sigma Rotor solidity ratio +quadrotor["thetat"] = 6.8 * (pi / 180) # thetat Blade tip angle +quadrotor["theta0"] = 14.6 * (pi / 180) # theta0 Blade root angle +quadrotor["theta1"] = ( + quadrotor["thetat"] - quadrotor["theta0"] +) # theta1 Blade twist angle +quadrotor["theta75"] = ( + quadrotor["theta0"] + 0.75 * quadrotor["theta1"] +) # theta76 3/4 blade angle +try: + quadrotor["thetai"] = quadrotor["thetat"] * ( + quadrotor["r"] / quadrotor["e"] + ) # thetai Blade ideal root approximation +except ZeroDivisionError: + quadrotor["thetai"] = inf +quadrotor["a"] = 5.5 # a Lift slope gradient + +# derived constants +quadrotor["A"] = pi * quadrotor["r"] ** 2 # A Rotor disc area +quadrotor["gamma"] = ( + quadrotor["rho"] + * quadrotor["a"] + * quadrotor["c"] + * quadrotor["r"] ** 4 + / (quadrotor["Ib"] + quadrotor["Ic"]) +) # gamma Lock number + +quadrotor["b"] = ( + quadrotor["Ct"] * quadrotor["rho"] * quadrotor["A"] * quadrotor["r"] ** 2 +) # T = b w^2 +quadrotor["k"] = ( + quadrotor["Cq"] * quadrotor["rho"] * quadrotor["A"] * quadrotor["r"] ** 3 +) # Q = k w^2 + +quadrotor["verbose"] = False diff --git a/roboticstoolbox/blocks/spatial.py b/roboticstoolbox/blocks/spatial.py new file mode 100644 index 000000000..fcb4113cc --- /dev/null +++ b/roboticstoolbox/blocks/spatial.py @@ -0,0 +1,247 @@ +import numpy as np +from math import sin, cos, pi + +# import matplotlib.pyplot as plt +import time +from spatialmath import SE3 +import spatialmath.base as smb + +from bdsim.components import TransferBlock, FunctionBlock, SourceBlock +from bdsim.graphics import GraphicsBlock + +from roboticstoolbox import quintic_func, trapezoidal_func + + +class Tr2Delta(FunctionBlock): + r""" + :blockname:`TR2DELTA` + + Transforms to delta + + :inputs: 2 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - SE3 + - :math:`\mathbf{T}_1` pose. + * - Input + - 1 + - SE3 + - :math:`\mathbf{T}_2` pose. + * - Output + - 0 + - ndarray(6) + - :math:`\Delta` + + Difference between :math:`\mathbf{T}_1` and :math:`\mathbf{T}_2` as a 6-vector + + :seealso: :class:`Delta2Tr` :func:`~spatialmath.base.transforms3d.tr2delta` + """ + + nin = 2 + nout = 1 + inlabels = ("T1", "T2") + outlabels = ("Δ",) + + def __init__(self, **blockargs): + """ + :param blockargs: |BlockOptions| + :type blockargs: dict + """ + super().__init__(**blockargs) + + self.inport_names(("T1", "T2")) + self.outport_names(("$\delta$",)) + + def output(self, t, inports, x): + return [smb.tr2delta(inports[0].A, inports[1].A)] + + +# ------------------------------------------------------------------------ # + + +class Delta2Tr(FunctionBlock): + r""" + :blockname:`DELTA2TR` + + Delta to transform + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(6) + - :math:`\Delta` + * - Output + - 0 + - SE3 + - :math:`\mathbf{T}`, pose. + + 6-vector spatial displacement to transform. + + :seealso: :class:`Tr2Delta` :func:`~spatialmath.base.transforms3d.delta2tr` + """ + + nin = 1 + nout = 1 + outlabels = ("T",) + inlabels = ("Δ",) + + def __init__(self, **blockargs): + """ + :param blockargs: |BlockOptions| + :type blockargs: dict + """ + super().__init__(**blockargs) + + self.inport_names(("$\delta$",)) + self.outport_names(("T",)) + + def output(self, t, inports, x): + return [SE3.Delta(inports[0])] + + +# ------------------------------------------------------------------------ # + + +class Point2Tr(FunctionBlock): + r""" + :blockname:`POINT2TR` + + Point to transform. + + :inputs: 1 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(3) + - :math:`\mathit{p}`, point. + * - Output + - 0 + - SE3 + - :math:`\mathbf{T}`, pose. + + The parameter ``T`` is an SE3 object whose translation part is replaced by the input + """ + + nin = 1 + nout = 1 + + def __init__(self, T=None, **blockargs): + """ + :param T: the transform + :type T: SE3 + :param blockargs: |BlockOptions| + :type blockargs: dict + + If ``T`` is None then it defaults to the identity matrix. + """ + super().__init__(**blockargs) + + self.inport_names(("t",)) + self.outport_names(("T",)) + if T is None: + T = SE3() + self.pose = T + + def output(self, t, inports, x): + T = SE3.Rt(self.pose.R, t=inports[0]) + return [T] + + +# ------------------------------------------------------------------------ # + + +class TR2T(FunctionBlock): + r""" + :blockname:`TR2T` + + Translation components of transform + + :inputs: 1 + :outputs: 3 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - SE3 + - :math:`\mathit{T}` transform. + * - Output + - 0 + - float + - :math:`x` component of translation. + * - Output + - 1 + - float + - :math:`y` component of translation. + * - Output + - 2 + - float + - :math:`z` component of translation. + + :seealso: :func:`~spatialmath.base.transforms3d.transl` + """ + + nin = 1 + nout = 3 + inlabels = ("T",) + outlabels = ("x", "y", "z") + + def __init__(self, **blockargs): + """ + :param blockargs: |BlockOptions| + :type blockargs: dict + """ + super().__init__(**blockargs) + + self.inport_names(("T",)) + self.outport_names(("x", "y", "z")) + + def output(self, t, inports, x): + t = inports[0].t + return list(t) + + +if __name__ == "__main__": + + from pathlib import Path + + exec( + open( + Path(__file__).parent.parent.parent.absolute() / "tests" / "test_blocks.py" + ).read() + ) diff --git a/roboticstoolbox/blocks/uav.py b/roboticstoolbox/blocks/uav.py index 6206e90ac..18381b0fa 100644 --- a/roboticstoolbox/blocks/uav.py +++ b/roboticstoolbox/blocks/uav.py @@ -9,75 +9,150 @@ class MultiRotor(TransferBlock): - """ + r""" :blockname:`MULTIROTOR` - - .. table:: - :align: left - - +------------+---------+---------+ - | inputs | outputs | states | - +------------+---------+---------+ - | 1 | 1 | 16 | - +------------+---------+---------+ - | ndarray(4) | dict | | - +------------+---------+---------+ + + Dynamic model of a multi-rotor flying robot. + + :inputs: 1 + :outputs: 1 + :states: 16 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - ndarray(N) + - :math:`\varpi`, rotor velocities in (radians/sec) + * - Output + - 0 + - dict + - :math:`\mathit{x}`, vehicle state + + Dynamic model of a multi-rotor flying robot that includes rotor flapping. The + vehicle state is a dict containing the following items: + + - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` + - ``trans`` position and velocity in the world frame as + :math:`[x, y, z, \dot{x}, \dot{y}, \dot{z}]` + - ``rot`` orientation and angular rate in the world frame as + :math:`[\theta_Y, \theta_P, \theta_R, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]` + - ``vb`` translational velocity in the body frame as + :math:`[\dot{x}, \dot{y}, \dot{z}]` + - ``w`` angular rates in the body frame as + :math:`[\dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]` + - ``a1s`` longitudinal flapping angles (radians) + - ``b1s`` lateral flapping angles (radians) + - ``X`` full state vector as + :math:`[x, y, z, \theta_Y, \theta_P, \theta_R, \dot{x}, \dot{y}, \dot{z}, \dot{\theta_Y}, \dot{\theta_P}, \dot{\theta_R}]` + + The dynamic model is a dict with the following key/value pairs. + + =========== ========================================== + key description + =========== ========================================== + ``nrotors`` Number of rotors (even integer) + ``J`` Flyer rotational inertia matrix (3x3) + ``h`` Height of rotors above CoG + ``d`` Length of flyer arms + ``nb`` Number of blades per rotor + ``r`` Rotor radius + ``c`` Blade chord + ``e`` Flapping hinge offset + ``Mb`` Rotor blade mass + ``Mc`` Estimated hub clamp mass + ``ec`` Blade root clamp displacement + ``Ib`` Rotor blade rotational inertia + ``Ic`` Estimated root clamp inertia + ``mb`` Static blade moment + ``Ir`` Total rotor inertia + ``Ct`` Non-dim. thrust coefficient + ``Cq`` Non-dim. torque coefficient + ``sigma`` Rotor solidity ratio + ``thetat`` Blade tip angle + ``theta0`` Blade root angle + ``theta1`` Blade twist angle + ``theta75`` 3/4 blade angle + ``thetai`` Blade ideal root approximation + ``a`` Lift slope gradient + ``A`` Rotor disc area + ``gamma`` Lock number + =========== ========================================== + + .. note:: + - Based on MATLAB code developed by Pauline Pounds 2004. + - SI units are used. + - Rotor velocity is defined looking down, clockwise from the front rotor + which lies on the x-axis. + + :References: + - Design, Construction and Control of a Large Quadrotor micro air vehicle. + P.Pounds, `PhD thesis `_ + Australian National University, 2007. + - Robotics, Vision & Control by Peter Corke, sec 4.2 in all editions + + :seealso: :class:`MultiRotorMixer` :class:`MultiRotorPlot` """ + nin = 1 nout = 1 - - # Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04 - # A simulation of idealised X-4 Flyer II flight dynamics. - # version 2.0 2005 modified to be compatible with latest version of Matlab - # version 3.0 2006 fixed rotation matrix problem - # version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring - # version 5.0 8/8/11, simplified and restructured - # version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug - # - # New in version 2: - # - Generalised rotor thrust model - # - Rotor flapping model - # - Frame aerodynamic drag model - # - Frame aerodynamic surfaces model - # - Internal motor model - # - Much coolage - # - # Version 1.3 - # - Rigid body dynamic model - # - Rotor gyroscopic model - # - External motor model - # - # ARGUMENTS - # u Reference inputs 1x4 - # tele Enable telemetry (1 or 0) 1x1 - # crash Enable crash detection (1 or 0) 1x1 - # init Initial conditions 1x12 - # - # INPUTS - # u = [N S E W] - # NSEW motor commands 1x4 - # - # CONTINUOUS STATES - # z Position 3x1 (x,y,z) - # v Velocity 3x1 (xd,yd,zd) - # n Attitude 3x1 (Y,P,R) - # o Angular velocity 3x1 (wx,wy,wz) - # w Rotor angular velocity 4x1 - # - # Notes: z-axis downward so altitude is -z(3) - # - # CONTINUOUS STATE MATRIX MAPPING - # x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4] - # - # - # CONTINUOUS STATE EQUATIONS - # z` = v - # v` = g*e3 - (1/m)*T*R*e3 - # I*o` = -o X I*o + G + torq - # R = f(n) - # n` = inv(W)*o - # + # Flyer2dynamics lovingly coded by Paul Pounds, first coded 12/4/04 + # A simulation of idealised X-4 Flyer II flight dynamics. + # version 2.0 2005 modified to be compatible with latest version of Matlab + # version 3.0 2006 fixed rotation matrix problem + # version 4.0 4/2/10, fixed rotor flapping rotation matrix bug, mirroring + # version 5.0 8/8/11, simplified and restructured + # version 6.0 25/10/13, fixed rotation matrix/inverse wronskian definitions, flapping cross-product bug + # + # New in version 2: + # - Generalised rotor thrust model + # - Rotor flapping model + # - Frame aerodynamic drag model + # - Frame aerodynamic surfaces model + # - Internal motor model + # - Much coolage + # + # Version 1.3 + # - Rigid body dynamic model + # - Rotor gyroscopic model + # - External motor model + # + # ARGUMENTS + # u Reference inputs 1x4 + # tele Enable telemetry (1 or 0) 1x1 + # crash Enable crash detection (1 or 0) 1x1 + # init Initial conditions 1x12 + # + # INPUTS + # u = [N S E W] + # NSEW motor commands 1x4 + # + # CONTINUOUS STATES + # z Position 3x1 (x,y,z) + # v Velocity 3x1 (xd,yd,zd) + # n Attitude 3x1 (Y,P,R) + # o Angular velocity 3x1 (wx,wy,wz) + # w Rotor angular velocity 4x1 + # + # Notes: z-axis downward so altitude is -z(3) + # + # CONTINUOUS STATE MATRIX MAPPING + # x = [z1 z2 z3 n1 n2 n3 z1 z2 z3 o1 o2 o3 w1 w2 w3 w4] + # + # + # CONTINUOUS STATE EQUATIONS + # z` = v + # v` = g*e3 - (1/m)*T*R*e3 + # I*o` = -o X I*o + G + torq + # R = f(n) + # n` = inv(W)*o + # def __init__(self, model, groundcheck=True, speedcheck=True, x0=None, **blockargs): r""" Create a multi-rotor dynamic model block. @@ -92,82 +167,18 @@ def __init__(self, model, groundcheck=True, speedcheck=True, x0=None, **blockarg :type x0: array_like(6) or array_like(12), optional :param blockargs: |BlockOptions| :type blockargs: dict - :return: a MULTIROTOR block - :rtype: MultiRotor instance - - Dynamic model of a multi-rotor flying robot, includes rotor flapping. - - **Block ports** - - :input ω: a vector of input rotor speeds in (radians/sec). These are, - looking down, clockwise from the front rotor which lies on the x-axis. - - :output x: a dictionary signal with the following items: - - - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - - ``vb`` translational velocity in the world frame (metres/sec) - - ``w`` angular rates in the world frame as yaw-pitch-roll rates (radians/second) - - ``a1s`` longitudinal flapping angles (radians) - - ``b1s`` lateral flapping angles (radians) - - **Model parameters** - - The dynamic model is a dict with the following key/value pairs. - - =========== ========================================== - key description - =========== ========================================== - ``nrotors`` Number of rotors (even integer) - ``J`` Flyer rotational inertia matrix (3x3) - ``h`` Height of rotors above CoG - ``d`` Length of flyer arms - ``nb`` Number of blades per rotor - ``r`` Rotor radius - ``c`` Blade chord - ``e`` Flapping hinge offset - ``Mb`` Rotor blade mass - ``Mc`` Estimated hub clamp mass - ``ec`` Blade root clamp displacement - ``Ib`` Rotor blade rotational inertia - ``Ic`` Estimated root clamp inertia - ``mb`` Static blade moment - ``Ir`` Total rotor inertia - ``Ct`` Non-dim. thrust coefficient - ``Cq`` Non-dim. torque coefficient - ``sigma`` Rotor solidity ratio - ``thetat`` Blade tip angle - ``theta0`` Blade root angle - ``theta1`` Blade twist angle - ``theta75`` 3/4 blade angle - ``thetai`` Blade ideal root approximation - ``a`` Lift slope gradient - ``A`` Rotor disc area - ``gamma`` Lock number - =========== ========================================== - - .. note:: - - SI units are used. - - Based on MATLAB code developed by Pauline Pounds 2004. - - :References: - - Design, Construction and Control of a Large Quadrotor micro air vehicle. - P.Pounds, `PhD thesis `_ - Australian National University, 2007. - - :seealso: :class:`MultiRotorMixer` :class:`MultiRotorPlot` """ if model is None: - raise ValueError('no model provided') - + raise ValueError("no model provided") + super().__init__(nin=1, nout=1, **blockargs) - self.type = 'quadrotor' - + try: - nrotors = model['nrotors'] + nrotors = model["nrotors"] except KeyError: - raise RuntimeError('vehicle model does not contain nrotors') - assert nrotors % 2 == 0, 'Must have an even number of rotors' - + raise RuntimeError("vehicle model does not contain nrotors") + assert nrotors % 2 == 0, "Must have an even number of rotors" + self.nstates = 12 if x0 is None: x0 = np.zeros((self.nstates,)) @@ -184,120 +195,145 @@ def __init__(self, model, groundcheck=True, speedcheck=True, x0=None, **blockarg x0 = np.r_[x0[:3], np.zeros((9,))] elif len(x0) != self.nstates: raise ValueError("x0 is the wrong length") - + self._x0 = x0 - + self.nrotors = nrotors self.model = model - + self.groundcheck = groundcheck self.speedcheck = speedcheck - self.D = np.zeros((3,self.nrotors)) + self.D = np.zeros((3, self.nrotors)) self.theta = np.zeros((self.nrotors,)) for i in range(0, self.nrotors): theta = i / self.nrotors * 2 * pi # Di Rotor hub displacements (1x3) # first rotor is on the x-axis, clockwise order looking down from above - self.D[:,i] = np.r_[ model['d'] * cos(theta), model['d'] * sin(theta), model['h']] + self.D[:, i] = np.r_[ + model["d"] * cos(theta), model["d"] * sin(theta), model["h"] + ] self.theta[i] = theta - + self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,)) - - def output(self, t=None): - - model = self.model - + + def output(self, t, inports, x): + + model = self.model + # compute output vector as a function of state vector # z Position 3x1 (x,y,z) # v Velocity 3x1 (xd,yd,zd) # n Attitude 3x1 (Y,P,R) # o Angular velocity 3x1 (Yd,Pd,Rd) - - n = self._x[3:6] # RPY angles - phi = n[0] # yaw - the = n[1] # pitch - psi = n[2] # roll - + + n = x[3:6] # RPY angles + phi = n[0] # yaw + the = n[1] # pitch + psi = n[2] # roll + # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix - R = np.array([ - [cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)], - [cos(the) * sin(phi), sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi)], - [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)] - ]) - - #inverted Wronskian - iW = np.array([ - [0, sin(psi), cos(psi)], - [0, cos(psi) * cos(the), -sin(psi) * cos(the)], - [cos(the), sin(psi) * sin(the), cos(psi) * sin(the)] - ]) / cos(the) - + R = np.array( + [ + [ + cos(the) * cos(phi), + sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), + cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi), + ], + [ + cos(the) * sin(phi), + sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), + cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi), + ], + [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)], + ] + ) + + # inverted Wronskian + iW = np.array( + [ + [0, sin(psi), cos(psi)], + [0, cos(psi) * cos(the), -sin(psi) * cos(the)], + [cos(the), sin(psi) * sin(the), cos(psi) * sin(the)], + ] + ) / cos(the) + # return velocity in the body frame - vd = np.linalg.inv(R) @ self._x[6:9] # translational velocity mapped to body frame - rpyd = iW @ self._x[9:12] # RPY rates mapped to body frame + vd = np.linalg.inv(R) @ x[6:9] # translational velocity mapped to body frame + rpyd = iW @ x[9:12] # RPY rates mapped to body frame out = {} - out['x'] = self._x[0:6] - out['trans'] = np.r_[self._x[:3], vd] - out['rot'] = np.r_[self._x[3:6], rpyd] + out["x"] = x[0:6] + out["trans"] = np.r_[x[:3], vd] + out["rot"] = np.r_[x[3:6], rpyd] + out["vb"] = np.linalg.inv(R) @ x[6:9] # translational velocity mapped to body frame + out["w"] = iW @ x[9:12] # RPY rates mapped to body frame - out['a1s'] = self.a1s - out['b1s'] = self.b1s - out['X'] = np.r_[self._x[:6], vd, rpyd] + out["a1s"] = self.a1s + out["b1s"] = self.b1s + out["X"] = np.r_[x[:6], vd, rpyd] # sys = [ x(1:6); # inv(R)*x(7:9); % translational velocity mapped to body frame - # iW*x(10:12)]; - + # iW*x(10:12)]; + return [out] - - def deriv(self): - + + def deriv(self, t, inports, x): + model = self.model - + # Body-fixed frame references # ei Body fixed frame references 3x1 e3 = np.r_[0, 0, 1] - + # process inputs - w = self.inputs[0] + w = inports[0] if len(w) != self.nrotors: - raise RuntimeError('input vector wrong size') - + raise RuntimeError("input vector wrong size") + if self.speedcheck and np.any(w == 0): # might need to fix this, preculudes aerobatics :( # mu becomes NaN due to 0/0 - raise RuntimeError('quadrotor_dynamics: not defined for zero rotor speed'); - + raise RuntimeError("quadrotor_dynamics: not defined for zero rotor speed") + # EXTRACT STATES FROM X - z = self._x[0:3] # position in {W} - n = self._x[3:6] # RPY angles {W} - v = self._x[6:9] # velocity in {W} - o = self._x[9:12] # angular velocity in {W} - + z = x[0:3] # position in {W} + n = x[3:6] # RPY angles {W} + v = x[6:9] # velocity in {W} + o = x[9:12] # angular velocity in {W} + # PREPROCESS ROTATION AND WRONSKIAN MATRICIES - phi = n[0] # yaw - the = n[1] # pitch - psi = n[2] # roll + phi = n[0] # yaw + the = n[1] # pitch + psi = n[2] # roll - # phi = n(1); % yaw - # the = n(2); % pitch - # psi = n(3); % roll + # phi = n(1); % yaw + # the = n(2); % pitch + # psi = n(3); % roll - # rotz(phi)*roty(the)*rotx(psi) # BBF > Inertial rotation matrix - R = np.array([ - [cos(the)*cos(phi), sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi), cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi)], - [cos(the)*sin(phi), sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi), cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi)], - [-sin(the), sin(psi)*cos(the), cos(psi)*cos(the)] - ]) - + R = np.array( + [ + [ + cos(the) * cos(phi), + sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), + cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi), + ], + [ + cos(the) * sin(phi), + sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), + cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi), + ], + [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)], + ] + ) + # Manual Construction # Q3 = [cos(phi) -sin(phi) 0;sin(phi) cos(phi) 0;0 0 1]; % RZ %Rotation mappings # Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; % RY @@ -306,78 +342,107 @@ def deriv(self): # # RZ * RY * RX # inverted Wronskian - iW = np.array([ - [0, sin(psi), cos(psi)], - [0, cos(psi)*cos(the), -sin(psi)*cos(the)], - [cos(the), sin(psi)*sin(the), cos(psi)*sin(the)] - ]) / cos(the) - - - # % rotz(phi)*roty(the)*rotx(psi) - # R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix - # cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); - # -sin(the) sin(psi)*cos(the) cos(psi)*cos(the)]; - - # iW = [0 sin(psi) cos(psi); %inverted Wronskian - # 0 cos(psi)*cos(the) -sin(psi)*cos(the); - # cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the); + iW = np.array( + [ + [0, sin(psi), cos(psi)], + [0, cos(psi) * cos(the), -sin(psi) * cos(the)], + [cos(the), sin(psi) * sin(the), cos(psi) * sin(the)], + ] + ) / cos(the) + + # % rotz(phi)*roty(the)*rotx(psi) + # R = [cos(the)*cos(phi) sin(psi)*sin(the)*cos(phi)-cos(psi)*sin(phi) cos(psi)*sin(the)*cos(phi)+sin(psi)*sin(phi); %BBF > Inertial rotation matrix + # cos(the)*sin(phi) sin(psi)*sin(the)*sin(phi)+cos(psi)*cos(phi) cos(psi)*sin(the)*sin(phi)-sin(psi)*cos(phi); + # -sin(the) sin(psi)*cos(the) cos(psi)*cos(the)]; + + # iW = [0 sin(psi) cos(psi); %inverted Wronskian + # 0 cos(psi)*cos(the) -sin(psi)*cos(the); + # cos(the) sin(psi)*sin(the) cos(psi)*sin(the)] / cos(the); # ROTOR MODEL - T = np.zeros((3,4)) - Q = np.zeros((3,4)) - tau = np.zeros((3,4)) - + T = np.zeros((3, 4)) + Q = np.zeros((3, 4)) + tau = np.zeros((3, 4)) + a1s = self.a1s b1s = self.b1s - + for i in range(0, self.nrotors): # for each rotor - + # Relative motion - Vr = np.cross(o, self.D[:,i]) + v - mu = sqrt(np.sum(Vr[0:2]**2)) / (abs(w[i]) * model['r']) # Magnitude of mu, planar components - lc = Vr[2] / (abs(w[i]) * model['r']) # Non-dimensionalised normal inflow - li = mu # Non-dimensionalised induced velocity approximation + Vr = np.cross(o, self.D[:, i]) + v + mu = sqrt(np.sum(Vr[0:2] ** 2)) / ( + abs(w[i]) * model["r"] + ) # Magnitude of mu, planar components + lc = Vr[2] / (abs(w[i]) * model["r"]) # Non-dimensionalised normal inflow + li = mu # Non-dimensionalised induced velocity approximation alphas = atan2(lc, mu) - j = atan2(Vr[1], Vr[0]) # Sideslip azimuth relative to e1 (zero over nose) - J = np.array([ - [cos(j), -sin(j)], - [sin(j), cos(j)] - ]) # BBF > mu sideslip rotation matrix - + j = atan2(Vr[1], Vr[0]) # Sideslip azimuth relative to e1 (zero over nose) + J = np.array( + [[cos(j), -sin(j)], [sin(j), cos(j)]] + ) # BBF > mu sideslip rotation matrix + # Flapping - beta = np.array([ - [((8/3*model['theta0'] + 2 * model['theta1']) * mu - 2 * lc * mu) / (1 - mu**2 / 2)], # Longitudinal flapping - [0] # Lattitudinal flapping (note sign) - ]) - - # sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; - - beta = J.T @ beta; # Rotate the beta flapping angles to longitudinal and lateral coordinates. - a1s[i] = beta[0] - 16 / model['gamma'] / abs(w[i]) * o[1] - b1s[i] = beta[1] - 16 / model['gamma'] / abs(w[i]) * o[0] - + beta = np.array( + [ + [ + ( + (8 / 3 * model["theta0"] + 2 * model["theta1"]) * mu + - 2 * lc * mu + ) + / (1 - mu**2 / 2) + ], # Longitudinal flapping + [0], # Lattitudinal flapping (note sign) + ] + ) + + # sign(w) * (4/3)*((Ct/sigma)*(2*mu*gamma/3/a)/(1+3*e/2/r) + li)/(1+mu^2/2)]; + + beta = J.T @ beta + # Rotate the beta flapping angles to longitudinal and lateral coordinates. + a1s[i] = beta[0] - 16 / model["gamma"] / abs(w[i]) * o[1] + b1s[i] = beta[1] - 16 / model["gamma"] / abs(w[i]) * o[0] + # Forces and torques - + # Rotor thrust, linearised angle approximations - - T[:,i] = model['Ct'] * model['rho'] * model['A'] * model['r']**2 * w[i]**2 * \ - np.r_[-cos(b1s[i]) * sin(a1s[i]), sin(b1s[i]), -cos(a1s[i])*cos(b1s[i])] - + + T[:, i] = ( + model["Ct"] + * model["rho"] + * model["A"] + * model["r"] ** 2 + * w[i] ** 2 + * np.r_[ + -cos(b1s[i]) * sin(a1s[i]), sin(b1s[i]), -cos(a1s[i]) * cos(b1s[i]) + ] + ) + # Rotor drag torque - note that this preserves w[i] direction sign - - Q[:,i] = -model['Cq'] * model['rho'] * model['A'] * model['r']**3 * w[i] * abs(w[i]) * e3 - - tau[:,i] = np.cross(T[:,i], self.D[:,i]) # Torque due to rotor thrust - + + Q[:, i] = ( + -model["Cq"] + * model["rho"] + * model["A"] + * model["r"] ** 3 + * w[i] + * abs(w[i]) + * e3 + ) + + tau[:, i] = np.cross(T[:, i], self.D[:, i]) # Torque due to rotor thrust + # print(f"{tau=}") # print(f"{T=}") # RIGID BODY DYNAMIC MODEL dz = v dn = iW @ o - - dv = model['g'] * e3 + R @ np.sum(T, axis=1) / model['M'] - do = -np.linalg.inv(model['J']) @ (np.cross(o, model['J'] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1)) # row sum of torques - + + dv = model["g"] * e3 + R @ np.sum(T, axis=1) / model["M"] + do = -np.linalg.inv(model["J"]) @ ( + np.cross(o, model["J"] @ o) + np.sum(tau, axis=1) + np.sum(Q, axis=1) + ) # row sum of torques + # dv = quad.g*e3 + R*(1/quad.M)*sum(T,2); # do = inv(quad.J)*(cross(-o,quad.J*o) + sum(tau,2) + sum(Q,2)); %row sum of torques @@ -389,36 +454,77 @@ def deriv(self): # # stash the flapping information for plotting # self.a1s = a1s # self.b1s = b1s - + return np.r_[dz, dn, dv, do] # This is the state derivative vector + # ------------------------------------------------------------------------ # + class MultiRotorMixer(FunctionBlock): - """ + r""" :blockname:`MULTIROTORMIXER` - - .. table:: - :align: left - - +--------+------------+---------+ - | inputs | outputs | states | - +--------+------------+---------+ - | 4 | 1 | 0 | - +--------+------------+---------+ - | float | ndarray(4) | | - +--------+------------+---------+ + + Speed mixer for a multi-rotor flying vehicle. + + :inputs: 4 + :outputs: 1 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - float + - :math:`\tau_R`, roll torque + * - Input + - 1 + - float + - :math:`\tau_P`, pitch torque + * - Input + - 2 + - float + - :math:`\tau_Y`, yaw torque + * - Input + - 3 + - float + - :math:`T`, total thrust + * - Output + - 0 + - ndarray(N) + - :math:`\varpi`, rotor speeds + + This block converts airframe moments and total thrust into a 1D + array of rotor speeds which can be input to the ``MULTIROTOR`` block. + + The model is a dict with the following key/value pairs. + + =========== ========================================== + key description + =========== ========================================== + ``nrotors`` Number of rotors (even integer) + ``h`` Height of rotors above CoG + ``d`` Length of flyer arms + ``r`` Rotor radius + =========== ========================================== + + .. note:: Based on MATLAB code developed by Pauline Pounds 2004. + + :seealso: :class:`MultiRotor` :class:`MultiRotorPlot` """ - + nin = 4 nout = 1 - inlabels = ('𝛕r', '𝛕p', '𝛕y', 'T') - outlabels = ('ω',) + inlabels = ("𝛕r", "𝛕p", "𝛕y", "T") + outlabels = ("ω",) def __init__(self, model=None, wmax=1000, wmin=5, **blockargs): """ - Create a speed mixer block for a multi-rotor flying vehicle. - :param model: A dictionary of vehicle geometric and inertial properties :type model: dict :param maxw: maximum rotor speed in rad/s, defaults to 1000 @@ -427,60 +533,28 @@ def __init__(self, model=None, wmax=1000, wmin=5, **blockargs): :type minw: float :param blockargs: |BlockOptions| :type blockargs: dict - :return: a MULTIROTORMIXER block - :rtype: MultiRotorMixer instance - - This block converts airframe moments and total thrust into a 1D - array of rotor speeds which can be input to the MULTIROTOR block. - - **Block ports** - - :input 𝛕r: roll torque - :input 𝛕p: pitch torque - :input 𝛕y: yaw torque - :input T: total thrust - - :output ω: 1D array of rotor speeds - - **Model parameters** - - The model is a dict with the following key/value pairs. - - =========== ========================================== - key description - =========== ========================================== - ``nrotors`` Number of rotors (even integer) - ``h`` Height of rotors above CoG - ``d`` Length of flyer arms - ``r`` Rotor radius - =========== ========================================== - - .. note:: - - Based on MATLAB code developed by Pauline Pounds 2004. - - :seealso: :class:`MultiRotor` :class:`MultiRotorPlot` """ if model is None: - raise ValueError('no model provided') + raise ValueError("no model provided") super().__init__(**blockargs) - self.type = 'multirotormixer' + self.type = "multirotormixer" self.model = model - self.nrotors = model['nrotors'] + self.nrotors = model["nrotors"] self.minw = wmin**2 self.maxw = wmax**2 self.theta = np.arange(self.nrotors) / self.nrotors * 2 * np.pi - + # build the Nx4 mixer matrix M = [] s = [] for i in range(self.nrotors): # roll and pitch coupling column = np.r_[ - -sin(self.theta[i]) * model['d'] * model['b'], - cos(self.theta[i]) * model['d'] * model['b'], - model['k'] if (i % 2) == 0 else -model['k'] , - -model['b'] + -sin(self.theta[i]) * model["d"] * model["b"], + cos(self.theta[i]) * model["d"] * model["b"], + model["k"] if (i % 2) == 0 else -model["k"], + -model["b"], ] s.append(1 if (i % 2) == 0 else -1) M.append(column) @@ -488,8 +562,8 @@ def __init__(self, model=None, wmax=1000, wmin=5, **blockargs): self.Minv = np.linalg.inv(self.M) self.signs = np.array(s) - def output(self, t): - tau = self.inputs + def output(self, t, inports, x): + tau = inports # mix airframe force/torque to rotor thrusts w = self.Minv @ tau @@ -508,25 +582,64 @@ def output(self, t): # ------------------------------------------------------------------------ # + class MultiRotorPlot(GraphicsBlock): """ :blockname:`MULTIROTORPLOT` - - .. table:: - :align: left - - +--------+---------+---------+ - | inputs | outputs | states | - +--------+---------+---------+ - | 1 | 0 | 0 | - +--------+---------+---------+ - | dict | | | - +--------+---------+---------+ + + Displays/animates a multi-rotor flying vehicle. + + :inputs: 1 + :outputs: 0 + :states: 0 + + .. list-table:: + :header-rows: 1 + + * - Port type + - Port number + - Types + - Description + * - Input + - 0 + - dict + - :math:`\mathit{x}`, vehicle state + + Animate a multi-rotor flying vehicle using Matplotlib graphics. The + rotors are shown as circles and their orientation includes rotor + flapping which can be exagerated by ``flapscale``. + + .. figure:: ../figs/multirotorplot.png + :width: 500px + :alt: example of generated graphic + + Example of quad-rotor display. + + The input is a dictionary signal and the block requires the items: + + - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` + - ``a1s`` rotor flap angle + - ``b1s`` rotor flap angle + + The model is a dict with the following key/value pairs. + + =========== ========================================== + key description + =========== ========================================== + ``nrotors`` Number of rotors (even integer) + ``h`` Height of rotors above CoG + ``d`` Length of flyer arms + ``r`` Rotor radius + =========== ========================================== + + .. note:: Based on MATLAB code developed by Pauline Pounds 2004. + + :seealso: :class:`MultiRotor` :class:`MultiRotorMixer` """ - + nin = 1 nout = 0 - inlabels = ('x',) + inlabels = ("x",) # Based on code lovingly coded by Paul Pounds, first coded 17/4/02 # version 2 2004 added scaling and ground display @@ -550,12 +663,17 @@ class MultiRotorPlot(GraphicsBlock): nin = 1 nout = 0 - inlabels = ('x',) - - def __init__(self, model, scale=[-2, 2, -2, 2, 10], flapscale=1, projection='ortho', **blockargs): + inlabels = ("x",) + + def __init__( + self, + model, + scale=[-2, 2, -2, 2, 10], + flapscale=1, + projection="ortho", + **blockargs, + ): """ - Create a block that displays/animates a multi-rotor flying vehicle. - :param model: A dictionary of vehicle geometric and inertial properties :type model: dict :param scale: dimensions of workspace: xmin, xmax, ymin, ymax, zmin, zmax, defaults to [-2,2,-2,2,10] @@ -566,85 +684,58 @@ def __init__(self, model, scale=[-2, 2, -2, 2, 10], flapscale=1, projection='ort :type projection: str :param blockargs: |BlockOptions| :type blockargs: dict - :return: a MULTIROTORPLOT block - :rtype: MultiRotorPlot instance - - Animate a multi-rotor flying vehicle using Matplotlib graphics. The - rotors are shown as circles and their orientation includes rotor - flapping which can be exagerated by ``flapscale``. - - .. figure:: ../../figs/multirotorplot.png - :width: 500px - :alt: example of generated graphic - - Example of quad-rotor display. - - **Block ports** - - :input x: a dictionary signal that includes the item: - - - ``x`` pose in the world frame as :math:`[x, y, z, \theta_Y, \theta_P, \theta_R]` - - ``a1s`` rotor flap angle - - ``b1s`` rotor flap angle - - **Model parameters** - - The model is a dict with the following key/value pairs. - - =========== ========================================== - key description - =========== ========================================== - ``nrotors`` Number of rotors (even integer) - ``h`` Height of rotors above CoG - ``d`` Length of flyer arms - ``r`` Rotor radius - =========== ========================================== - - .. note:: - - Based on MATLAB code developed by Pauline Pounds 2004. - - :seealso: :class:`MultiRotor` :class:`MultiRotorMixer` """ if model is None: - raise ValueError('no model provided') + raise ValueError("no model provided") super().__init__(nin=1, **blockargs) - self.type = 'quadrotorplot' + self.type = "quadrotorplot" self.model = model self.scale = scale - self.nrotors = model['nrotors'] + self.nrotors = model["nrotors"] self.projection = projection self.flapscale = flapscale - def start(self, state): + def start(self, simstate): quad = self.model - + # vehicle dimensons - d = quad['d']; # Hub displacement from COG - r = quad['r']; # Rotor radius + d = quad["d"] + # Hub displacement from COG + r = quad["r"] + # Rotor radius - #C = np.zeros((3, self.nrotors)) ## WHERE USED? - self.D = np.zeros((3,self.nrotors)) + # C = np.zeros((3, self.nrotors)) ## WHERE USED? + self.D = np.zeros((3, self.nrotors)) for i in range(0, self.nrotors): theta = i / self.nrotors * 2 * pi # Di Rotor hub displacements (1x3) # first rotor is on the x-axis, clockwise order looking down from above - self.D[:,i] = np.r_[ quad['d'] * cos(theta), quad['d'] * sin(theta), quad['h']] - - #draw ground - self.fig = self.create_figure(state) + self.D[:, i] = np.r_[ + quad["d"] * cos(theta), quad["d"] * sin(theta), quad["h"] + ] + + # draw ground + self.fig = self.create_figure(simstate) # no axes in the figure, create a 3D axes - self.ax = self.fig.add_subplot(111, projection='3d', proj_type=self.projection) + self.ax = self.fig.add_subplot(111, projection="3d", proj_type=self.projection) # ax.set_aspect('equal') - self.ax.set_xlabel('X') - self.ax.set_ylabel('Y') - self.ax.set_zlabel('-Z (height above ground)') - - self.panel = self.ax.text2D(0.05, 0.95, '', transform=self.ax.transAxes, - fontsize=10, family='monospace', verticalalignment='top', - bbox=dict(boxstyle='round', facecolor='white', edgecolor='black')) + self.ax.set_xlabel("X") + self.ax.set_ylabel("Y") + self.ax.set_zlabel("-Z (height above ground)") + + self.panel = self.ax.text2D( + 0.05, + 0.95, + "", + transform=self.ax.transAxes, + fontsize=10, + family="monospace", + verticalalignment="top", + bbox=dict(boxstyle="round", facecolor="white", edgecolor="black"), + ) # TODO allow user to set maximum height of plot volume self.ax.set_xlim(self.scale[0], self.scale[1]) @@ -652,127 +743,154 @@ def start(self, state): self.ax.set_zlim(0, self.scale[4]) # plot the ground boundaries and the big cross - self.ax.plot([self.scale[0], self.scale[1]], [self.scale[2], self.scale[3]], [0, 0], 'b-') - self.ax.plot([self.scale[0], self.scale[1]], [self.scale[3], self.scale[2]], [0, 0], 'b-') + self.ax.plot( + [self.scale[0], self.scale[1]], [self.scale[2], self.scale[3]], [0, 0], "b-" + ) + self.ax.plot( + [self.scale[0], self.scale[1]], [self.scale[3], self.scale[2]], [0, 0], "b-" + ) self.ax.grid(True) - - self.shadow, = self.ax.plot([0, 0], [0, 0], 'k--') - self.groundmark, = self.ax.plot([0], [0], [0], 'kx') - + + (self.shadow,) = self.ax.plot([0, 0], [0, 0], "k--") + (self.groundmark,) = self.ax.plot([0], [0], [0], "kx") + self.arm = [] self.disk = [] for i in range(0, self.nrotors): - h, = self.ax.plot([0], [0], [0]) + (h,) = self.ax.plot([0], [0], [0]) self.arm.append(h) if i == 0: - color = 'b-' + color = "b-" else: - color = 'g-' - h, = self.ax.plot([0], [0], [0], color) + color = "g-" + (h,) = self.ax.plot([0], [0], [0], color) self.disk.append(h) - + self.a1s = np.zeros((self.nrotors,)) self.b1s = np.zeros((self.nrotors,)) - + plt.draw() plt.show(block=False) - super().start() - - def step(self, state): + super().start(simstate) + def step(self, t, inports): def plot3(h, x, y, z): h.set_data_3d(x, y, z) # h.set_data(x, y) # h.set_3d_properties(np.r_[z]) - - # READ STATE - z = self.inputs[0]['x'][0:3] - n = self.inputs[0]['x'][3:6] - + + # read UAV output "bus" + input = inports[0] + z = input["x"][0:3] + n = input["x"][3:6] + # TODO, check input dimensions, 12 or 12+2N, deal with flapping - - a1s = self.inputs[0]['a1s'] - b1s = self.inputs[0]['b1s'] - + + a1s = input["a1s"] + b1s = input["b1s"] + quad = self.model - + # vehicle dimensons - d = quad['d'] # Hub displacement from COG - r = quad['r'] # Rotor radius - + d = quad["d"] # Hub displacement from COG + r = quad["r"] # Rotor radius + # PREPROCESS ROTATION MATRIX - phi, the, psi = n # Euler angles + phi, the, psi = n # Euler angles # BBF > Inertial rotation matrix - R = np.array([ - [cos(the) * cos(phi), sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi)], - [cos(the) * sin(phi), sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), cos(psi) * sin(the) * sin(phi) - sin(psi)* cos(phi)], - [-sin(the), sin(psi)*cos(the), cos(psi) * cos(the)] - ]) - + R = np.array( + [ + [ + cos(the) * cos(phi), + sin(psi) * sin(the) * cos(phi) - cos(psi) * sin(phi), + cos(psi) * sin(the) * cos(phi) + sin(psi) * sin(phi), + ], + [ + cos(the) * sin(phi), + sin(psi) * sin(the) * sin(phi) + cos(psi) * cos(phi), + cos(psi) * sin(the) * sin(phi) - sin(psi) * cos(phi), + ], + [-sin(the), sin(psi) * cos(the), cos(psi) * cos(the)], + ] + ) + # Manual Construction - #Q3 = [cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1]; %Rotation mappings - #Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; - #Q1 = [1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)]; - #R = Q3*Q2*Q1; %Rotation matrix - + # Q3 = [cos(psi) -sin(psi) 0;sin(psi) cos(psi) 0;0 0 1]; %Rotation mappings + # Q2 = [cos(the) 0 sin(the);0 1 0;-sin(the) 0 cos(the)]; + # Q1 = [1 0 0;0 cos(phi) -sin(phi);0 sin(phi) cos(phi)]; + # R = Q3*Q2*Q1; %Rotation matrix + # CALCULATE FLYER TIP POSITONS USING COORDINATE FRAME ROTATION - F = np.array([ - [1, 0, 0], - [0, -1, 0], - [0, 0, -1] - ]) - + F = np.array([[1, 0, 0], [0, -1, 0], [0, 0, -1]]) + # Draw flyer rotors theta = np.linspace(0, 2 * pi, 20) circle = np.zeros((3, 20)) for j, t in enumerate(theta): - circle[:,j] = np.r_[r * sin(t), r * cos(t), 0] - + circle[:, j] = np.r_[r * sin(t), r * cos(t), 0] + hub = np.zeros((3, self.nrotors)) tippath = np.zeros((3, 20, self.nrotors)) for i in range(0, self.nrotors): - hub[:,i] = F @ (z + R @ self.D[:,i]) # points in the inertial frame - - q = self.flapscale # Flapping angle scaling for output display - makes it easier to see what flapping is occurring + hub[:, i] = F @ (z + R @ self.D[:, i]) # points in the inertial frame + + q = ( + self.flapscale + ) # Flapping angle scaling for output display - makes it easier to see what flapping is occurring # Rotor -> Plot frame - Rr = np.array([ - [cos(q * a1s[i]), sin(q * b1s[i]) * sin(q * a1s[i]), cos(q * b1s[i]) * sin(q * a1s[i])], - [0, cos(q * b1s[i]), -sin(q*b1s[i])], - [-sin(q * a1s[i]), sin(q * b1s[i]) * cos(q * a1s[i]), cos(q * b1s[i]) * cos(q * a1s[i])] - ]) - - tippath[:,:,i] = F @ R @ Rr @ circle - plot3(self.disk[i], hub[0,i] + tippath[0,:,i], hub[1,i] + tippath[1,:,i], hub[2,i] + tippath[2,:,i]) + Rr = np.array( + [ + [ + cos(q * a1s[i]), + sin(q * b1s[i]) * sin(q * a1s[i]), + cos(q * b1s[i]) * sin(q * a1s[i]), + ], + [0, cos(q * b1s[i]), -sin(q * b1s[i])], + [ + -sin(q * a1s[i]), + sin(q * b1s[i]) * cos(q * a1s[i]), + cos(q * b1s[i]) * cos(q * a1s[i]), + ], + ] + ) + + tippath[:, :, i] = F @ R @ Rr @ circle + plot3( + self.disk[i], + hub[0, i] + tippath[0, :, i], + hub[1, i] + tippath[1, :, i], + hub[2, i] + tippath[2, :, i], + ) # Draw flyer hub0 = F @ z # centre of vehicle for i in range(0, self.nrotors): # line from hub to centre plot3([hub(1,N) hub(1,S)],[hub(2,N) hub(2,S)],[hub(3,N) hub(3,S)],'-b') - plot3(self.arm[i], [hub[0,i], hub0[0]], [hub[1,i], hub0[1]], [hub[2,i], hub0[2]]) - + plot3( + self.arm[i], + [hub[0, i], hub0[0]], + [hub[1, i], hub0[1]], + [hub[2, i], hub0[2]], + ) + # plot a circle at the hub itself - #plot3([hub(1,i)],[hub(2,i)],[hub(3,i)],'o') + # plot3([hub(1,i)],[hub(2,i)],[hub(3,i)],'o') # plot the vehicle's centroid on the ground plane plot3(self.shadow, [z[0], 0], [-z[1], 0], [0, 0]) - plot3(self.groundmark, z[0], -z[1], 0) + plot3(self.groundmark, [z[0]], [-z[1]], [0]) - textstr = f"t={state.t: .2f}\nh={z[2]: .2f}\nγ={n[0]: .2f}" + textstr = f"t={t: .2f}\nh={z[2]: .2f}\nγ={n[0]: .2f}" self.panel.set_text(textstr) - super().step(state=state) + super().step(t, inports) - def done(self, block=False, **kwargs): - if self.bd.options.graphics: - plt.show(block=block) - - super().done() if __name__ == "__main__": - from bdsim.blocks.quad_model import quadrotor + from quad_model import quadrotor # m = MultiRotorMixer(model=quadrotor) # print(m.M) @@ -794,29 +912,28 @@ def done(self, block=False, **kwargs): m = MultiRotor(model=quadrotor) - def show(w): print() - print(w[0]**2 + w[2]**2 - w[1]**2 - w[3]**2) + print(w[0] ** 2 + w[2] ** 2 - w[1] ** 2 - w[3] ** 2) print(w) - m._x = np.r_[0.0, 0, -4, 0, 0, 0, 0, 0, 0, 0, 0, 0] + x = np.r_[0.0, 0, -4, 0, 0, 0, 0, 0, 0, 0, 0, 0] + inputs = [np.r_[w]] - m.inputs = [np.r_[w]] - dx = m.deriv() + dx = m.deriv(0.0, inputs, x) - print('zdd', dx[8]) - print('wd', dx[9:12]) + print("zdd", dx[8]) + print("wd", dx[9:12]) - m._x = dx - x = m.output()[0]['X'] - print('zd', x[8]) - print('ypr_dot', x[9:12]) + x = dx + x = m.output(0.0, inputs, x)[0]["X"] + print("zd", x[8]) + print("ypr_dot", x[9:12]) show([800.0, -800, 800, -800]) # tau_y pitch - z = np.sqrt((900**2 + 700**2) /2) + z = np.sqrt((900**2 + 700**2) / 2) show([900.0, -z, 700, -z]) show([700.0, -z, 900, -z]) @@ -824,7 +941,5 @@ def show(w): show([z, -900, z, -700]) show([z, -700, z, -900]) - # tau_z roll show([900, -800, 900, -800]) - diff --git a/roboticstoolbox/core/fknm.cpp b/roboticstoolbox/core/fknm.cpp index 2f7e53368..59de16809 100644 --- a/roboticstoolbox/core/fknm.cpp +++ b/roboticstoolbox/core/fknm.cpp @@ -18,8 +18,13 @@ #include #include #include +#include static PyMethodDef fknmMethods[] = { + {"Angle_Axis", + (PyCFunction)Angle_Axis, + METH_VARARGS, + "Link"}, {"IK_GN_c", (PyCFunction)IK_GN_c, METH_VARARGS, @@ -28,18 +33,18 @@ static PyMethodDef fknmMethods[] = { (PyCFunction)IK_NR_c, METH_VARARGS, "Link"}, - {"IK_LM_Chan_c", - (PyCFunction)IK_LM_Chan_c, - METH_VARARGS, - "Link"}, - {"IK_LM_Wampler_c", - (PyCFunction)IK_LM_Wampler_c, - METH_VARARGS, - "Link"}, - {"IK_LM_Sugihara_c", - (PyCFunction)IK_LM_Sugihara_c, + {"IK_LM_c", + (PyCFunction)IK_LM_c, METH_VARARGS, "Link"}, + // {"IK_LM_Wampler_c", + // (PyCFunction)IK_LM_Wampler_c, + // METH_VARARGS, + // "Link"}, + // {"IK_LM_Sugihara_c", + // (PyCFunction)IK_LM_Sugihara_c, + // METH_VARARGS, + // "Link"}, {"Robot_link_T", (PyCFunction)Robot_link_T, METH_VARARGS, @@ -104,122 +109,59 @@ PyMODINIT_FUNC PyInit_fknm(void) extern "C" { - static PyObject *IK_GN_c(PyObject *self, PyObject *args) + static PyObject *Angle_Axis(PyObject *self, PyObject *args) { - ETS *ets; - npy_float64 *np_Tep, *np_ret, *np_q0, *np_we; - PyArrayObject *py_np_Tep; - PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0, *py_we, *py_np_we; - PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E; - npy_intp dim[1] = {1}; - int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl, use_pinv; - double tol, E, pinv_damping; - - int it = 0, search = 1, solution = 0; + npy_float64 *np_Te, *np_Tep, *np_ret; + PyObject *py_Te, *py_Tep; + PyArrayObject *py_np_Te, *py_np_Tep; if (!PyArg_ParseTuple( - args, "OOOiidiOid", - &py_ets, - &py_Tep, - &py_q0, - &ilimit, - &slimit, - &tol, - &reject_jl, - &py_we, - &use_pinv, - &pinv_damping)) + args, "OO", + &py_Te, + &py_Tep)) return NULL; - if (!_check_array_type(py_Tep)) - return NULL; + // Inputs can be: + // Te, Tep can be SE3s or 4x4 numpy array - // Extract the ETS object from the python object - if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS"))) + // Make sure Te, Tep is number array + // Cast to numpy array + // Get data out + if (!_check_array_type(py_Te)) return NULL; + py_np_Te = (PyArrayObject *)PyArray_FROMANY(py_Te, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT); + np_Te = (npy_float64 *)PyArray_DATA(py_np_Te); - // Assign empty q0 and we - MapVectorX q0(NULL, 0); - MapVectorX we(NULL, 0); - - // Check if q0 is None - if (py_q0 != Py_None) - { - // Make sure q is number array - // Cast to numpy array - // Get data out - if (!_check_array_type(py_q0)) - return NULL; - q0_used = 1; - py_np_q0 = (PyObject *)PyArray_FROMANY(py_q0, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS); - np_q0 = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q0); - // MapVectorX q0(np_q0, ets->n); - new (&q0) MapVectorX(np_q0, ets->n); - } - - // Check if we is None - if (py_we != Py_None) - { - // Make sure we is number array - // Cast to numpy array - // Get data out - if (!_check_array_type(py_we)) - return NULL; - we_used = 1; - py_np_we = (PyObject *)PyArray_FROMANY(py_we, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS); - np_we = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_we); - new (&we) MapVectorX(np_we, 6); - } - - // Set the dimension of the returned array to match the number of joints - dim[0] = ets->n; - + if (!_check_array_type(py_Tep)) + return NULL; py_np_Tep = (PyArrayObject *)PyArray_FROMANY(py_Tep, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT); np_Tep = (npy_float64 *)PyArray_DATA(py_np_Tep); + // Make our empty error vector + npy_intp dims[1] = {6}; + PyObject *py_ret = PyArray_EMPTY(1, dims, NPY_DOUBLE, 0); + np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret); + MapVectorX ret(np_ret, 6); + + // Get eigen matrices // Tep in row major from Python MapMatrix4dr row_Tep(np_Tep); + MapMatrix4dr row_Te(np_Te); // Convert to col major here Matrix4dc Tep = row_Tep; + Matrix4dc Te = row_Te; - py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0); - np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret); - MapVectorX ret(np_ret, ets->n); - - _IK_GN(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, we, use_pinv, pinv_damping); - - // Free the memory - Py_DECREF(py_np_Tep); - - if (q0_used) - { - Py_DECREF(py_np_q0); - } - - if (we_used) - { - Py_DECREF(py_np_we); - } + // Get map matrix + MapMatrix4dc map_Te(&Te(0)); - // Build the return tuple - py_it = Py_BuildValue("i", it); - py_search = Py_BuildValue("i", search); - py_solution = Py_BuildValue("i", solution); - py_E = Py_BuildValue("d", E); - - py_tup = PyTuple_Pack(5, py_ret, py_solution, py_it, py_search, py_E); - - Py_DECREF(py_it); - Py_DECREF(py_search); - Py_DECREF(py_solution); - Py_DECREF(py_E); - Py_DECREF(py_ret); + // Do the job + _angle_axis(map_Te, Tep, ret); - return py_tup; + return py_ret; } - static PyObject *IK_NR_c(PyObject *self, PyObject *args) + static PyObject *IK_GN_c(PyObject *self, PyObject *args) { ETS *ets; npy_float64 *np_Tep, *np_ret, *np_q0, *np_we; @@ -302,7 +244,7 @@ extern "C" np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret); MapVectorX ret(np_ret, ets->n); - _IK_NR(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, we, use_pinv, pinv_damping); + _IK_GN(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, we, use_pinv, pinv_damping); // Free the memory Py_DECREF(py_np_Tep); @@ -334,7 +276,7 @@ extern "C" return py_tup; } - static PyObject *IK_LM_Chan_c(PyObject *self, PyObject *args) + static PyObject *IK_NR_c(PyObject *self, PyObject *args) { ETS *ets; npy_float64 *np_Tep, *np_ret, *np_q0, *np_we; @@ -342,13 +284,13 @@ extern "C" PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0, *py_we, *py_np_we; PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E; npy_intp dim[1] = {1}; - int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl; - double tol, E, lambda; + int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl, use_pinv; + double tol, E, pinv_damping; int it = 0, search = 1, solution = 0; if (!PyArg_ParseTuple( - args, "OOOiidiOd", + args, "OOOiidiOid", &py_ets, &py_Tep, &py_q0, @@ -357,7 +299,8 @@ extern "C" &tol, &reject_jl, &py_we, - &lambda)) + &use_pinv, + &pinv_damping)) return NULL; if (!_check_array_type(py_Tep)) @@ -416,10 +359,7 @@ extern "C" np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret); MapVectorX ret(np_ret, ets->n); - // std::cout << Tep << std::endl; - // std::cout << ret << std::endl; - - _IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); + _IK_NR(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, we, use_pinv, pinv_damping); // Free the memory Py_DECREF(py_np_Tep); @@ -451,7 +391,7 @@ extern "C" return py_tup; } - static PyObject *IK_LM_Wampler_c(PyObject *self, PyObject *args) + static PyObject *IK_LM_c(PyObject *self, PyObject *args) { ETS *ets; npy_float64 *np_Tep, *np_ret, *np_q0, *np_we; @@ -461,11 +401,12 @@ extern "C" npy_intp dim[1] = {1}; int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl; double tol, E, lambda; + const char *method; int it = 0, search = 1, solution = 0; if (!PyArg_ParseTuple( - args, "OOOiidiOd", + args, "OOOiidiOds", &py_ets, &py_Tep, &py_q0, @@ -474,7 +415,8 @@ extern "C" &tol, &reject_jl, &py_we, - &lambda)) + &lambda, + &method)) return NULL; if (!_check_array_type(py_Tep)) @@ -536,125 +478,22 @@ extern "C" // std::cout << Tep << std::endl; // std::cout << ret << std::endl; - _IK_LM_Wampler(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); - - // Free the memory - Py_DECREF(py_np_Tep); - - if (q0_used) - { - Py_DECREF(py_np_q0); - } - - if (we_used) + if (method[0] == 's') { - Py_DECREF(py_np_we); + // std::cout << "sugi" << std::endl; + _IK_LM_Sugihara(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); } - - // Build the return tuple - py_it = Py_BuildValue("i", it); - py_search = Py_BuildValue("i", search); - py_solution = Py_BuildValue("i", solution); - py_E = Py_BuildValue("d", E); - - py_tup = PyTuple_Pack(5, py_ret, py_solution, py_it, py_search, py_E); - - Py_DECREF(py_it); - Py_DECREF(py_search); - Py_DECREF(py_solution); - Py_DECREF(py_E); - Py_DECREF(py_ret); - - return py_tup; - } - - static PyObject *IK_LM_Sugihara_c(PyObject *self, PyObject *args) - { - ETS *ets; - npy_float64 *np_Tep, *np_ret, *np_q0, *np_we; - PyArrayObject *py_np_Tep; - PyObject *py_ets, *py_ret, *py_Tep, *py_q0, *py_np_q0, *py_we, *py_np_we; - PyObject *py_tup, *py_it, *py_search, *py_solution, *py_E; - npy_intp dim[1] = {1}; - int ilimit, slimit, q0_used = 0, we_used = 0, reject_jl; - double tol, E, lambda; - - int it = 0, search = 1, solution = 0; - - if (!PyArg_ParseTuple( - args, "OOOiidiOd", - &py_ets, - &py_Tep, - &py_q0, - &ilimit, - &slimit, - &tol, - &reject_jl, - &py_we, - &lambda)) - return NULL; - - if (!_check_array_type(py_Tep)) - return NULL; - - // Extract the ETS object from the python object - if (!(ets = (ETS *)PyCapsule_GetPointer(py_ets, "ETS"))) - return NULL; - - // Assign empty q0 and we - MapVectorX q0(NULL, 0); - MapVectorX we(NULL, 0); - - // Check if q0 is None - if (py_q0 != Py_None) + else if (method[0] == 'w') { - // Make sure q is number array - // Cast to numpy array - // Get data out - if (!_check_array_type(py_q0)) - return NULL; - q0_used = 1; - py_np_q0 = (PyObject *)PyArray_FROMANY(py_q0, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS); - np_q0 = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q0); - // MapVectorX q0(np_q0, ets->n); - new (&q0) MapVectorX(np_q0, ets->n); + // std::cout << "wampl" << std::endl; + _IK_LM_Wampler(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); } - - // Check if we is None - if (py_we != Py_None) + else { - // Make sure we is number array - // Cast to numpy array - // Get data out - if (!_check_array_type(py_we)) - return NULL; - we_used = 1; - py_np_we = (PyObject *)PyArray_FROMANY(py_we, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS); - np_we = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_we); - new (&we) MapVectorX(np_we, 6); + // std::cout << "chan" << std::endl; + _IK_LM_Chan(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); } - // Set the dimension of the returned array to match the number of joints - dim[0] = ets->n; - - py_np_Tep = (PyArrayObject *)PyArray_FROMANY(py_Tep, NPY_DOUBLE, 1, 2, NPY_ARRAY_DEFAULT); - np_Tep = (npy_float64 *)PyArray_DATA(py_np_Tep); - - // Tep in row major from Python - MapMatrix4dr row_Tep(np_Tep); - - // Convert to col major here - Matrix4dc Tep = row_Tep; - - py_ret = PyArray_EMPTY(1, dim, NPY_DOUBLE, 0); - np_ret = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_ret); - MapVectorX ret(np_ret, ets->n); - - // std::cout << Tep << std::endl; - // std::cout << ret << std::endl; - - _IK_LM_Sugihara(ets, Tep, q0, ilimit, slimit, tol, reject_jl, ret, &it, &search, &solution, &E, lambda, we); - // Free the memory Py_DECREF(py_np_Tep); @@ -1116,9 +955,11 @@ extern "C" // Get data out if (!_check_array_type(py_q)) return NULL; - py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_F_CONTIGUOUS); + py_np_q = (PyObject *)PyArray_FROMANY(py_q, NPY_DOUBLE, 1, 2, NPY_ARRAY_C_CONTIGUOUS); q = (npy_float64 *)PyArray_DATA((PyArrayObject *)py_np_q); + // std::cout << "q: " << q[0] << ", " << q[1] << ", " << q[2] << ", " << q[3] << ", " << q[4] << ", " << q[5] << std::endl; + // Check the dimesnions of q q_nd = PyArray_NDIM((PyArrayObject *)py_np_q); q_shape = PyArray_SHAPE((PyArrayObject *)py_np_q); @@ -1153,8 +994,15 @@ extern "C" } else { + // if using a trajectory, make a duplicate of ret as we will need to + // extreme reshape it due to Fortran ordering + // Fortran ordering of 3D array wants (4, 4, n) while numpy looping + // typically likes to have (n, 4, 4) + + // therefore we make the returned python array (n, 4, 4) and row-major + // and later on we transpose each (4, 4) component dim3[0] = trajn; - py_ret = PyArray_EMPTY(3, dim3, NPY_DOUBLE, 1); + py_ret = PyArray_EMPTY(3, dim3, NPY_DOUBLE, 0); } // Get numpy reference to return array @@ -1189,11 +1037,18 @@ extern "C" // Do the actual job for (int i = 0; i < trajn; i++) { - // Get pointers to the new section of return array and q array retp = ret + (4 * 4 * i); + MapMatrix4dc e_retp(retp); qp = q + (n * i); _ETS_fkine(ets, qp, base, tool, e_retp); + + // Transpose if we have a trajectory + // as the returned trajectory is row-major + if (trajn > 1) + { + e_retp.transposeInPlace(); + } } // Free memory @@ -1269,8 +1124,9 @@ extern "C" et = (ET *)PyMem_RawMalloc(sizeof(ET)); - if (!PyArg_ParseTuple(args, "OiiiiO!O!", + if (!PyArg_ParseTuple(args, "OiiiiiO!O!", &py_et, + &et->isstaticsym, &isjoint, &isflip, &jindex, @@ -1333,7 +1189,8 @@ extern "C" et = (ET *)PyMem_RawMalloc(sizeof(ET)); - if (!PyArg_ParseTuple(args, "iiiiO!O!", + if (!PyArg_ParseTuple(args, "iiiiiO!O!", + &et->isstaticsym, &et->isjoint, &et->isflip, &et->jindex, @@ -1397,6 +1254,12 @@ extern "C" if (!(et = (ET *)PyCapsule_GetPointer(py_et, "ET"))) return NULL; + if (et->isstaticsym) + { + PyErr_SetString(PyExc_TypeError, "Symbolic value"); + return NULL; + } + if (py_eta != Py_None) { if (PyFloat_Check(py_eta)) diff --git a/roboticstoolbox/core/fknm.h b/roboticstoolbox/core/fknm.h index 672f518b3..eb272e13a 100644 --- a/roboticstoolbox/core/fknm.h +++ b/roboticstoolbox/core/fknm.h @@ -16,11 +16,14 @@ extern "C" #endif /* __cplusplus */ // forward defines + static PyObject *Angle_Axis(PyObject *self, PyObject *args); + static PyObject *IK_GN_c(PyObject *self, PyObject *args); static PyObject *IK_NR_c(PyObject *self, PyObject *args); - static PyObject *IK_LM_Chan_c(PyObject *self, PyObject *args); - static PyObject *IK_LM_Wampler_c(PyObject *self, PyObject *args); - static PyObject *IK_LM_Sugihara_c(PyObject *self, PyObject *args); + static PyObject *IK_LM_c(PyObject *self, PyObject *args); + // static PyObject *IK_LM_Chan_c(PyObject *self, PyObject *args); + // static PyObject *IK_LM_Wampler_c(PyObject *self, PyObject *args); + // static PyObject *IK_LM_Sugihara_c(PyObject *self, PyObject *args); static PyObject *Robot_link_T(PyObject *self, PyObject *args); diff --git a/roboticstoolbox/core/ik.cpp b/roboticstoolbox/core/ik.cpp index a2ecfd9c5..103931977 100644 --- a/roboticstoolbox/core/ik.cpp +++ b/roboticstoolbox/core/ik.cpp @@ -94,7 +94,7 @@ extern "C" // wrap q to +- pi for (int i = 0; i < ets->n; i++) { - q(i) = std::fmod(q(i), PI); + q(i) = std::fmod(q(i) + PI, PI_x2) - PI; } // Check for joint limit violation @@ -228,7 +228,7 @@ extern "C" // wrap q to +- pi for (int i = 0; i < ets->n; i++) { - q(i) = std::fmod(q(i), PI); + q(i) = std::fmod(q(i) + PI, PI_x2) - PI; } // Check for joint limit violation @@ -293,6 +293,9 @@ extern "C" { int iter = 1; + // std::cout << Tep << "\n"; + // std::cout << std::endl; + double *np_Te = (double *)PyMem_RawCalloc(16, sizeof(double)); MapMatrix4dc Te(np_Te); @@ -351,7 +354,7 @@ extern "C" // wrap q to +- pi for (int i = 0; i < ets->n; i++) { - q(i) = std::fmod(q(i), PI); + q(i) = std::fmod(q(i) + PI, PI_x2) - PI; } // Check for joint limit violation @@ -465,7 +468,7 @@ extern "C" // wrap q to +- pi for (int i = 0; i < ets->n; i++) { - q(i) = std::fmod(q(i), PI); + q(i) = std::fmod(q(i) + PI, PI_x2) - PI; } // Check for joint limit violation @@ -577,7 +580,7 @@ extern "C" // wrap q to +- pi for (int i = 0; i < ets->n; i++) { - q(i) = std::fmod(q(i), PI); + q(i) = std::fmod(q(i) + PI, PI_x2) - PI; } // Check for joint limit violation diff --git a/roboticstoolbox/core/methods.cpp b/roboticstoolbox/core/methods.cpp index 858b15d2e..2fbdb2544 100644 --- a/roboticstoolbox/core/methods.cpp +++ b/roboticstoolbox/core/methods.cpp @@ -118,6 +118,7 @@ extern "C" Matrix4dc temp; Matrix4dc ret; int j = ets->n - 1; + double sign = 1.0; if (tool != NULL) { @@ -132,35 +133,66 @@ extern "C" if (et->isjoint) { + if (et->axis == 0) { tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3); tJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2)); + + if (et->isflip) + { + tJ(Eigen::seq(0, 5), j) = -tJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 1) { tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3); tJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2)); + + if (et->isflip) + { + tJ(Eigen::seq(0, 5), j) = -tJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 2) { tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3); tJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2)); + + if (et->isflip) + { + tJ(Eigen::seq(0, 5), j) = -tJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 3) { tJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)); tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + tJ(Eigen::seq(0, 2), j) = -tJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 4) { tJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)); tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + tJ(Eigen::seq(0, 2), j) = -tJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 5) { tJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)); tJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + tJ(Eigen::seq(0, 2), j) = -tJ(Eigen::seq(0, 5), j); + } } _ET_T(et, &ret(0), q[et->jindex]); @@ -212,31 +244,61 @@ extern "C" { eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)) * U(1, 3) - U(1, Eigen::seq(0, 2)) * U(2, 3); eJ(Eigen::seq(3, 5), j) = U(0, Eigen::seq(0, 2)); + + if (et->isflip) + { + eJ(Eigen::seq(0, 5), j) = -eJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 1) { eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)) * U(2, 3) - U(2, Eigen::seq(0, 2)) * U(0, 3); eJ(Eigen::seq(3, 5), j) = U(1, Eigen::seq(0, 2)); + + if (et->isflip) + { + eJ(Eigen::seq(0, 5), j) = -eJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 2) { eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)) * U(0, 3) - U(0, Eigen::seq(0, 2)) * U(1, 3); eJ(Eigen::seq(3, 5), j) = U(2, Eigen::seq(0, 2)); + + if (et->isflip) + { + eJ(Eigen::seq(0, 5), j) = -eJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 3) { eJ(Eigen::seq(0, 2), j) = U(0, Eigen::seq(0, 2)); eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + eJ(Eigen::seq(0, 2), j) = -eJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 4) { eJ(Eigen::seq(0, 2), j) = U(1, Eigen::seq(0, 2)); eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + eJ(Eigen::seq(0, 2), j) = -eJ(Eigen::seq(0, 5), j); + } } else if (et->axis == 5) { eJ(Eigen::seq(0, 2), j) = U(2, Eigen::seq(0, 2)); eJ(Eigen::seq(3, 5), j) = Eigen::Vector3d::Zero(); + + if (et->isflip) + { + eJ(Eigen::seq(0, 2), j) = -eJ(Eigen::seq(0, 5), j); + } } _ET_T(et, &ret(0), q[et->jindex]); diff --git a/roboticstoolbox/core/structs.h b/roboticstoolbox/core/structs.h index 9dfe2efee..37f923a93 100644 --- a/roboticstoolbox/core/structs.h +++ b/roboticstoolbox/core/structs.h @@ -32,7 +32,7 @@ extern "C" int m; // While this information is stored in the ET's - // Its much faster for IK to cahce it here + // Its much faster for IK to cache it here double *qlim_l; double *qlim_h; double *q_range2; @@ -40,6 +40,7 @@ extern "C" struct ET { + int isstaticsym; /* this ET is static and has a symbolic value */ int isjoint; int isflip; int jindex; diff --git a/roboticstoolbox/examples/eigdemo.py b/roboticstoolbox/examples/eigdemo.py index 4dfb183a8..71b863a59 100644 --- a/roboticstoolbox/examples/eigdemo.py +++ b/roboticstoolbox/examples/eigdemo.py @@ -39,15 +39,20 @@ def eigdemo(A): plt.ylim(-s, s) ax.set_aspect('equal') - l1, = plt.plot([0, 0], [0, 0], color='r', linewidth=1.5) # input vector - l2, = plt.plot([0, 0], [0, 0], color='b', linewidth=1.5) # transformed vector + l1, = plt.plot([0, 0], [0, 0], color='r', linewidth=3) # input vector + l2, = plt.plot([0, 0], [0, 0], color='b', linewidth=3) # transformed vector - plt.legend(['$x$', r'${\bf A} x$']) + plt.plot([0, x[0, 0]], [0, x[1, 0]], color='k', linewidth=1) + plt.plot([0, x[0, 1]], [0, x[1, 1]], color='k', linewidth=1) + + plt.legend(['$x$', r'${\bf A} x$', r'$x_1$', r'$x_2$']) + + print("\nto exit: type q, or close the window") def animate(theta): - x = np.r_[cos(theta), sin(theta)] + x = np.r_[cos(-theta), sin(-theta)] y = A @ x l1.set_xdata([0, x[0]]) @@ -68,10 +73,13 @@ def animate(theta): def main(): def help(): - print("eigdemo uses default matrix [1 2; 3 4]") - print("eigdemo a b c d uses matrix [a b; c d]") + print("eigdemo # uses default matrix [1 2; 3 4]") + print("eigdemo a b c d # uses matrix [a b; c d]") sys.exit(0) - + + if sys.argv in ("-h", "--help", "help"): + help() + if len(sys.argv) == 5: try: @@ -80,15 +88,16 @@ def help(): except: help() - elif sys.argv == 1: + elif len(sys.argv) == 1: A = np.array([ [1, 2], [3, 3] ]) - + else: help() + eigdemo(A) if __name__ == "__main__": diff --git a/roboticstoolbox/robot/ikine_evaluate.py b/roboticstoolbox/examples/ikine_evaluate.py similarity index 100% rename from roboticstoolbox/robot/ikine_evaluate.py rename to roboticstoolbox/examples/ikine_evaluate.py diff --git a/roboticstoolbox/robot/ikine_evaluate2.py b/roboticstoolbox/examples/ikine_evaluate2.py similarity index 100% rename from roboticstoolbox/robot/ikine_evaluate2.py rename to roboticstoolbox/examples/ikine_evaluate2.py diff --git a/roboticstoolbox/examples/puma_fdyn.py b/roboticstoolbox/examples/puma_fdyn.py index b8daead5e..350c80a78 100644 --- a/roboticstoolbox/examples/puma_fdyn.py +++ b/roboticstoolbox/examples/puma_fdyn.py @@ -1,4 +1,5 @@ import roboticstoolbox as rtb +import numpy as np # load a model with inertial parameters p560 = rtb.models.DH.Puma560() @@ -7,13 +8,13 @@ p560 = p560.nofriction() # print the kinematic & dynamic parameters -p560.printdyn() +p560.dynamics() # simulate motion over 5s with zero torque input -d = p560.fdyn(5, p560.qr, dt=0.05) +d = p560.fdyn(5, p560.qr, lambda robot, t, q, qd: np.zeros((robot.n,)), dt=0.05) # show the joint angle trajectory -rtb.tools.trajectory.qplot(d.q) +rtb.tools.xplot(d.q) # animate it p560.plot(d.q) # movie='falling_puma.gif') diff --git a/roboticstoolbox/examples/test.py b/roboticstoolbox/examples/test.py index ec5166538..1ef320ee8 100644 --- a/roboticstoolbox/examples/test.py +++ b/roboticstoolbox/examples/test.py @@ -1,15 +1,25 @@ import numpy as np import roboticstoolbox as rtb import spatialmath as sm -import fknm +q0 = np.array( + [ + -1.66441371, + -1.20998727, + 1.04248366, + -2.10222463, + 1.05097407, + 1.41173279, + 0.0053529, + ] +) -r = rtb.models.Panda() -r.q = r.qr +tol = 1e-6 -Tep = r.fkine(r.q) * sm.SE3.Rx(0.1) * sm.SE3.Tx(0.2) +panda = rtb.models.Panda().ets() -a = fknm.IK(r.ets()._fknm, Tep) +Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) -print(a) +solver = rtb.IK_QP() +sol = panda.ik_LM(Tep, tol=tol, q0=q0, method="chan") diff --git a/roboticstoolbox/mobile/Animations.py b/roboticstoolbox/mobile/Animations.py index 5b6a24011..86d545e15 100644 --- a/roboticstoolbox/mobile/Animations.py +++ b/roboticstoolbox/mobile/Animations.py @@ -5,6 +5,7 @@ from abc import ABC from math import pi, atan2 import numpy as np + # from scipy import integrate, linalg, interpolate from pathlib import Path import matplotlib.pyplot as plt @@ -14,15 +15,16 @@ from spatialmath import SE2, base from roboticstoolbox import rtb_load_data + class VehicleAnimationBase(ABC): """ - Abstract base class to support animation of a vehicle on Matplotlib plot + Abstract base class to support animation of a vehicle in a Matplotlib plot There are three concrete subclasses: - - ``VehicleMarker`` animates a Matplotlib marker - - ``VehiclePolygon`` animates a polygon shape (outline or filled), including predefined shapes - - ``VehicleIcon`` animates an image + - ``VehicleMarker`` animates a Matplotlib marker (shows position only) + - ``VehiclePolygon`` animates a polygon shape (outline or filled), including predefined shapes (shows position and orientation) + - ``VehicleIcon`` animates an image (shows position and orientation) An instance ``a`` of these classes can be used in three different ways, firstly:: @@ -34,7 +36,7 @@ class VehicleAnimationBase(ABC): a.update(q) - will animate it. + will animate it with the configuration given by ``q``. Secondly, an instance can be passed to a Vehicle subclass object to make an animation during simulation:: @@ -114,6 +116,7 @@ def __del__(self): if self._object is not None: self._object.remove() + # ========================================================================= # class VehicleMarker(VehicleAnimationBase): def __init__(self, **kwargs): @@ -161,6 +164,7 @@ def _add(self, x=None, **kwargs): # ========================================================================= # + class VehiclePolygon(VehicleAnimationBase): def __init__(self, shape="car", scale=1, **kwargs): """ @@ -272,7 +276,7 @@ def __init__(self, filename, origin=None, scale=1, rotation=0): Creates an object that can be passed to a ``Vehicle`` subclass to depict the moving robot as an image icon during simulation. The image - is translated and rotated to represent the vehicle configuration. + is translated and rotated to represent the vehicle configuration. The car is scaled to an image with a horizontal length (width) of ``scale`` in the units of the plot. By default the image is assumed to @@ -416,7 +420,7 @@ def _update(self, x): V = np.diag(np.r_[0.02, 0.5 * pi / 180] ** 2) - v = VehiclePolygon(facecolor='None', edgecolor='k') + v = VehiclePolygon(facecolor="None", edgecolor="k") # v = VehicleIcon('greycar2', scale=2, rotation=90) veh = Bicycle(covar=V, animation=v, control=RandomPath(10), verbose=False) diff --git a/roboticstoolbox/mobile/Bug2.py b/roboticstoolbox/mobile/Bug2.py index c4644af4a..302ef691a 100644 --- a/roboticstoolbox/mobile/Bug2.py +++ b/roboticstoolbox/mobile/Bug2.py @@ -10,11 +10,14 @@ from spatialmath import base from spatialmath.base.animate import * from scipy.ndimage import * + # from matplotlib import cm import matplotlib.pyplot as plt + # from matplotlib import animation from roboticstoolbox.mobile.PlannerBase import PlannerBase + class Bug2(PlannerBase): """ Construct a Bug2 reactive navigator @@ -38,8 +41,8 @@ class Bug2(PlannerBase): :author: Kristian Gibson and Peter Corke, based on MATLAB version by Peter Corke :seealso: :class:`PlannerBase` """ - def __init__(self, **kwargs): + def __init__(self, **kwargs): super().__init__(ndims=2, **kwargs) self.H = [] # hit points @@ -49,7 +52,6 @@ def __init__(self, **kwargs): self._edge = None self._k = None - @property def m_line(self): """ @@ -63,15 +65,24 @@ def m_line(self): return self._m_line # override query method of base class - def run(self, start=None, goal=None, animate=False, pause=0.001, trail=True, movie=None, **kwargs): + def run( + self, + start=None, + goal=None, + animate=False, + pause=0.001, + trail=True, + movie=None, + **kwargs, + ): """ Find a path using Bug2 reactive navigation algorithm - + :param start: starting position :type start: array_like(2) :param goal: goal position :type goal: array_like(2) - :param animate: show animation of robot moving over the map, + :param animate: show animation of robot moving over the map, defaults to False :type animate: bool, optional :param movie: save animation as a movie, defaults to None. Is either @@ -84,7 +95,7 @@ def run(self, start=None, goal=None, animate=False, pause=0.001, trail=True, mov Compute the path from ``start`` to ``goal`` assuming the robot is capable of 8-way motion from its current cell to the next. - + .. note:: ``start`` and ``goal`` are given as (x,y) coordinates in the occupancy grid map, not as matrix row and column coordinates. @@ -117,8 +128,8 @@ def run(self, start=None, goal=None, animate=False, pause=0.001, trail=True, mov path = robot h = None - trail_line, = plt.plot(0, 0, 'y.', label='robot path') - trailHead, = plt.plot(0, 0, 'ko', zorder=10) + (trail_line,) = plt.plot(0, 0, "y.", label="robot path") + (trailHead,) = plt.plot(0, 0, "ko", zorder=10) # iterate using the next() method until we reach the goal while True: @@ -164,22 +175,21 @@ def plot_m_line(self, ls=None): Plots the m-line on the current axes. """ if ls is None: - ls = 'k--' + ls = "k--" x_min, x_max = plt.gca().get_xlim() y_min, y_max = plt.gca().get_ylim() if self._m_line[1] == 0: # handle the case that the line is vertical - plt.plot([self._start[0], self._start[0]], - [y_min, y_max], 'k--', label='m-line') + plt.plot( + [self._start[0], self._start[0]], [y_min, y_max], "k--", label="m-line" + ) else: # regular line - x = np.array([ - [x_min, 1], - [x_max, 1] ]) + x = np.array([[x_min, 1], [x_max, 1]]) y = -x @ np.r_[self._m_line[0], self._m_line[2]] y = y / self._m_line[1] - plt.plot([x_min, x_max], y, ls, zorder=10, label='m-line') + plt.plot([x_min, x_max], y, ls, zorder=10, label="m-line") def next(self, position): """ @@ -240,14 +250,16 @@ def next(self, position): # we are at the end of the list of edge points, we # are back where we started. Step 2.c test. plt.show(block=True) - raise RuntimeError('robot is trapped') + raise RuntimeError("robot is trapped") # are we on the M-line now ? if abs(np.inner(np.r_[position, 1], self._m_line)) <= 0.5: self.message(f" {position}: crossed the M-line") # are we closer than when we encountered the obstacle? - if base.norm(position - self._goal) < base.norm(self.H[-1] - self._goal): + if base.norm(position - self._goal) < base.norm( + self.H[-1] - self._goal + ): self._step = 1 # transition to step 1 self.message(f" {position}: change to step 1") return n @@ -258,14 +270,16 @@ def next(self, position): return n def query(self): - raise NotImplementedError('This class has no query method') + raise NotImplementedError("This class has no query method") def plan(self): - raise NotImplementedError('This class has no plan method') + raise NotImplementedError("This class has no plan method") + # Ported from Peter Corke's edgelist function found: # https://github.com/petercorke/toolbox-common-matlab/blob/master/edgelist.m + # these are directions of 8-neighbours in a clockwise direction # fmt: off _dirs = np.array([ @@ -309,7 +323,7 @@ def edgelist(im, p, direction=1): - Coordinates are given and returned assuming the matrix is an image, so the indices are always in the form (x,y) or (column,row). - - ``im` is a binary image where 0 is assumed to be background, non-zero + - ``im` is a binary image where 0 is assumed to be background, non-zero is an object. - ``p`` must be a point on the edge of the region. - The initial point is always the first and last element of the returned edgelist. @@ -324,7 +338,6 @@ def edgelist(im, p, direction=1): """ p = base.getvector(p, 2, dtype=int) - if direction > 0: neighbours = np.arange(start=0, stop=8, step=1) else: @@ -333,22 +346,20 @@ def edgelist(im, p, direction=1): try: pix0 = im[p[1], p[0]] # color of pixel we start at except: - raise ValueError('specified coordinate is not within image') - + raise ValueError("specified coordinate is not within image") q = adjacent_point(im, p, pix0) # find an adjacent point outside the blob if q is None: - raise ValueError('no neighbour outside the blob') + raise ValueError("no neighbour outside the blob") d = None e = [p] # initialize the edge list - dir = [] # initialize the direction list + dir = [] # initialize the direction list p0 = None while True: - # find which direction is Q dq = q - p for kq in range(0, 8): @@ -362,7 +373,6 @@ def edgelist(im, p, direction=1): k = (j + kq) % 8 # if k > 7: # k = k - 7 - # compute coordinate of the k'th neighbour nk = p + _dirs[k] @@ -380,19 +390,21 @@ def edgelist(im, p, direction=1): dir.append(k) # check if we are back where we started if p0 is None: - p0 = p # make a note of where we started + p0 = p # make a note of where we started else: if all(p == p0): break # keep going, add this point to the edgelist e.append(p) - + return e, dir + # Ported from Peter Corke's adjacent_point function found: # https://github.com/petercorke/toolbox-common-matlab/blob/master/edgelist.m + def adjacent_point(im, seed, pix0): """Find adjacent point @@ -417,7 +429,7 @@ def adjacent_point(im, seed, pix0): if im[p[1], p[0]] != pix0: return p except: - pass + pass return None @@ -433,11 +445,10 @@ def hom_line(p1, p2): if __name__ == "__main__": - from roboticstoolbox import rtb_load_matfile vars = rtb_load_matfile("data/map1.mat") - map = vars['map'] + map = vars["map"] bug2 = Bug2(occgrid=map) # bug.plan() diff --git a/roboticstoolbox/mobile/DistanceTransformPlanner.py b/roboticstoolbox/mobile/DistanceTransformPlanner.py index cbc727239..d14995f50 100644 --- a/roboticstoolbox/mobile/DistanceTransformPlanner.py +++ b/roboticstoolbox/mobile/DistanceTransformPlanner.py @@ -40,7 +40,7 @@ class DistanceTransformPlanner(PlannerBase): The map is described by a 2D occupancy ``occgrid`` whose elements are zero if traversable of nonzero if untraversable, ie. an obstacle. - + The cells are assumed to be unit squares. Crossing the cell horizontally or vertically is a travel distance of 1, and for the Euclidean distance measure, the crossing the cell diagonally is a distance of :math:`\sqrt{2}`. @@ -106,7 +106,7 @@ def __str__(self): return s - def plan(self, goal=None, animate=False, verbose=False): + def plan(self, goal=None, animate=False, summary=False): r""" Plan path using distance transform @@ -135,7 +135,8 @@ def plan(self, goal=None, animate=False, verbose=False): self.occgrid.grid, goal=self._goal, metric=self._metric, - animate=animate + animate=animate, + summary=summary, ) def next(self, position): @@ -147,7 +148,7 @@ def next(self, position): :raises RuntimeError: no plan has been computed :return: next robot position :rtype: ndarray(2) - + Return the robot position that is one step closer to the goal. Called by :meth:`query` to find a path from start to goal. @@ -215,7 +216,7 @@ def plot_3d(self, path=None, ls=None): line/surface handling. """ fig = plt.figure() - ax = fig.gca(projection="3d") + ax = fig.add_subplot(111, projection="3d") distance = self._distancemap X, Y = np.meshgrid(np.arange(distance.shape[1]), np.arange(distance.shape[0])) @@ -273,7 +274,7 @@ def distancexform(occgrid, goal, metric="cityblock", animate=False, summary=Fals # - other cells are inf # - goal is zero - goal = base.getvector(goal, 2, dtype=np.int) + goal = base.getvector(goal, 2, dtype=int) distance = occgrid.astype(np.float32) distance[occgrid > 0] = np.nan # assign nan to obstacle cells diff --git a/roboticstoolbox/mobile/DstarPlanner.py b/roboticstoolbox/mobile/DstarPlanner.py index 7d83cdd01..334dfabf0 100644 --- a/roboticstoolbox/mobile/DstarPlanner.py +++ b/roboticstoolbox/mobile/DstarPlanner.py @@ -28,13 +28,13 @@ # D* grid planning # Author: Nirnay Roy # Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE # for performance reasons there are some important differences compared to # the version from Python Robotics: # # 1. replace the classic D* functions min__State(), get_kmin(), insert(), remove() -# with heapq.heappush() and heapq.heappop(). The open_list is now a list of +# with heapq.heappush() and heapq.heappop(). The open_list is now a list of # tuples (k, _State) maintained by heapq, rather than a set # # 2. use enums rather than strings for cell state @@ -44,22 +44,24 @@ # - replace equality tests with math.isclose() which is faster than np.isclose() # - add an offset to inequality tests, X > Y becomes X > Y + tol + class _Tag(IntEnum): NEW = auto() OPEN = auto() CLOSED = auto() -class _State: + +class _State: def __init__(self, x, y): self.x = x self.y = y - self.parent = None # 'back pointer' to next _State + self.parent = None # 'back pointer' to next _State self.t = _Tag.NEW # open closed new self.h = 0 # cost to goal self.k = 0 # estimate of shortest path cost def __str__(self): - return f"({self.x}, {self.y})" #[{self.h:.1f}, {self.k:.1f}]" + return f"({self.x}, {self.y})" # [{self.h:.1f}, {self.k:.1f}]" def __repr__(self): return self.__str__() @@ -67,8 +69,8 @@ def __repr__(self): def __lt__(self, other): return True -class _Map: +class _Map: def __init__(self, row, col): self.row = row self.col = col @@ -81,7 +83,7 @@ def init__Map(self): tmp = [] for j in range(self.col): tmp.append(_State(j, i)) # append column to the row - _Map_list.append(tmp) # append row to _Map + _Map_list.append(tmp) # append row to _Map return _Map_list _neighbours = [(-1, -1), (-1, 0), (-1, 1), (0, -1), (0, 1), (1, -1), (1, 0), (1, 1)] @@ -100,7 +102,9 @@ def get_neighbors(self, _State): _root2 = np.sqrt(2) def cost(self, _State1, _State2): - c = (self.costmap[_State1.y, _State1.x] + self.costmap[_State2.y, _State2.x]) / 2 + c = ( + self.costmap[_State1.y, _State1.x] + self.costmap[_State2.y, _State2.x] + ) / 2 dx = _State1.x - _State2.x dy = _State1.y - _State2.y @@ -132,12 +136,14 @@ def __init__(self, _Map, tol=1e-6): def process__State(self, verbose=False): if verbose: - print('FRONTIER ', ' '.join([f"({x[1].x}, {x[1].y})" for x in self.open_list])) + print( + "FRONTIER ", " ".join([f"({x[1].x}, {x[1].y})" for x in self.open_list]) + ) # get _State from frontier if len(self.open_list) == 0: if verbose: - print(' x is None ') + print(" x is None ") return -1 k_old, x = heapq.heappop(self.open_list) x.t = _Tag.CLOSED @@ -150,24 +156,35 @@ def process__State(self, verbose=False): if x.h > k_old + self.tol: # RAISE _State if verbose: - print(' raise') + print(" raise") for y in self._Map.get_neighbors(x): - if (y.t is not _Tag.NEW and - y.h <= k_old - self.tol and - x.h > y.h + self._Map.cost(x, y) + self.tol): + if ( + y.t is not _Tag.NEW + and y.h <= k_old - self.tol + and x.h > y.h + self._Map.cost(x, y) + self.tol + ): if verbose: - print(f" {x} cost from {x.h:.1f} to {y.h + self._Map.cost(x, y):.1f}; parent from {x.parent} to {y}") + print( + f" {x} cost from {x.h:.1f} to {y.h + self._Map.cost(x, y):.1f}; parent from {x.parent} to {y}" + ) x.parent = y x.h = y.h + self._Map.cost(x, y) if math.isclose(x.h, k_old, rel_tol=0, abs_tol=self.tol): # normal _State if verbose: - print(' normal') + print(" normal") for y in self._Map.get_neighbors(x): - if y.t is _Tag.NEW \ - or (y.parent == x and not math.isclose(y.h, x.h + self._Map.cost(x, y), rel_tol=0, abs_tol=self.tol)) \ - or (y.parent != x and y.h > x.h + self._Map.cost(x, y) + self.tol): + if ( + y.t is _Tag.NEW + or ( + y.parent == x + and not math.isclose( + y.h, x.h + self._Map.cost(x, y), rel_tol=0, abs_tol=self.tol + ) + ) + or (y.parent != x and y.h > x.h + self._Map.cost(x, y) + self.tol) + ): if verbose: print(f" reparent {y} from {y.parent} to {x}") y.parent = x @@ -175,21 +192,36 @@ def process__State(self, verbose=False): else: # RAISE or LOWER _State if verbose: - print(' raise/lower') + print(" raise/lower") for y in self._Map.get_neighbors(x): - if y.t is _Tag.NEW or (y.parent == x and not math.isclose(y.h, x.h + self._Map.cost(x, y), rel_tol=0, abs_tol=self.tol)): + if y.t is _Tag.NEW or ( + y.parent == x + and not math.isclose( + y.h, x.h + self._Map.cost(x, y), rel_tol=0, abs_tol=self.tol + ) + ): if verbose: - print(f" {y} cost from {y.h:.1f} to {y.h + self._Map.cost(x, y):.1f}; parent from {y.parent} to {x}; add to frontier") + print( + f" {y} cost from {y.h:.1f} to {y.h + self._Map.cost(x, y):.1f}; parent from {y.parent} to {x}; add to frontier" + ) y.parent = x self.insert(y, x.h + self._Map.cost(x, y)) else: - if y.parent != x and y.h > x.h + self._Map.cost(x, y) + self.tol and x.t is _Tag.CLOSED: + if ( + y.parent != x + and y.h > x.h + self._Map.cost(x, y) + self.tol + and x.t is _Tag.CLOSED + ): self.insert(x, x.h) if verbose: print(f" {x}, {x.h:.1f} add to frontier") else: - if y.parent != x and x.h > y.h + self._Map.cost(y, x) + self.tol \ - and y.t is _Tag.CLOSED and y.h > k_old + self.tol: + if ( + y.parent != x + and x.h > y.h + self._Map.cost(y, x) + self.tol + and y.t is _Tag.CLOSED + and y.h > k_old + self.tol + ): self.insert(y, y.h) if verbose: print(f" {y}, {y.h:.1f} add to frontier") @@ -236,7 +268,7 @@ def insert(self, _State, h_new): # for i, item in enumerate(self.open_list): # if item[1] is _State: # del self.open_list[i] - # break + # break elif _State.t is _Tag.CLOSED: _State.k = min(_State.h, h_new) @@ -258,27 +290,28 @@ def modify_cost(self, x, newcost): return self.open_list[0][0] def showparents(self): - for y in range(self._Map.row-1, -1, -1): - if y == self._Map.row-1: - print(" ", end='') + for y in range(self._Map.row - 1, -1, -1): + if y == self._Map.row - 1: + print(" ", end="") for x in range(self._Map.col): - print(f" {x} ", end='') + print(f" {x} ", end="") print() - print(f"{y}: ", end='') + print(f"{y}: ", end="") for x in range(self._Map.col): x = self._Map._Map[y][x] par = x.parent if par is None: - print(' G ', end='') + print(" G ", end="") else: - print(f"({par.x},{par.y}) ", end='') + print(f"({par.x},{par.y}) ", end="") print() print() + # ====================== RTB wrapper ============================= # # Copyright (c) 2022 Peter Corke: https://github.com/petercorke/robotics-toolbox-python -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE class DstarPlanner(PlannerBase): r""" D* path planner @@ -322,6 +355,7 @@ class DstarPlanner(PlannerBase): :thanks: based on D* grid planning included from `Python Robotics `_ :seealso: :class:`PlannerBase` """ + def __init__(self, costmap=None, **kwargs): super().__init__(ndims=2, **kwargs) @@ -332,10 +366,10 @@ def __init__(self, costmap=None, **kwargs): elif self.occgrid is not None: self.costmap = np.where(self.occgrid.grid > 0, np.inf, 1) else: - raise ValueError('unknown type of map') + raise ValueError("unknown type of map") self._Map = _Map(self.costmap.shape[0], self.costmap.shape[1]) self._Map.costmap = self.costmap - self._Dstar = _Dstar(self._Map) #, tol=0) + self._Dstar = _Dstar(self._Map) # , tol=0) def plan(self, goal=None, animate=False, progress=True, summary=False): r""" @@ -371,7 +405,7 @@ def plan(self, goal=None, animate=False, progress=True, summary=False): if ret == -1: break - + if summary: print(self._Dstar.ninsert, self._Dstar.nin) @@ -427,7 +461,7 @@ def query(self, start, sensor=None, animate=False, verbose=False): tmp = start_State if sensor is not None and not callable(sensor): - raise ValueError('sensor must be callable') + raise ValueError("sensor must be callable") cost = tmp.h self.goal_State.h = 0 @@ -437,7 +471,7 @@ def query(self, start, sensor=None, animate=False, verbose=False): path.append((tmp.x, tmp.y)) if tmp == self.goal_State: break - + # x, y = tmp.parent.x, tmp.parent.y if sensor is not None: @@ -449,18 +483,23 @@ def query(self, start, sensor=None, animate=False, verbose=False): # print(f"change cost at ({x}, {y}) to {newcost}") val = self._Dstar.modify_cost(X, newcost) # propagate the changes to plan - print('propagate') + print("propagate") # self._Dstar.showparents() while val != -1 and val < tmp.h: val = self._Dstar.process__State(verbose=verbose) # print('open set', len(self._Dstar.open_list)) # self._Dstar.showparents() - + tmp = tmp.parent # print('move to ', tmp) - status = namedtuple('_DstarStatus', ['cost',]) - + status = namedtuple( + "_DstarStatus", + [ + "cost", + ], + ) + return np.array(path), status(cost) # # Just feed self._h into plot function from parent as p @@ -479,17 +518,18 @@ def query(self, start, sensor=None, animate=False, verbose=False): # x = np.unravel_index((i), self._costmap.shape) # return x[1], x[0] + if __name__ == "__main__": - og = np.zeros((10,10)) + og = np.zeros((10, 10)) og[4:8, 3:6] = 1 print(og) ds = DstarPlanner(occgrid=og) print(ds.costmap) - start = (1,1) - goal = (7,6) + start = (1, 1) + goal = (7, 6) ds.plan(goal=goal) ds._Map.show_h() @@ -500,7 +540,6 @@ def query(self, start, sensor=None, animate=False, verbose=False): # ds.plot(path=path) - def sensorfunc(pos): if pos == (3, 3): changes = [] @@ -549,4 +588,3 @@ def sensorfunc(pos): # _Dstar.replan() plt.show(block=True) - diff --git a/roboticstoolbox/mobile/DubinsPlanner.py b/roboticstoolbox/mobile/DubinsPlanner.py index f82cd26d2..ec6cef47e 100644 --- a/roboticstoolbox/mobile/DubinsPlanner.py +++ b/roboticstoolbox/mobile/DubinsPlanner.py @@ -391,6 +391,15 @@ def __str__(self): super().__str__() + f"\n curvature={self.curvature}, stepsize={self.stepsize}" ) + return s + + @property + def curvature(self): + return self._curvature + + @property + def stepsize(self): + return self._stepsize def query(self, start, goal, **kwargs): r""" diff --git a/roboticstoolbox/mobile/EKF.py b/roboticstoolbox/mobile/EKF.py index 13b63477e..27c7d37ec 100644 --- a/roboticstoolbox/mobile/EKF.py +++ b/roboticstoolbox/mobile/EKF.py @@ -10,6 +10,7 @@ from scipy.linalg import sqrtm, block_diag from scipy.stats.distributions import chi2 import matplotlib.pyplot as plt +from matplotlib import animation from spatialmath.base.animate import Animate from spatialmath import base, SE2 @@ -19,11 +20,20 @@ class EKF: - - def __init__(self, robot, sensor=None, map=None, - P0=None, x_est=None, joseph=True, - animate=True, x0=[0, 0, 0], - verbose=False, history=True, workspace=None): + def __init__( + self, + robot, + sensor=None, + map=None, + P0=None, + x_est=None, + joseph=True, + animate=True, + x0=[0, 0, 0], + verbose=False, + history=True, + workspace=None, + ): r""" Extended Kalman filter @@ -166,8 +176,8 @@ def __init__(self, robot, sensor=None, map=None, time a new landmark is observed. Create a vehicle with perfect odometry (no covariance), add a driver to it, - create a sensor that uses the map and vehicle state to estimate landmark range - and bearing with covariance ``W``, the Kalman filter with estimated sensor + create a sensor that uses the map and vehicle state to estimate landmark range + and bearing with covariance ``W``, the Kalman filter with estimated sensor covariance ``W``, then run the filter for N time steps:: robot = Bicycle() @@ -208,7 +218,7 @@ def __init__(self, robot, sensor=None, map=None, W = np.diag([0.1, np.radians(1)]) ** 2 sensor = RangeBearingSensor(robot=robot, map=map, covar=W, angle=[-np.pi/2, np.pi/2], range=4, animate=True) - + ekf = EKF(robot=(robot, V), P0=P0, sensor=(sensor, W)) ekf.run(T=20) # run the simulation for 20 seconds @@ -229,16 +239,22 @@ def __init__(self, robot, sensor=None, map=None, """ if robot is not None: - if not isinstance(robot, tuple) or len(robot) != 2 \ - or not isinstance(robot[0], VehicleBase): - raise TypeError('robot must be tuple (vehicle, V_est)') + if ( + not isinstance(robot, tuple) + or len(robot) != 2 + or not isinstance(robot[0], VehicleBase) + ): + raise TypeError("robot must be tuple (vehicle, V_est)") self._robot = robot[0] # reference to the robot vehicle self._V_est = robot[1] # estimate of vehicle state covariance V if sensor is not None: - if not isinstance(sensor, tuple) or len(sensor) != 2 \ - or not isinstance(sensor[0], SensorBase): - raise TypeError('sensor must be tuple (sensor, W_est)') + if ( + not isinstance(sensor, tuple) + or len(sensor) != 2 + or not isinstance(sensor[0], SensorBase) + ): + raise TypeError("sensor must be tuple (sensor, W_est)") self._sensor = sensor[0] # reference to the sensor self._W_est = sensor[1] # estimate of sensor covariance W else: @@ -246,7 +262,7 @@ def __init__(self, robot, sensor=None, map=None, self._W_est = None if map is not None and not isinstance(map, LandmarkMap): - raise TypeError('map must be LandmarkMap instance') + raise TypeError("map must be LandmarkMap instance") self._ekf_map = map # prior map for localization if animate: @@ -257,15 +273,15 @@ def __init__(self, robot, sensor=None, map=None, self._workspace = sensor[0].map.workspace self._robot._workspace = sensor[0].map.workspace elif self.robot.workspace is None: - raise ValueError('for animation robot must have a defined workspace') + raise ValueError("for animation robot must have a defined workspace") self.animate = animate self._P0 = P0 # initial system covariance self._x0 = x0 # initial vehicle state - self._x_est = x_est # estimated state - self._landmarks = None # ekf_map state - + self._x_est = x_est # estimated state + self._landmarks = None # ekf_map state + self._est_vehicle = False self._est_ekf_map = False if self._V_est is not None: @@ -278,11 +294,11 @@ def __init__(self, robot, sensor=None, map=None, if map is None and sensor is not None: # estimating ekf_map self._est_ekf_map = True - self._joseph = joseph # flag: use Joseph form to compute p + self._joseph = joseph # flag: use Joseph form to compute p self._verbose = verbose - self._keep_history = history # keep history + self._keep_history = history # keep history self._htuple = namedtuple("EKFlog", "t xest odo P innov S K lm z") if workspace is not None: @@ -306,14 +322,14 @@ def __init__(self, robot, sensor=None, map=None, else: # noisy odometry case if self.V_est.shape != (2, 2): - raise ValueError('vehicle state covariance V_est must be 2x2') + raise ValueError("vehicle state covariance V_est must be 2x2") self._x_est = self.robot.x self._P_est = P0 self._est_vehicle = True if self.W_est is not None: if self.W_est.shape != (2, 2): - raise ValueError('sensor covariance W_est must be 2x2') + raise ValueError("sensor covariance W_est must be 2x2") # if np.any(self._sensor): # self._landmarks = None*np.zeros(2, self._sensor.ekf_map.nlandmarks) @@ -325,7 +341,7 @@ def __init__(self, robot, sensor=None, map=None, # if np.any(sensor) and not isinstance(sensor, 'Sensor'): # raise ValueError('expecting Sensor object') - self._init() + self.init() self.xxdata = ([], []) @@ -333,28 +349,28 @@ def __str__(self): s = f"EKF object: {len(self._x_est)} states" def indent(s, n=2): - spaces = ' ' * n - return s.replace('\n', '\n' + spaces) + spaces = " " * n + return s.replace("\n", "\n" + spaces) estimating = [] if self._est_vehicle is not None: - estimating.append('vehicle pose') + estimating.append("vehicle pose") if self._est_ekf_map is not None: - estimating.append('map') + estimating.append("map") if len(estimating) > 0: - s += ', estimating: ' + ", ".join(estimating) + s += ", estimating: " + ", ".join(estimating) if self.robot is not None: - s += indent("\nrobot: " + str(self.robot)) + s += indent("\nrobot: " + str(self.robot)) if self.V_est is not None: - s += indent('\nV_est: ' + base.array2str(self.V_est)) + s += indent("\nV_est: " + base.array2str(self.V_est)) if self.sensor is not None: s += indent("\nsensor: " + str(self.sensor)) if self.W_est is not None: - s += indent('\nW_est: ' + base.array2str(self.W_est)) + s += indent("\nW_est: " + base.array2str(self.W_est)) return s - + def __repr__(self): return str(self) @@ -370,7 +386,7 @@ def x_est(self): simulation. The dimensions depend on the problem being solved. """ return self._x_est - + @property def P_est(self): """ @@ -395,7 +411,7 @@ def P0(self): Returns the value of the covariance matrix passed to the constructor. """ return self._P0 - + @property def V_est(self): """ @@ -421,7 +437,7 @@ def W_est(self): the constructor """ return self._W_est - + @property def robot(self): """ @@ -431,7 +447,7 @@ def robot(self): :rtype: :class:`VehicleBase` subclass """ return self._robot - + @property def sensor(self): """ @@ -451,7 +467,7 @@ def map(self): :rtype: :class:`LandmarkMap` subclass """ return self._map - + @property def verbose(self): """ @@ -461,7 +477,7 @@ def verbose(self): :rtype: bool """ return self._verbose - + @property def history(self): """ @@ -474,11 +490,11 @@ def history(self): list. It contains, for that time step, estimated state and covariance, and sensor observation. - :seealso: :meth:`get_t` :meth:`get_xyt` :meth:`get_map` :meth:`get_P` - :meth:`get_Pnorm` + :seealso: :meth:`get_t` :meth:`get_xyt` :meth:`get_map` :meth:`get_P` + :meth:`get_Pnorm` """ return self._history - + @property def workspace(self): """ @@ -492,7 +508,6 @@ def workspace(self): """ return self._workspace - @property def landmarks(self): """ @@ -505,7 +520,7 @@ def landmarks(self): - order in which landmark was seen - number of times seen - + The order in which the landmark was first seen. The first observed landmark has order 0 and so on. @@ -585,19 +600,19 @@ def landmark_x(self, id): Returns the landmark position from the current state vector. """ jx = self.landmark_index(id) - return self._x_est[jx: jx+2] + return self._x_est[jx : jx + 2] - def _init(self): - #EKF.init Reset the filter + def init(self): + # EKF.init Reset the filter # # E.init() resets the filter state and clears landmarks and history. self.robot.init() if self.sensor is not None: self.sensor.init() - #clear the history + # clear the history self._history = [] - + if self._V_est is None: # perfect vehicle case self._estVehicle = False @@ -608,14 +623,14 @@ def _init(self): self._x_est = self._x0 self._P_est = self._P0 self._estVehicle = True - + if self.sensor is not None: # landmark dictionary maps lm_id to list[index, nseen] self._landmarks = {} # np.full((2, len(self.sensor.map)), -1, dtype=int) - def run(self, T, animate=False, movie=None): + def run(self, T, animate=False): """ Run the EKF simulation @@ -623,8 +638,6 @@ def run(self, T, animate=False, movie=None): :type T: float :param animate: animate motion of vehicle, defaults to False :type animate: bool, optional - :param movie: name of movie file to create, defaults to None - :type movie: str, optional Simulates the motion of a vehicle (under the control of a driving agent) and the EKF estimator. The steps are: @@ -637,57 +650,134 @@ def run(self, T, animate=False, movie=None): - execute the EKF - save information as a namedtuple to the history list for later display - :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` + :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` :meth:`get_xyt` :meth:`get_t` :meth:`get_map` :meth:`get_P` :meth:`get_Pnorm` :meth:`plot_xy` :meth:`plot_ellipse` :meth:`plot_error` :meth:`plot_map` + :meth:`run_animation` """ - self._init() + self.init() if animate: if self.sensor is not None: self.sensor.map.plot() - elif self._dim is not None: - if not Iterable(self._dim): - d = self._dim - plt.axis([-d, d, -d, d]) - elif len(self._dim) == 2: - w = self._dim[1] - h = self._dim(2) - plt.axis([-w, w, -h, h]) - elif len(self._dim) == 4: - plt.axis(self._dim) - else: - animate = False - plt.xlabel('X') - plt.ylabel('Y') + plt.xlabel("X") + plt.ylabel("Y") - # anim = Animate(movie) for k in range(round(T / self.robot.dt)): if animate: - self.robot.plot() - self._step() - # anim.add() + # self.robot.plot() + self.robot._animation.update(self.robot.x) + self.step() - # anim.close() + def run_animation(self, T=10, x0=None, control=None, format=None, file=None): + r""" + Run the EKF simulation + :param T: maximum simulation time in seconds + :type T: float + :param format: Output format + :type format: str, optional + :param file: File name + :type file: str, optional + :return: Matplotlib animation object + :rtype: :meth:`matplotlib.animation.FuncAnimation` + + Simulates the motion of a vehicle (under the control of a driving agent) + and the EKF estimator for ``T`` seconds and returns an animation + in various formats:: + + ``format`` ``file`` description + ============ ========= ============================ + ``"html"`` str, None return HTML5 video + ``"jshtml"`` str, None return JS+HTML video + ``"gif"`` str return animated GIF + ``"mp4"`` str return MP4/H264 video + ``None`` return a ``FuncAnimation`` object + + If ``file`` can be ``None`` then return the video as a string, otherwise it + must be a filename. + + The simulation steps are: + + - initialize the filter, vehicle and vehicle driver agent, sensor + - for each time step: - # TODO: Make the following methods private. - def _step(self, z_pred=None): + - step the vehicle and its driver agent, obtain odometry + - take a sensor reading + - execute the EKF + - save information as a namedtuple to the history list for later display + + :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` + :meth:`get_xyt` :meth:`get_t` :meth:`get_map` :meth:`get_P` :meth:`get_Pnorm` + :meth:`plot_xy` :meth:`plot_ellipse` :meth:`plot_error` :meth:`plot_map` + :meth:`run_animation` """ - Execute one timestep of the simulation - :param z_pred: _description_, defaults to None - :type z_pred: _type_, optional + fig, ax = plt.subplots() + + def init(): + self.init() + if self.sensor is not None: + self.sensor.map.plot() + ax.set_xlabel("X") + ax.set_ylabel("Y") + + def animate(i): + self.robot._animation.update(self.robot.x) + self.step(pause=False) + + nframes = round(T / self.robot._dt) + anim = animation.FuncAnimation( + fig=fig, + func=animate, + init_func=init, + frames=nframes, + interval=self.robot.dt * 1000, + blit=False, + repeat=False, + ) + + ret = None + if format == "html": + ret = anim.to_html5_video() # convert to embeddable HTML5 animation + elif format == "jshtml": + ret = anim.to_jshtml() # convert to embeddable Javascript/HTML animation + elif format == "gif": + anim.save( + file, writer=animation.PillowWriter(fps=1 / self.robot.dt) + ) # convert to GIF + ret = None + elif format == "mp4": + anim.save( + file, writer=animation.FFMpegWriter(fps=1 / self.robot.dt) + ) # convert to mp4/H264 + ret = None + elif format == None: + # return the anim object + return anim + else: + raise ValueError("unknown format") + + if ret is not None and file is not None: + with open(file, "w") as f: + f.write(ret) + ret = None + plt.close(fig) + return ret + + def step(self, pause=None): + """ + Execute one timestep of the simulation """ # move the robot - odo = self.robot.step() + odo = self.robot.step(pause=pause) # ================================================================= # P R E D I C T I O N # ================================================================= if self._est_vehicle: - # split the state vector and covariance into chunks for + # split the state vector and covariance into chunks for # vehicle and map xv_est = self._x_est[:3] xm_est = self._x_est[3:] @@ -722,7 +812,7 @@ def _step(self, z_pred=None): if self._est_vehicle and not self._est_ekf_map: # vehicle only x_pred = xv_pred - P_pred = Pvv_pred + P_pred = Pvv_pred elif not self._est_vehicle and self._est_ekf_map: # map only x_pred = xm_pred @@ -738,7 +828,7 @@ def _step(self, z_pred=None): # fmt: on # at this point we have: - # xv_pred the state of the vehicle to use to + # xv_pred the state of the vehicle to use to # predict observations # xm_pred the state of the map # x_pred the full predicted state vector @@ -755,7 +845,6 @@ def _step(self, z_pred=None): # P R O C E S S O B S E R V A T I O N S # ================================================================= - if self.sensor is not None: # read the sensor z, lm_id = self.sensor.reading() @@ -770,33 +859,29 @@ def _step(self, z_pred=None): # compute the innovation z_pred = self.sensor.h(xv_pred, lm_id) - innov = np.array([ - z[0] - z_pred[0], - base.angdiff(z[1], z_pred[1]) - ]) + innov = np.array([z[0] - z_pred[0], base.wrap_mpi_pi(z[1] - z_pred[1])]) if self._est_ekf_map: # the ekf_map is estimated MM or SLAM case if self._isseenbefore(lm_id): # landmark is previously seen - + # get previous estimate of its state jx = self.landmark_mindex(lm_id) - xf = xm_pred[jx: jx+2] + xf = xm_pred[jx : jx + 2] # compute Jacobian for this particular landmark # xf = self.sensor.g(xv_pred, z) # HACK Hx_k = self.sensor.Hp(xv_pred, xf) z_pred = self.sensor.h(xv_pred, xf) - innov = np.array([ - z[0] - z_pred[0], - base.angdiff(z[1], z_pred[1]) - ]) + innov = np.array( + [z[0] - z_pred[0], base.wrap_mpi_pi(z[1] - z_pred[1])] + ) # create the Jacobian for all landmarks Hx = np.zeros((2, len(xm_pred))) - Hx[:, jx:jx+2] = Hx_k + Hx[:, jx : jx + 2] = Hx_k Hw = self.sensor.Hw(xv_pred, xf) @@ -807,28 +892,37 @@ def _step(self, z_pred=None): self._landmark_increment(lm_id) # update the count if self._verbose: - print(f"landmark {lm_id} seen {self._landmark_count(lm_id)} times, state_idx={self.landmark_index(lm_id)}") + print( + f"landmark {lm_id} seen" + f" {self._landmark_count(lm_id)} times," + f" state_idx={self.landmark_index(lm_id)}" + ) doUpdatePhase = True else: # new landmark, seen for the first time # extend the state vector and covariance - x_pred, P_pred = self._extend_map(P_pred, xv_pred, xm_pred, z, lm_id) + x_pred, P_pred = self._extend_map( + P_pred, xv_pred, xm_pred, z, lm_id + ) # if lm_id == 17: # print(P_pred) # # print(x_pred[-2:], self._sensor._map.landmark(17), base.norm(x_pred[-2:] - self._sensor._map.landmark(17))) self._landmark_add(lm_id) if self._verbose: - print(f"landmark {lm_id} seen for first time, state_idx={self.landmark_index(lm_id)}") + print( + f"landmark {lm_id} seen for first time," + f" state_idx={self.landmark_index(lm_id)}" + ) doUpdatePhase = False else: - # LBL - Hx = self.sensor.Hx(xv_pred, lm_id) - Hw = self.sensor.Hw(xv_pred, lm_id) - doUpdatePhase = True + # LBL + Hx = self.sensor.Hx(xv_pred, lm_id) + Hw = self.sensor.Hw(xv_pred, lm_id) + doUpdatePhase = True else: innov = None @@ -858,14 +952,13 @@ def _step(self, z_pred=None): if self._est_vehicle: # wrap heading state for a vehicle - x_est[2] = base.angdiff(x_est[2]) + x_est[2] = base.wrap_mpi_pi(x_est[2]) # update the covariance if self._joseph: # we use the Joseph form I = np.eye(P_pred.shape[0]) - P_est = (I - K @ Hx) @ P_pred @ (I - K @ Hx).T \ - + K @ self._W_est @ K.T + P_est = (I - K @ Hx) @ P_pred @ (I - K @ Hx).T + K @ self._W_est @ K.T else: P_est = P_pred - K @ S @ K.T # enforce P to be symmetric @@ -910,24 +1003,24 @@ def _landmark_count(self, lm_id): return self._landmarks[lm_id][1] def _landmark_add(self, lm_id): - self._landmarks[lm_id] = [len(self._landmarks) , 1] - + self._landmarks[lm_id] = [len(self._landmarks), 1] + def _extend_map(self, P, xv, xm, z, lm_id): - # this is a new landmark, we haven't seen it before - # estimate position of landmark in the world based on - # noisy sensor reading and current vehicle pose + # this is a new landmark, we haven't seen it before + # estimate position of landmark in the world based on + # noisy sensor reading and current vehicle pose # M = None # estimate its position based on observation and vehicle state xf = self.sensor.g(xv, z) - + # append this estimate to the state vector if self._est_vehicle: x_ext = np.r_[xv, xm, xf] else: x_ext = np.r_[xm, xf] - + # get the Jacobian for the new landmark Gz = self.sensor.Gz(xv, z) @@ -944,7 +1037,7 @@ def _extend_map(self, P, xv, xm, z, lm_id): # fmt: on else: # estimating landmarks only - #P_ext = block_diag(P, Gz @ self._W_est @ Gz.T) + # P_ext = block_diag(P, Gz @ self._W_est @ Gz.T) # fmt: off Yz = np.block([ [np.eye(n), np.zeros((n, 2)) ], @@ -955,7 +1048,6 @@ def _extend_map(self, P, xv, xm, z, lm_id): return x_ext, P_ext - def get_t(self): """ Get time from simulation @@ -985,13 +1077,13 @@ def get_xyt(self): xyt = None return xyt - def plot_xy(self, *args, block=False, **kwargs): + def plot_xy(self, *args, block=None, **kwargs): """ Plot estimated vehicle position :param args: position arguments passed to :meth:`~matplotlib.axes.Axes.plot` :param kwargs: keywords arguments passed to :meth:`~matplotlib.axes.Axes.plot` - :param block: hold plot until figure is closed, defaults to False + :param block: hold plot until figure is closed, defaults to None :type block: bool, optional Plot the estimated vehicle path in the xy-plane. @@ -999,13 +1091,14 @@ def plot_xy(self, *args, block=False, **kwargs): :seealso: :meth:`get_xyt` :meth:`plot_error` :meth:`plot_ellipse` :meth:`plot_P` :meth:`run` :meth:`history` """ - if args is None and 'color' not in kwargs: - kwargs['color'] = 'r' + if args is None and "color" not in kwargs: + kwargs["color"] = "r" xyt = self.get_xyt() plt.plot(xyt[:, 0], xyt[:, 1], *args, **kwargs) - # plt.show(block=block) + if block is not None: + plt.show(block=block) - def plot_ellipse(self, confidence=0.95, N=10, **kwargs): + def plot_ellipse(self, confidence=0.95, N=10, block=None, **kwargs): """ Plot uncertainty ellipses @@ -1013,6 +1106,8 @@ def plot_ellipse(self, confidence=0.95, N=10, **kwargs): :type confidence: float, optional :param N: number of ellipses to plot, defaults to 10 :type N: int, optional + :param block: hold plot until figure is closed, defaults to None + :type block: bool, optional :param kwargs: arguments passed to :meth:`spatialmath.base.graphics.plot_ellipse` Plot ``N`` uncertainty ellipses spaced evenly along the trajectory. @@ -1026,17 +1121,31 @@ def plot_ellipse(self, confidence=0.95, N=10, **kwargs): del kwargs["label"] else: label = f"{confidence*100:.3g}% confidence" - - for k in np.linspace(0, nhist-1, N): + + for k in np.linspace(0, nhist - 1, N): k = round(k) h = self._history[k] if k == 0: - base.plot_ellipse(h.P[:2, :2], centre=h.xest[:2], confidence=confidence, label=label, inverted=True, **kwargs) + base.plot_ellipse( + h.P[:2, :2], + centre=h.xest[:2], + confidence=confidence, + label=label, + inverted=True, + **kwargs, + ) else: - base.plot_ellipse(h.P[:2, :2], centre=h.xest[:2], confidence=confidence, inverted=True, **kwargs) - - - def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): + base.plot_ellipse( + h.P[:2, :2], + centre=h.xest[:2], + confidence=confidence, + inverted=True, + **kwargs, + ) + if block is not None: + plt.show(block=block) + + def plot_error(self, bgcolor="r", confidence=0.95, ax=None, block=None, **kwargs): r""" Plot error with uncertainty bounds @@ -1044,13 +1153,15 @@ def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): :type bgcolor: str, optional :param confidence: confidence interval, defaults to 0.95 :type confidence: float, optional + :param block: hold plot until figure is closed, defaults to None + :type block: bool, optional - Plot the error between actual and estimated vehicle + Plot the error between actual and estimated vehicle path :math:`(x, y, \theta)`` versus time as three stacked plots. Heading error is wrapped into the range :math:`[-\pi,\pi)` - Behind each line draw a shaded polygon ``bgcolor`` showing the specified + Behind each line draw a shaded polygon ``bgcolor`` showing the specified ``confidence`` bounds based on the covariance at each time step. Ideally the lines should be within the shaded polygon ``confidence`` of the time. @@ -1060,7 +1171,7 @@ def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): :seealso: :meth:`get_P` :meth:`run` :meth:`history` """ error = [] - bounds = [] + bounds = [] ppf = chi2.ppf(confidence, df=2) x_gt = self.robot.x_hist @@ -1068,7 +1179,7 @@ def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): hk = self.history[k] # error is true - estimated e = x_gt[k, :] - hk.xest - e[2] = base.angdiff(e[2]) + e[2] = base.wrap_mpi_pi(e[2]) error.append(e) P = np.diag(hk.P) @@ -1086,17 +1197,24 @@ def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): for k, ax in enumerate(axes): if confidence is not None: - edge = np.array([ - np.r_[t, t[::-1]], - np.r_[bounds[:, k], -bounds[::-1, k]], - ]) - polygon = plt.Polygon(edge.T, closed=True, facecolor='r', edgecolor='none', alpha=0.3) + edge = np.array( + [ + np.r_[t, t[::-1]], + np.r_[bounds[:, k], -bounds[::-1, k]], + ] + ) + polygon = plt.Polygon( + edge.T, closed=True, facecolor="r", edgecolor="none", alpha=0.3 + ) ax.add_patch(polygon) - ax.plot(error[:, k], **kwargs); + ax.plot(error[:, k], **kwargs) ax.grid(True) ax.set_ylabel(labels[k] + " error") ax.set_xlim(0, t[-1]) - + + if block is not None: + plt.show(block=block) + # subplot(opt.nplots*100+12) # if opt.confidence # edge = [pxy(:,2); -pxy(end:-1:1,2)]; @@ -1108,7 +1226,7 @@ def plot_error(self, bgcolor='r', confidence=0.95, ax=None, **kwargs): # hold off # grid # ylabel('y error') - + # subplot(opt.nplots*100+13) # if opt.confidence # edge = [pxy(:,3); -pxy(end:-1:1,3)]; @@ -1141,11 +1259,11 @@ def get_map(self): # state if we are estimating vehicle as well if self._est_vehicle: jx += 3 - xf = self._x_est[jx: jx+2] + xf = self._x_est[jx : jx + 2] xy.append(xf) return np.array(xy) - def plot_map(self, marker=None, ellipse=None, confidence = 0.95): + def plot_map(self, marker=None, ellipse=None, confidence=0.95, block=None): """ Plot estimated landmarks @@ -1155,6 +1273,8 @@ def plot_map(self, marker=None, ellipse=None, confidence = 0.95): :type ellipse: dict, optional :param confidence: ellipse confidence interval, defaults to 0.95 :type confidence: float, optional + :param block: hold plot until figure is closed, defaults to None + :type block: bool, optional Plot a marker and covariance ellipses for each estimated landmark. @@ -1162,12 +1282,12 @@ def plot_map(self, marker=None, ellipse=None, confidence = 0.95): """ if marker is None: marker = { - 'marker': '+', - 'markersize': 10, - 'markerfacecolor': 'red', - 'linewidth': 0 + "marker": "+", + "markersize": 10, + "markerfacecolor": "red", + "linewidth": 0, } - + xm = self._x_est P = self._P_est if self._est_vehicle: @@ -1176,19 +1296,33 @@ def plot_map(self, marker=None, ellipse=None, confidence = 0.95): # mark the estimate as a point xm = xm.reshape((-1, 2)) # arrange as Nx2 - plt.plot(xm[:, 0], xm[:, 1], label='estimated landmark', **marker) + plt.plot(xm[:, 0], xm[:, 1], label="estimated landmark", **marker) # add an ellipse if ellipse is not None: for i in range(xm.shape[0]): - Pi = self.P_est[i: i+2, i: i+2] + Pi = self.P_est[i : i + 2, i : i + 2] # put ellipse in the legend only once if i == 0: - base.plot_ellipse(Pi, centre=xm[i, :], confidence=confidence, - inverted=True, label=f"{confidence*100:.3g}% confidence", **ellipse) + base.plot_ellipse( + Pi, + centre=xm[i, :], + confidence=confidence, + inverted=True, + label=f"{confidence*100:.3g}% confidence", + **ellipse, + ) else: - base.plot_ellipse(Pi, centre=xm[i, :], confidence=confidence, inverted=True, **ellipse) + base.plot_ellipse( + Pi, + centre=xm[i, :], + confidence=confidence, + inverted=True, + **ellipse, + ) # plot_ellipse( P * chi2inv_rtb(opt.confidence, 2), xf, args{:}); + if block is not None: + plt.show(block=block) def get_P(self, k=None): """ @@ -1229,7 +1363,6 @@ def get_Pnorm(self, k=None): p = [np.sqrt(np.linalg.det(h.P)) for h in self._history] return np.array(p) - def disp_P(self, P, colorbar=False): """ Display covariance matrix @@ -1251,12 +1384,12 @@ def disp_P(self, P, colorbar=False): z = np.log10(abs(P)) mn = min(z[~np.isinf(z)]) z[np.isinf(z)] = mn - plt.xlabel('State') - plt.ylabel('State') + plt.xlabel("State") + plt.ylabel("State") - plt.imshow(z, cmap='Reds') + plt.imshow(z, cmap="Reds") if colorbar is True: - plt.colorbar(label='log covariance') + plt.colorbar(label="log covariance") elif isinstance(colorbar, dict): plt.colorbar(**colorbar) @@ -1282,28 +1415,30 @@ def get_transform(self, map): p.append(map[lm_id]) q.append(self.landmark_x(lm_id)) - p = np.array(p) - q = np.array(q) + p = np.array(p).T + q = np.array(q).T T = base.points2tr2(p, q) return SE2(T) + if __name__ == "__main__": from roboticstoolbox import * - robot=Bicycle() + + V = np.diag([0.02, np.deg2rad(0.5)]) ** 2 + robot = Bicycle(covar=V, animation="car") robot.control = RandomPath(workspace=10) - P0=np.diag([1,1,1]) - V=np.diag([1,1]) - ekf = EKF(robot=(robot, V), P0=P0) + P0 = np.diag([1, 1, 1]) + V = np.diag([1, 1]) + ekf = EKF(robot=(robot, V), P0=P0, animate=False) print(ekf) - + ekf.run_animation(T=20, format="mp4", file="ekf.mp4") # from roboticstoolbox import Bicycle # ### RVC2: Chapter 6 - # ## 6.1 Dead reckoning # ## 6.1.1 Modeling the vehicle @@ -1350,7 +1485,6 @@ def get_transform(self, map): # # sensor = RangeBearingSensor(veh, map, 'covar', W) - # # [z,i] = sensor.reading() # # map.landmark(17) @@ -1384,7 +1518,6 @@ def get_transform(self, map): # # ekf.plot_map('g'); # # veh.plot_xy('b'); - # # ekf.landmarks(:,6) # # ekf.x_est(19:20)' @@ -1458,7 +1591,6 @@ def get_transform(self, map): # # clf # # pf.plot_pdf() - # # 6.8 Application: Scanning laser rangefinder # # Laser odometry @@ -1481,7 +1613,6 @@ def get_transform(self, map): # # pg.time(2581)-pg.time(2580) - # # Laser-based map building # # map = pg.scanmap(); - # # pg.plot_occgrid(map) \ No newline at end of file + # # pg.plot_occgrid(map) diff --git a/roboticstoolbox/mobile/LatticePlanner.py b/roboticstoolbox/mobile/LatticePlanner.py index 63970502e..64a528f7c 100644 --- a/roboticstoolbox/mobile/LatticePlanner.py +++ b/roboticstoolbox/mobile/LatticePlanner.py @@ -7,29 +7,32 @@ from roboticstoolbox.mobile.OccGrid import BinaryOccupancyGrid from collections import namedtuple + def make_arc(dir, radius=1, npoints=20): points = [] - if dir == 'S': + if dir == "S": points.append((0, 0)) points.append((radius, 0)) - elif dir == 'L': - for theta in np.linspace(0, np.pi/2, npoints): + elif dir == "L": + for theta in np.linspace(0, np.pi / 2, npoints): x = radius * np.sin(theta) y = radius * (1 - np.cos(theta)) points.append((x, y)) - elif dir == 'R': - for theta in np.linspace(0, np.pi/2, npoints): + elif dir == "R": + for theta in np.linspace(0, np.pi / 2, npoints): x = radius * np.sin(theta) y = radius * (-1 + np.cos(theta)) points.append((x, y)) - + return np.array(points).T + arcs = {} + class LatticeVertex(DVertex): def __init__(self, move=None, pose=None, name=None): super().__init__(name=name) @@ -41,9 +44,10 @@ def icoord(self): xyt = self.coord ix = int(round(xyt[0])) iy = int(round(xyt[1])) - it = int(round(xyt[2]*2/np.pi)) + it = int(round(xyt[2] * 2 / np.pi)) return f"({ix:d},{iy:d},{it:d}), {self.name}" + class LatticeEdge(Edge): def __init__(self, v1, v2, cost, pose, move): super().__init__(v1, v2, cost) @@ -57,11 +61,11 @@ def plot(self, configspace=False, unwrap=False, **kwargs): if configspace: # 3D plot theta0 = self.pose.theta() - if self.move == 'L': + if self.move == "L": thetaf = theta0 + np.pi / 2 - elif self.move == 'R': + elif self.move == "R": thetaf = theta0 - np.pi / 2 - elif self.move == 'S': + elif self.move == "S": thetaf = theta0 theta = np.linspace(theta0, thetaf, self.arc.shape[1]) if unwrap: @@ -130,31 +134,33 @@ class LatticePlanner(PlannerBase): :seealso: :meth:`plan` :meth:`query` :class:`PlannerBase` """ - def __init__(self, costs=None, root=(0,0,0), **kwargs): + def __init__(self, costs=None, root=(0, 0, 0), **kwargs): global arcs super().__init__(ndims=3, **kwargs) - self.poses = [SE2(1, 0, 0), SE2(1, 1, np.pi/2), SE2(1, -1, -np.pi/2)] - self.moves = ['S', 'L', 'R'] + self.poses = [SE2(1, 0, 0), SE2(1, 1, np.pi / 2), SE2(1, -1, -np.pi / 2)] + self.moves = ["S", "L", "R"] if costs is None: - costs = [1, np.pi/2, np.pi/2] + costs = [1, np.pi / 2, np.pi / 2] self.costs = costs self.root = root - # create the set of possible moves for move in self.moves: arcs[move] = make_arc(move) def __str__(self): - s = super().__str__() + f"\n curvature={self.curvature}, stepsize={self.stepsize}" + s = ( + super().__str__() + + f"\n curvature={self.curvature}, stepsize={self.stepsize}" + ) def _icoord(self, xyt): ix = int(round(xyt[0])) iy = int(round(xyt[1])) - it = int(round(xyt[2]*2/np.pi)) + it = int(round(xyt[2] * 2 / np.pi)) return f"({ix:d},{iy:d},{it:d})" def plan(self, iterations=None, verbose=False, summary=False): @@ -172,13 +178,15 @@ def plan(self, iterations=None, verbose=False, summary=False): :seealso: :meth:`query` """ if iterations is None and self.occgrid is None: - raise ValueError('iterations must be finite if no occupancy grid is specified') + raise ValueError( + "iterations must be finite if no occupancy grid is specified" + ) - self.graph = DGraph(metric='SE2') + self.graph = DGraph(metric="SE2") # add root vertex to the graph, place it in the frontier v0 = LatticeVertex(pose=SE2(self.root)) - self.graph.add_vertex(v0, name='0') + self.graph.add_vertex(v0, name="0") frontier = [v0] iteration = 0 @@ -187,7 +195,7 @@ def plan(self, iterations=None, verbose=False, summary=False): newfrontier = [] for vertex in frontier: if verbose: - print('EXPAND:', vertex.icoord()) + print("EXPAND:", vertex.icoord()) for pose, move, cost in zip(self.poses, self.moves, self.costs): newpose = vertex.pose * pose @@ -195,11 +203,11 @@ def plan(self, iterations=None, verbose=False, summary=False): # theta is guaranteed to be in range [-pi, pi) if verbose: - print(' MOVE', move, self._icoord(xyt)) + print(" MOVE", move, self._icoord(xyt)) if self.isoccupied(xyt[:2]): if verbose: - print(' is occupied') + print(" is occupied") continue vclose, d = self.graph.closest(xyt) @@ -213,9 +221,11 @@ def plan(self, iterations=None, verbose=False, summary=False): # vnew = LatticeVertex(move, newpose, name=f"{ix:d},{iy:d},{it:d}") self.graph.add_vertex(vnew) if verbose: - print(' add to graph as', vnew.name) + print(" add to graph as", vnew.name) - edge = LatticeEdge(vertex, vnew, cost=cost, pose=vertex.pose, move=move) + edge = LatticeEdge( + vertex, vnew, cost=cost, pose=vertex.pose, move=move + ) # connect it into the graph, add to frontier vertex.connect(vnew, edge=edge) @@ -226,8 +236,10 @@ def plan(self, iterations=None, verbose=False, summary=False): # connect it into the graph, don't add to frontier if verbose: - print(' already in graph, connect to', vclose.icoord()) - edge = LatticeEdge(vertex, vclose, cost=cost, pose=vertex.pose, move=move) + print(" already in graph, connect to", vclose.icoord()) + edge = LatticeEdge( + vertex, vclose, cost=cost, pose=vertex.pose, move=move + ) vertex.connect(vclose, edge=edge) frontier = newfrontier @@ -242,7 +254,7 @@ def plan(self, iterations=None, verbose=False, summary=False): break if summary: print(f"{self.graph.n} vertices and {self.graph.ne} edges created") - + def query(self, start, goal): r""" Find a path through the lattice @@ -273,17 +285,17 @@ def query(self, start, goal): vs, ds = self.graph.closest(start) if ds > 0.001: - raise ValueError('start configuration is not in the lattice') + raise ValueError("start configuration is not in the lattice") vg, dg = self.graph.closest(goal) if dg > 0.001: - raise ValueError('goal configuration is not in the lattice') + raise ValueError("goal configuration is not in the lattice") try: path, cost, _ = self.graph.path_Astar(vs, vg, verbose=False) except TypeError: - raise RuntimeError('no path found') from None + raise RuntimeError("no path found") from None - status = namedtuple('LatticeStatus', ['cost', 'segments', 'edges']) + status = namedtuple("LatticeStatus", ["cost", "segments", "edges"]) segments = [] edges = [] @@ -296,8 +308,8 @@ def query(self, start, goal): def plot(self, path=None, **kwargs): super().plot(**kwargs) - - if kwargs.get('configspace', False): + + if kwargs.get("configspace", False): # 3D plot for k, vertex in enumerate(self.graph): @@ -305,10 +317,10 @@ def plot(self, path=None, **kwargs): # if k == 0: # plt.plot(vertex.coord[0], vertex.coord[1], vertex.coord[2], 'k>', markersize=10) # else: - plt.plot(vertex.coord[0], vertex.coord[1], vertex.coord[2], 'bo') + plt.plot(vertex.coord[0], vertex.coord[1], vertex.coord[2], "bo") for edge in self.graph.edges(): - edge.plot(color='k', **kwargs) + edge.plot(color="k", **kwargs) if path is not None: for p, n in zip(path[:-1], path[1:]): @@ -317,10 +329,10 @@ def plot(self, path=None, **kwargs): vn, _ = self.graph.closest(n) e = vp.edgeto(vn) - #e.plot(color='b', linewidth=4) - - e.plot(color='k', linewidth=4) - e.plot(color='yellow', linewidth=3, dashes=(4,4)) + # e.plot(color='b', linewidth=4) + + e.plot(color="k", linewidth=4) + e.plot(color="yellow", linewidth=3, dashes=(4, 4)) else: # 2D plot for k, vertex in enumerate(self.graph): @@ -328,10 +340,10 @@ def plot(self, path=None, **kwargs): # if k == 0: # plt.plot(vertex.coord[0], vertex.coord[1], 'k>', markersize=10) # else: - plt.plot(vertex.coord[0], vertex.coord[1], 'bo') + plt.plot(vertex.coord[0], vertex.coord[1], "bo") for edge in self.graph.edges(): - edge.plot(color='k') + edge.plot(color="k") if path is not None: for p, n in zip(path[:-1], path[1:]): @@ -340,18 +352,17 @@ def plot(self, path=None, **kwargs): vn, _ = self.graph.closest(n) e = vp.edgeto(vn) - #e.plot(color='b', linewidth=4) - - e.plot(color='k', linewidth=4) - e.plot(color='yellow', linewidth=3, dashes=(4,4)) + # e.plot(color='b', linewidth=4) + e.plot(color="k", linewidth=4) + e.plot(color="yellow", linewidth=3, dashes=(4, 4)) if __name__ == "__main__": lattice = LatticePlanner() lattice.plan(iterations=6) - path = lattice.query(start=(0, 0, np.pi/2), goal=(1, 1, 0)) + path = lattice.query(start=(0, 0, np.pi / 2), goal=(1, 1, 0)) print(path) # og = BinaryOccupancyGrid(workspace=[-5, 5, -5, 5], value=False) @@ -388,7 +399,6 @@ def plot(self, path=None, **kwargs): # print(path) # print(status) - # lattice.plot(path=path) # plt.show(block=True) diff --git a/roboticstoolbox/mobile/OccGrid.py b/roboticstoolbox/mobile/OccGrid.py index e91ec4976..f09a9cc67 100644 --- a/roboticstoolbox/mobile/OccGrid.py +++ b/roboticstoolbox/mobile/OccGrid.py @@ -3,7 +3,7 @@ from spatialmath import base import scipy.ndimage as sp from abc import ABC -from spatialmath import base +import spatialmath.base as smb from spatialmath.geom2d import Polygon2 @@ -30,7 +30,7 @@ def __init__(self, workspace=None, name=None, **unused): ============== ======= ======= """ if workspace is not None: - workspace = base.expand_dims(workspace) + workspace = smb.expand_dims(workspace) self._workspace = workspace self.dx = workspace[1] - workspace[0] self.dy = workspace[3] - workspace[2] @@ -66,7 +66,7 @@ def __init__(self, grid=None, origin=(0, 0), value=0, cellsize=1, **kwargs): if grid is not None: self._grid = grid - self._origin = base.getvector(origin, 2) + self._origin = smb.getvector(origin, 2) elif self._workspace is not None: self._grid = np.full( @@ -240,7 +240,7 @@ def g2w(self, p): The grid cell size and offset are used to convert occupancy grid coordinate ``p`` to a world coordinate. """ - p = base.getvector(p, 2) + p = smb.getvector(p, 2) return p * self._cellsize + self._origin def w2g(self, p): @@ -258,7 +258,7 @@ def w2g(self, p): """ return (np.round((p - self._origin) / self._cellsize)).astype(int) - def plot(self, map=None, ax=None, block=False, **kwargs): + def plot(self, map=None, ax=None, block=None, **kwargs): """ Plot the occupancy grid (superclass) @@ -267,7 +267,7 @@ def plot(self, map=None, ax=None, block=False, **kwargs): :type map: ndarray(N,M), optional :param ax: matplotlib axes to plot into, defaults to None :type ax: Axes2D, optional - :param block: block until plot is dismissed, defaults to False + :param block: block until plot is dismissed, defaults to None :type block: bool, optional :param kwargs: arguments passed to ``imshow`` @@ -279,7 +279,7 @@ def plot(self, map=None, ax=None, block=False, **kwargs): """ - ax = base.axes_logic(ax, 2) + ax = smb.axes_logic(ax, 2) if map is None: map = self._grid @@ -288,7 +288,8 @@ def plot(self, map=None, ax=None, block=False, **kwargs): ax.imshow(map, origin="lower", interpolation=None, **kwargs) ax.set_xlabel("x") ax.set_ylabel("y") - plt.show(block=block) + if block is not None: + plt.show(block=block) def line_w(self, p1, p2): """ @@ -316,7 +317,7 @@ def line_w(self, p1, p2): def _line(self, p1, p2): - x, y = base.bresenham(p1, p2, array=self.grid) + x, y = smb.bresenham(p1, p2) z = np.ravel_multi_index(np.vstack((y, x)), self.grid.shape) return z @@ -468,9 +469,12 @@ def __init__(self, workspace=None, polygons=[]): :param workspace: dimensions of 2D plot area, defaults to (-10:10) x (-10:10), see :func:`~spatialmath.base.graphics.plotvol2` :type workspace: float, array_like(2), array_like(4) - :param polygons: _description_, defaults to [] + :param polygons: obstacle polygons, defaults to [] :type polygons: list, optional + The obstacle polygons are specified as instances of :class:`~spatialmath.geom2d.Polygon2` + or ndarray(2,N). + The workspace can be specified in several ways: ============== ======= ======= @@ -518,13 +522,14 @@ def iscollision(self, polygon): """ return polygon.intersects(self.polygons) - def plot(self, block=False): - base.plotvol2(self.workspace) + def plot(self, block=None): + smb.plotvol2(self.workspace) for polygon in self.polygons: polygon.plot(color="r") - plt.show(block=block) + if block is not None: + plt.show(block=block) def isoccupied(self, p): """ diff --git a/roboticstoolbox/mobile/PRMPlanner.py b/roboticstoolbox/mobile/PRMPlanner.py index 5db458465..d2ccc6d39 100644 --- a/roboticstoolbox/mobile/PRMPlanner.py +++ b/roboticstoolbox/mobile/PRMPlanner.py @@ -9,12 +9,14 @@ # from spatialmath.base.animate import Animate from spatialmath.base.transforms2d import * from spatialmath.base.vectors import * + # from spatialmath.pose2d import SE2 # from spatialmath.base import animate from scipy.ndimage import * from matplotlib import cm, pyplot as plt from roboticstoolbox.mobile.PlannerBase import PlannerBase from pgraph import UGraph + # from progress.bar import FillingCirclesBar @@ -61,6 +63,7 @@ class PRMPlanner(PlannerBase): :author: Peter Corke :seealso: :class:`PlannerBase` """ + def __init__(self, occgrid=None, npoints=100, dist_thresh=None, **kwargs): super().__init__(occgrid, ndims=2, **kwargs) @@ -81,7 +84,7 @@ def __init__(self, occgrid=None, npoints=100, dist_thresh=None, **kwargs): def __str__(self): s = super().__str__() if self.graph is not None: - s += '\n ' + str(self.graph) + s += "\n " + str(self.graph) return s @property @@ -125,7 +128,6 @@ def graph(self): """ return self._graph - def _create_roadmap(self, npoints, dist_thresh, animate=None): # a = Animate(animate, fps=5) self.progress_start(npoints) @@ -152,17 +154,17 @@ def _create_roadmap(self, npoints, dist_thresh, animate=None): # skip self if vertex is othervertex: continue - # add (distance, vertex) tuples to list + # add (distance, vertex) tuples to list distances.append((vertex.distance(othervertex), othervertex)) - + # sort into ascending distance - distances.sort(key=lambda x:x[0]) + distances.sort(key=lambda x: x[0]) # create edges to vertex if permissible for distance, othervertex in distances: # test if below distance threshold if dist_thresh is not None and distance > dist_thresh: - break # sorted into ascending order, so we are done + break # sorted into ascending order, so we are done # test if obstacle free path connecting them if self._test_path(vertex, othervertex): @@ -171,10 +173,10 @@ def _create_roadmap(self, npoints, dist_thresh, animate=None): self.progress_next() self.progress_end() - # if animate is not None: - # self.plot() - # if not np.empty(movie): - # a.add() + # if animate is not None: + # self.plot() + # if not np.empty(movie): + # a.add() def _test_path(self, v1, v2, npoints=None): # vector from v1 to v2 @@ -217,7 +219,7 @@ def plan(self, npoints=None, dist_thresh=None, animate=None): :seealso: :meth:`query` :meth:`graph` """ - self.message('create the graph') + self.message("create the graph") if npoints is None: npoints = self.npoints @@ -252,7 +254,7 @@ def query(self, start, goal, **kwargs): """ if self.graph.n == 0: - raise RuntimeError('no plan computed') + raise RuntimeError("no plan computed") super().query(start=start, goal=goal, next=False, **kwargs) @@ -263,7 +265,7 @@ def query(self, start, goal, **kwargs): # find A* path through the roadmap out = self.graph.path_Astar(vstart, vgoal) if out is None: - raise RuntimeError('no path found') + raise RuntimeError("no path found") path = [v.coord for v in out[0]] path.insert(0, start) # insert start at head of path @@ -281,7 +283,7 @@ def plot(self, *args, vertex={}, edge={}, **kwargs): :type edge: dict, optional Displays: - + - the planner background (obstacles) - the roadmap graph - the path @@ -289,30 +291,46 @@ def plot(self, *args, vertex={}, edge={}, **kwargs): :seealso: :meth:`UGraph.plot` """ # plot the obstacles and path - super().plot(*args, **kwargs) + ax = super().plot(*args, **kwargs) + print("plotted background") vertex = {**dict(markersize=4), **vertex} edge = {**dict(linewidth=0.5), **edge} # add the roadmap graph - self.graph.plot(text=False, vopt=vertex, eopt=edge) + self.graph.plot(text=False, vopt=vertex, eopt=edge, ax=ax) + print("plotted roadmap") if __name__ == "__main__": import matplotlib.pyplot as plt + from roboticstoolbox import * + + house = rtb_load_matfile("data/house.mat") + floorplan = house["floorplan"] + places = house["places"] + + occgrid = floorplan.copy() + + prm = PRMPlanner(occgrid=floorplan, seed=0) + prm.plan(npoints=50) + prm.plot() # start and goal position - start = (10, 10) - goal = (50, 50) + # start = (10, 10) + # goal = (50, 50) + + # occgrid = np.zeros((100, 100)) + # occgrid[20:40, 15:30] = 1 + + # prm = PRMPlanner(occgrid=occgrid, verbose=True) - occgrid = np.zeros((100, 100)) - occgrid[20:40, 15:30] = 1 + # prm.plan() - prm = PRMPlanner(occgrid=occgrid, verbose=True) + # prm.plot() - prm.plan() - path = prm.query(start, goal) - print(path) + # path = prm.query(start, goal) + # print(path) - prm.plot(path, path_marker=dict(zorder=8, linewidth=2, markersize=6, color='k')) - prm.plot(ax=plt.gca(), text=False, vertex=dict(markersize=4), edge=dict(linewidth=0.5)) + # prm.plot(path, path_marker=dict(zorder=8, linewidth=2, markersize=6, color='k')) + # prm.plot(ax=plt.gca(), text=False, vertex=dict(markersize=4), edge=dict(linewidth=0.5)) plt.show(block=True) diff --git a/roboticstoolbox/mobile/ParticleFilter.py b/roboticstoolbox/mobile/ParticleFilter.py index 2e0ebd788..8744a1bea 100644 --- a/roboticstoolbox/mobile/ParticleFilter.py +++ b/roboticstoolbox/mobile/ParticleFilter.py @@ -14,7 +14,9 @@ import numpy as np import scipy as sp import matplotlib.pyplot as plt -from spatialmath import base +from matplotlib import animation + +import spatialmath.base as smb """ Monte-carlo based localisation for estimating vehicle pose based on @@ -23,10 +25,22 @@ # TODO: refactor this and EKF, RNG, history, common plots, animation, movie + class ParticleFilter: - - def __init__(self, robot, sensor, R, L, nparticles=500, seed=0, x0=None, - verbose=False, animate=False, history=True, workspace=None): + def __init__( + self, + robot, + sensor, + R, + L, + nparticles=500, + seed=0, + x0=None, + verbose=False, + animate=False, + history=True, + workspace=None, + ): """ Particle filter @@ -54,7 +68,7 @@ def __init__(self, robot, sensor, R, L, nparticles=500, seed=0, x0=None, This class implements a Monte-Carlo estimator or particle filter for vehicle state, based on odometry, a landmark map, and landmark observations. The state of each particle is a possible vehicle - configuration :math:`(x,y,\theta)`. Bootstrap particle resampling is + configuration :math:`(x,y,\theta)`. Bootstrap particle resampling is used. The working area is defined by ``workspace`` or inherited from the @@ -116,19 +130,19 @@ def __init__(self, robot, sensor, R, L, nparticles=500, seed=0, x0=None, self._random = np.random.default_rng(seed) self._seed = seed - self._keep_history = history # keep history + self._keep_history = history # keep history self._htuple = namedtuple("PFlog", "t odo xest std weights") if workspace is not None: - self._dim = base.expand_dims(workspace) + self._dim = smb.expand_dims(workspace) else: self._dim = sensor.map.workspace self._workspace = self.robot.workspace - self._init() + # self._init() def __str__(self): - #ParticleFilter.char Convert to string + # ParticleFilter.char Convert to string # # PF.char() is a string representing the state of the ParticleFilter # object in human-readable form. @@ -136,14 +150,14 @@ def __str__(self): # See also ParticleFilter.display. def indent(s, n=2): - spaces = ' ' * n - return s.replace('\n', '\n' + spaces) + spaces = " " * n + return s.replace("\n", "\n" + spaces) s = f"ParticleFilter object: {self.nparticles} particles" - s += '\nR: ' + base.array2str(self.R) - s += '\nL: ' + base.array2str(self.L) + s += "\nR: " + smb.array2str(self.R) + s += "\nL: " + smb.array2str(self.L) if self.robot is not None: - s += indent("\nrobot: " + str(self.robot)) + s += indent("\nrobot: " + str(self.robot)) if self.sensor is not None: s += indent("\nsensor: " + str(self.sensor)) @@ -158,7 +172,7 @@ def robot(self): :rtype: :class:`VehicleBase` subclass """ return self._robot - + @property def sensor(self): """ @@ -178,7 +192,7 @@ def map(self): :rtype: :class:`LandmarkMap` subclass """ return self._map - + @property def verbose(self): """ @@ -188,7 +202,7 @@ def verbose(self): :rtype: bool """ return self._verbose - + @property def history(self): """ @@ -201,8 +215,8 @@ def history(self): list. It contains, for that time step, estimated state and covariance, and sensor observation. - :seealso: :meth:`get_t` :meth:`get_xy` :meth:`get_std` - :meth:`get_Pnorm` + :seealso: :meth:`get_t` :meth:`get_xy` :meth:`get_std` + :meth:`get_Pnorm` """ return self._history @@ -219,7 +233,6 @@ def workspace(self): """ return self._workspace - @property def random(self): """ @@ -242,8 +255,8 @@ def random(self): """ return self._random - def _init(self, x0=None): - #ParticleFilter.init Initialize the particle filter + def _init(self, x0=None, animate=False, ax=None): + # ParticleFilter.init Initialize the particle filter # # PF.init() initializes the particle distribution and clears the # history. @@ -256,7 +269,7 @@ def _init(self, x0=None): self.robot.init() self.sensor.init() - #clear the history + # clear the history self._history = [] # create a new private random number generator @@ -271,27 +284,41 @@ def _init(self, x0=None): if x0 is None: # create initial particle distribution as uniformly randomly distributed # over the map workspace and heading angles - x = self.random.uniform(self.workspace[0], self.workspace[1], size=(self.nparticles,)) - y = self.random.uniform(self.workspace[2], self.workspace[3], size=(self.nparticles,)) + x = self.random.uniform( + self.workspace[0], self.workspace[1], size=(self.nparticles,) + ) + y = self.random.uniform( + self.workspace[2], self.workspace[3], size=(self.nparticles,) + ) t = self.random.uniform(-np.pi, np.pi, size=(self.nparticles,)) - self.x = np.c_[x, y, t] + self.x = np.c_[x, y, t] + + if animate: + # display the initial particles + (self.h,) = ax.plot( + self.x[:, 0], + self.x[:, 1], + "go", + zorder=0, + markersize=3, + markeredgecolor="none", + alpha=0.3, + label="particle", + ) self.weight = np.ones((self.nparticles,)) - def run(self, T=10, x0=None): """ Run the particle filter simulation :param T: maximum simulation time in seconds :type T: float - :param animate: animate motion of vehicle, defaults to False - :type animate: bool, optional - :param movie: name of movie file to create, defaults to None - :type movie: str, optional + :param x0: Initial state, defaults to value given to Vehicle constructor + :type x0: array_like(3) or array_like(2) Simulates the motion of a vehicle (under the control of a driving agent) - and the EKF estimator. The steps are: + and the particle-filter estimator. The steps are: - initialize the filter, vehicle and vehicle driver agent, sensor - for each time step: @@ -301,7 +328,7 @@ def run(self, T=10, x0=None): - execute the EKF - save information as a namedtuple to the history list for later display - :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` + :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` :meth:`get_xy` :meth:`get_t` :meth:`get_std` :meth:`plot_xy` """ @@ -311,32 +338,172 @@ def run(self, T=10, x0=None): # anim = Animate(opt.movie) # display the initial particles + ax = smb.axes_logic(None, 2) if self._animate: - self.h, = plt.plot(self.x[:, 0], self.x[:, 1], 'go', zorder=0, markersize=3, markeredgecolor='none', alpha=0.3, label='particle') + (self.h,) = ax.plot( + self.x[:, 0], + self.x[:, 1], + "go", + zorder=0, + markersize=3, + markeredgecolor="none", + alpha=0.3, + label="particle", + ) # set(self.h, 'Tag', 'particles') - + # self.robot.plot() # iterate over time + import time + for i in range(round(T / self.robot.dt)): self._step() + # time.sleep(0.2) + plt.pause(0.2) + # plt.draw() # anim.add() # anim.close() + def run_animation(self, T=10, x0=None, format=None, file=None): + """ + Run the particle filter simulation + + :param T: maximum simulation time in seconds + :type T: float + :param x0: Initial state, defaults to value given to Vehicle constructor + :type x0: array_like(3) or array_like(2) + :param format: Output format + :type format: str, optional + :param file: File name + :type file: str, optional + :return: Matplotlib animation object + :rtype: :meth:`matplotlib.animation.FuncAnimation` + + Simulates the motion of a vehicle (under the control of a driving agent) + and the particle-filter estimator and returns an animation + in various formats:: + + ``format`` ``file`` description + ============ ========= ============================ + ``"html"`` str, None return HTML5 video + ``"jshtml"`` str, None return JS+HTML video + ``"gif"`` str return animated GIF + ``"mp4"`` str return MP4/H264 video + ``None`` return a ``FuncAnimation`` object + + The allowables types for ``file`` are given in the second column. A str + value is the file name. If ``None`` is an option then return the video as a string. + + For the last case, a reference to the animation object must be held if used for + animation in a Jupyter cell:: + + anim = robot.run_animation(T=20) + + The steps are: + + - initialize the filter, vehicle and vehicle driver agent, sensor + - for each time step: + + - step the vehicle and its driver agent, obtain odometry + - take a sensor reading + - execute the EKF + - save information as a namedtuple to the history list for later display + + :seealso: :meth:`history` :meth:`landmark` :meth:`landmarks` + :meth:`get_xy` :meth:`get_t` :meth:`get_std` + :meth:`plot_xy` + """ + + fig, ax = plt.subplots() + + nframes = round(T / self.robot.dt) + anim = animation.FuncAnimation( + fig=fig, + # func=lambda i: self._step(animate=True, pause=False), + # init_func=lambda: self._init(animate=True), + func=lambda i: self._step(), + init_func=lambda: self._init(ax=ax, animate=True), + frames=nframes, + interval=self.robot.dt * 1000, + blit=False, + repeat=False, + ) + # anim._interval = self.dt*1000/2 + # anim._repeat = True + ret = None + if format == "html": + ret = anim.to_html5_video() # convert to embeddable HTML5 animation + elif format == "jshtml": + ret = anim.to_jshtml() # convert to embeddable Javascript/HTML animation + elif format == "gif": + anim.save( + file, writer=animation.PillowWriter(fps=1 / self.dt) + ) # convert to GIF + ret = None + elif format == "mp4": + anim.save( + file, writer=animation.FFMpegWriter(fps=1 / self.dt) + ) # convert to mp4/H264 + ret = None + elif format == None: + # return the anim object + return anim + else: + raise ValueError("unknown format") + + if ret is not None and file is not None: + with open(file, "w") as f: + f.write(ret) + ret = None + plt.close(fig) + return ret + # self._init(x0=x0) + + # # anim = Animate(opt.movie) + + # # display the initial particles + # ax = smb.axes_logic(None, 2) + # if self._animate: + # (self.h,) = ax.plot( + # self.x[:, 0], + # self.x[:, 1], + # "go", + # zorder=0, + # markersize=3, + # markeredgecolor="none", + # alpha=0.3, + # label="particle", + # ) + # # set(self.h, 'Tag', 'particles') + + # # self.robot.plot() + + # # iterate over time + # import time + + # for i in range(round(T / self.robot.dt)): + # self._step() + # # time.sleep(0.2) + # plt.pause(0.2) + # # plt.draw() + # # anim.add() + # # anim.close() + def _step(self): - - #fprintf('---- step\n') - odo = self.robot.step() # move the robot + + # fprintf('---- step\n') + odo = self.robot.step(animate=self._animate) # move the robot # update the particles based on odometry self._predict(odo) # get a sensor reading - z, lm_id = self.sensor.reading() + z, lm_id = self.sensor.reading() if z is not None: self._observe(z, lm_id) - #fprintf(' observe beacon #d\n', lm_id) + # fprintf(' observe beacon #d\n', lm_id) self._select() @@ -345,7 +512,9 @@ def _step(self): std_est = self.x.std(axis=0) # std is more complex for angles, need to account for 2pi wrap - std_est[2] = np.sqrt(np.sum(base.angdiff(self.x[:,2], x_est[2]) ** 2)) / (self.nparticles-1) + std_est[2] = np.sqrt(np.sum(smb.angdiff(self.x[:, 2], x_est[2]) ** 2)) / ( + self.nparticles - 1 + ) # display the updated particles # set(self.h, 'Xdata', self.x(:,1), 'Ydata', self.x(:,2), 'Zdata', self.x(:,3)) @@ -353,17 +522,13 @@ def _step(self): if self._animate: self.h.set_xdata(self.x[:, 0]) self.h.set_ydata(self.x[:, 1]) - + # if ~isempty(self.anim) # self.anim.add() if self._keep_history: hist = self._htuple( - self.robot._t, - odo.copy(), - x_est, - std_est, - self.weight.copy() + self.robot._t, odo.copy(), x_est, std_est, self.weight.copy() ) self._history.append(hist) @@ -377,17 +542,17 @@ def plot_pdf(self): segment of height equal to particle weight. """ - ax = base.plotvol3() + ax = smb.plotvol3() for (x, y, t), weight in zip(self.x, self.weight): # ax.plot([x, x], [y, y], [0, weight], 'r') - ax.plot([x, x], [y, y], [0, weight], 'skyblue', linewidth=3) - ax.plot(x, y, weight, 'k.', markersize=6) - - plt.grid(True) - plt.xlabel('X') - plt.ylabel('Y') - plt.xlim() - ax.set_zlabel('particle weight') + ax.plot([x, x], [y, y], [0, weight], "skyblue", linewidth=3) + ax.plot(x, y, weight, "k.", markersize=6) + + ax.grid(True) + ax.set_xlabel("X") + ax.set_ylabel("Y") + ax.set_xlim() + ax.set_zlabel("particle weight") ax.view_init(29, 59) def _predict(self, odo): @@ -397,32 +562,32 @@ def _predict(self, odo): # Straightforward code: # # for i=1:self.nparticles - # x = self.robot.f( self.x(i,:), odo)' + sqrt(self.R)*self.randn[2,0] + # x = self.robot.f( self.x(i,:), odo)' + sqrt(self.R)*self.randn[2,0] # x[2] = angdiff(x[2]) # self.x(i,:) = x # # Vectorized code: - self.x = self.robot.f(self.x, odo) + \ - self.random.multivariate_normal((0, 0, 0), self.R, size=self.nparticles) - self.x[:, 2] = base.angdiff(self.x[:, 2]) - + self.x = self.robot.f(self.x, odo) + self.random.multivariate_normal( + (0, 0, 0), self.R, size=self.nparticles + ) + self.x[:, 2] = smb.angdiff(self.x[:, 2]) def _observe(self, z, lm_id): # step 3 # predict observation and score the particles - + # Straightforward code: # # for p = 1:self.nparticles # # what do we expect observation to be for this particle? # # use the sensor model h(.) # z_pred = self.sensor.h( self.x(p,:), lm_id) - # + # # # how different is it # innov[0] = z[0] - z_pred[0] # innov[1] = angdiff(z[1], z_pred[1]) - # + # # # get likelihood (new importance). Assume Gaussian but any PDF works! # # If predicted obs is very different from actual obs this score will be low # # ie. this particle is not very good at predicting the observation. @@ -436,30 +601,36 @@ def _observe(self, z, lm_id): invL = np.linalg.inv(self.L) z_pred = self.sensor.h(self.x, lm_id) z_pred[:, 0] = z[0] - z_pred[:, 0] - z_pred[:, 1] = base.angdiff(z[1], z_pred[:, 1]) - - LL = -0.5 * np.r_[invL[0,0], invL[1,1], 2*invL[0,1]] - e = np.c_[z_pred[:, 0]**2, z_pred[:, 1]**2, z_pred[:,0] * z_pred[:, 1]] @ LL - self.weight = np.exp(e) + self.w0 - + z_pred[:, 1] = smb.angdiff(z[1], z_pred[:, 1]) + LL = -0.5 * np.r_[invL[0, 0], invL[1, 1], 2 * invL[0, 1]] + e = ( + np.c_[z_pred[:, 0] ** 2, z_pred[:, 1] ** 2, z_pred[:, 0] * z_pred[:, 1]] + @ LL + ) + self.weight = np.exp(e) + self.w0 def _select(self): # step 4 # select particles based on their weights - # + # # particles with large weights will occupy a greater percentage of the # y axis in a cummulative plot cdf = np.cumsum(self.weight) / self.weight.sum() # so randomly (uniform) choosing y values is more likely to correspond to # better particles... - iselect = self.random.uniform(0, 1, size=(self.nparticles,)) + iselect = self.random.uniform(0, 1, size=(self.nparticles,)) # find the particle that corresponds to each y value (just a look up) - interpfun = sp.interpolate.interp1d(cdf, np.arange(self.nparticles), - assume_sorted=True, kind='nearest', fill_value='extrapolate') - inextgen = interpfun(iselect).astype(np.int) + interpfun = sp.interpolate.interp1d( + cdf, + np.arange(self.nparticles), + assume_sorted=True, + kind="nearest", + fill_value="extrapolate", + ) + inextgen = interpfun(iselect).astype(int) # copy selected particles for next generation.. self.x = self.x[inextgen, :] @@ -501,13 +672,13 @@ def get_std(self): """ return np.array([h.std for h in self._history]) - def plot_xy(self, block=False, **kwargs): + def plot_xy(self, block=None, **kwargs): r""" Plot estimated vehicle position :param args: position arguments passed to :meth:`~matplotlib.axes.Axes.plot` :param kwargs: keywords arguments passed to :meth:`~matplotlib.axes.Axes.plot` - :param block: hold plot until figure is closed, defaults to False + :param block: hold plot until figure is closed, defaults to None :type block: bool, optional Plot the estimated vehicle path in the xy-plane. @@ -516,4 +687,20 @@ def plot_xy(self, block=False, **kwargs): """ xyt = self.get_xyt() plt.plot(xyt[:, 0], xyt[:, 1], **kwargs) - # plt.show(block=block) \ No newline at end of file + if block is not None: + plt.show(block=block) + + +if __name__ == "__main__": + from roboticstoolbox import * + + map = LandmarkMap(20, workspace=10) + V = np.diag([0.02, np.deg2rad(0.5)]) ** 2 + robot = Bicycle(covar=V, animation="car", workspace=map) + robot.control = RandomPath(workspace=map) + W = np.diag([0.1, np.deg2rad(1)]) ** 2 + sensor = RangeBearingSensor(robot, map, covar=W, plot=True) + R = np.diag([0.1, 0.1, np.deg2rad(1)]) ** 2 + L = np.diag([0.1, 0.1]) + pf = ParticleFilter(robot, sensor=sensor, R=R, L=L, nparticles=1000, animate=True) + pf.run(T=10) diff --git a/roboticstoolbox/mobile/PlannerBase.py b/roboticstoolbox/mobile/PlannerBase.py index 9f43c48b5..d68487799 100644 --- a/roboticstoolbox/mobile/PlannerBase.py +++ b/roboticstoolbox/mobile/PlannerBase.py @@ -10,6 +10,8 @@ # from scipy.ndimage import interpolation from spatialmath.base.transforms2d import * from spatialmath.base.vectors import * +from spatialmath.base.argcheck import getvector +from spatialmath.base.graphics import axes_logic, plotvol2, axes_get_scale # from spatialmath import SE2, SE3 from matplotlib import cm @@ -123,8 +125,8 @@ def __str__(self): :rtype: str """ s = f"{self.__class__.__name__}: " - if self._occgrid0 is not None: - s += "\n " + str(self.occgrid) + if hasattr(self, "_occgrid") and self._occgrid is not None: + s += "\n " + str(self._occgrid) if self._start is not None: s += f"\n Start: {self.start}" if self._goal is not None: @@ -155,7 +157,7 @@ def start(self, start): if start is not None: if self.isoccupied(start): raise ValueError("Start location inside obstacle") - self._start = base.getvector(start) + self._start = getvector(start) @property def goal(self): @@ -185,7 +187,7 @@ def goal(self, goal): if goal is not None: if self.isoccupied(goal): raise ValueError("Goal location inside obstacle") - self._goal = base.getvector(goal) + self._goal = getvector(goal) @property def verbose(self): @@ -323,7 +325,7 @@ def validate_endpoint(self, p, dtype=None): :seealso: :meth:`isoccupied` :meth:`occgrid` """ if p is not None: - p = base.getvector(p, self._ndims, dtype=dtype) + p = getvector(p, self._ndims, dtype=dtype) if self.isoccupied(p): raise ValueError("Point is inside obstacle") return p @@ -474,7 +476,7 @@ def plot( start=None, goal=None, ax=None, - block=False, + block=None, bgargs={}, **unused, ): @@ -585,7 +587,6 @@ def plot( :seealso: :meth:`plot_bg` :func:`base.plot_poly` """ # create default markers - # passed to Matplotlib plot() if start_marker is None: start_marker = { @@ -635,7 +636,7 @@ def plot( if configspace and ndims < 3 and path is not None: raise ValueError(f"path should have {ndims} rows") - ax = base.axes_logic(ax, ndims) + ax = axes_logic(ax, ndims) # plot occupancy grid background if background: @@ -734,7 +735,7 @@ def plot( elif ndims == 2 and self._ndims == 3: # 2d projection of 3d plot, show start/goal configuration - scale = base.axes_get_scale(ax) / 10 + scale = axes_get_scale(ax) / 10 if self.marker is None: self.marker = VehiclePolygon(shape="car", scale=scale) @@ -764,7 +765,8 @@ def plot( else: ax.set_zlabel(r"$\theta$") - plt.show(block=block) + if block: + plt.show(block=block) return ax @@ -800,6 +802,7 @@ def plot_bg( ax=None, inflated=True, colorbar=True, + block=None, **unused, ): """ @@ -827,9 +830,9 @@ def plot_bg( return if isinstance(self._occgrid, BaseOccupancyGrid): - ax = base.plotvol2(dim=self._occgrid.workspace, ax=ax) + ax = plotvol2(dim=self._occgrid.workspace, ax=ax) else: - ax = base.axes_logic(ax, 2) + ax = axes_logic(ax, 2) # create color map for free space + obstacle: # free space, color index = 1, white, alpha=0 to allow background and grid lines to show @@ -882,8 +885,8 @@ def plot_bg( if colorbar is True: plt.colorbar( scalar_mappable_c_map, - shrink=0.75, - aspect=20 * 0.75, + # shrink=0.75, + # aspect=20 * 0.75, label="Distance", ) @@ -901,7 +904,7 @@ def plot_bg( # overlay obstacles c_map = mpl.colors.ListedColormap(colors) # self.occgrid.plot(image, cmap=c_map, zorder=1) - self.occgrid.plot(cmap=c_map, zorder=1) + self.occgrid.plot(cmap=c_map, zorder=1, ax=ax) ax.set_facecolor((1, 1, 1)) # create white background ax.set_xlabel("x (cells)") @@ -912,8 +915,9 @@ def plot_bg( # ax.set_xlim(ax.get_xlim()) # ax.set_ylim(ax.get_ylim()) - plt.draw() - plt.show(block=False) + # plt.draw() + if block is not None: + plt.show(block=block) def message(self, s, color=None): """ diff --git a/roboticstoolbox/mobile/PoseGraph.py b/roboticstoolbox/mobile/PoseGraph.py index cb3f9cb69..58c0c3384 100644 --- a/roboticstoolbox/mobile/PoseGraph.py +++ b/roboticstoolbox/mobile/PoseGraph.py @@ -1,8 +1,9 @@ -#PoseGraph Pose graph +# PoseGraph Pose graph import roboticstoolbox as rtb import pgraph -from spatialmath import base, SE2 +from spatialmath import SE2 +import spatialmath.base as smb import matplotlib.pyplot as plt import numpy as np import scipy as sp @@ -11,42 +12,53 @@ import math from pathlib import Path from progress.bar import FillingCirclesBar + + class PGVertex(pgraph.UVertex): - nvertices = 0 + nvertices = 0 # reset this before each PGGraph build def __init__(self, type, **kwargs): super().__init__(**kwargs) self.type = type self.index = PGVertex.nvertices PGVertex.nvertices += 1 + + class PGEdge(pgraph.Edge): def __init__(self, v1, v2, mean, info): super().__init__(v1, v2) self.mean = mean self.info = info + # The following method was inspired by, and modeled on, the MATLAB/Octave file + # ls-slam.m by Giorgio Grisetti and Gian Diego Tipaldi. + # + # The theory is covered in: + # G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, + # "A Tutorial on Graph-Based SLAM,” in IEEE Intelligent Transportation Systems Magazine, + # vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925. + def linear_factors(self): - #computes the Taylor expansion of the error function of the k_th edge - #vmeans: vertices positions - #eids: edge ids - #emeans: edge means + # computes the Taylor expansion of the error function of the k_th edge + # v: vertices positions + # z: edge means # k: edge number # e: e_k(x) # A: d e_k(x) / d(x_i) # B: d e_k(x) / d(x_j) - v = self.endpoints - v_i = v[0].coord - v_j = v[1].coord - z_ij = self.mean + v = self.endpoints + v_i = v[0].coord + v_j = v[1].coord + z_ij = self.mean # compute the homoeneous transforms of the previous solutions - zt_ij = base.trot2(z_ij[2], t=z_ij[0:2]) - vt_i = base.trot2(v_i[2], t=v_i[0:2]) - vt_j = base.trot2(v_j[2], t=v_j[0:2]) + zt_ij = smb.trot2(z_ij[2], t=z_ij[0:2]) + vt_i = smb.trot2(v_i[2], t=v_i[0:2]) + vt_j = smb.trot2(v_j[2], t=v_j[0:2]) # compute the displacement between x_i and x_j - f_ij = base.trinv2(vt_i) @ vt_j + f_ij = smb.trinv2(vt_i) @ vt_j # this below is too long to explain, to understand it derive it by hand theta_i = v_i[2] @@ -69,91 +81,113 @@ def linear_factors(self): [ 0, 0, 1 ], ]) # fmt: on - ztinv = base.trinv2(zt_ij) + ztinv = smb.trinv2(zt_ij) T = ztinv @ f_ij - e = base.tr2xyt(T) - ztinv[0:2,2] = 0 + e = smb.tr2xyt(T) + ztinv[0:2, 2] = 0 A = ztinv @ A B = ztinv @ B - return e, A, B + return e, A, B + class PoseGraph: - + # properties # graph - + # ngrid # center # cellsize def __init__(self, filename, lidar=False, verbose=False): + """ + Pose graph optimization for SLAM + + :param filename: name of file to load, G2O or TORO format, can be zipped + :type filename: str + :param lidar: process lidar data, defaults to False + :type lidar: bool, optional + :param verbose: show details of processing, defaults to False + :type verbose: bool, optional + :raises RuntimeError: file syntax error + + :references: + + - Robotics, Vision & Control for Python, §6.5, P. Corke, Springer 2023. + - A Tutorial on Graph-Based SLAM, + G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, + in IEEE Intelligent Transportation Systems Magazine, + vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925. + """ + # parse the file data # we assume g2o format # VERTEX* vertex_id X Y THETA # EDGE* startvertex_id endvertex_id X Y THETA IXX IXY IYY IXT IYT ITT # vertex numbers start at 0 - + self.lidar = lidar - + self.graph = pgraph.UGraph(verbose=verbose) path = Path(rtb.rtb_path_to_datafile(filename)) - if path.suffix == '.zip': - zf = zipfile.ZipFile(path, 'r') + if path.suffix == ".zip": + zf = zipfile.ZipFile(path, "r") opener = zf.open path = path.stem else: opener = open - with opener(path, 'r') as f: + with opener(path, "r") as f: toroformat = False nlidar = 0 - + # indices into ROBOTLASER1 record for the 3x3 info matrix in column major # order # IXX IXY IXT IYY IYT ITT # 6 7 8 9 10 11 # 0 1 2 3 4 5 - g2o = [0, 1, 2, 1, 3, 4, 2, 4, 5] + g2o = [0, 1, 2, 1, 3, 4, 2, 4, 5] # IXX IXY IYY ITT IXT IYT # 6 7 8 9 10 11 # 0 1 2 3 4 5 - toro = [0, 1, 4, 1, 2, 5, 4, 5, 3] - + toro = [0, 1, 4, 1, 2, 5, 4, 5, 3] + # we keep an array self. = vindex(gi) to map g2o vertex index to PGraph vertex index - + self.vindex = {} firstlidar = True + PGVertex.nvertices = 0 # reset vertex counter in PGVertex class for line in f: # for zip file, we get data as bytes not str if isinstance(line, bytes): line = line.decode() - + # is it a comment? - if line.startswith('#'): + if line.startswith("#"): continue - + tokens = line.split() - + # g2o format records - if tokens[0] == 'VERTEX_SE2': + if tokens[0] == "VERTEX_SE2": coord = np.array([float(x) for x in tokens[2:5]]) id = tokens[1] - v = PGVertex('vertex', coord=coord, name=id) + v = PGVertex("vertex", coord=coord, name=id) self.graph.add_vertex(v) - elif tokens[0] == 'VERTEX_XY': + elif tokens[0] == "VERTEX_XY": coord = np.array([float(x) for x in tokens[2:4]]) id = tokens[1] - v = PGVertex('landmark', coord=coord, name=id) + v = PGVertex("landmark", coord=coord, name=id) self.graph.add_vertex(v) - - elif tokens[0] == 'EDGE_SE2': + + elif tokens[0] == "EDGE_SE2": v1 = tokens[1] v2 = tokens[2] @@ -161,11 +195,11 @@ def __init__(self, filename, lidar=False, verbose=False): # X Y T # 3 4 5 mean = np.array([float(x) for x in tokens[3:6]]) - + # IXX IXY IXT IYY IYT ITT # 6 7 8 9 10 11 d = np.array([float(x) for x in tokens[6:12]]) - info = d[g2o].reshape((3,3)) + info = d[g2o].reshape((3, 3)) # create the edge v1 = self.graph[v1] @@ -175,18 +209,18 @@ def __init__(self, filename, lidar=False, verbose=False): ## TORO format records # https://openslam-org.github.io/toro.html - elif tokens[0] == 'VERTEX2': + elif tokens[0] == "VERTEX2": toroformat = True coord = np.array([float(x) for x in tokens[2:5]]) id = tokens[1] - v = PGVertex('vertex', coord=coord, name=id) + v = PGVertex("vertex", coord=coord, name=id) self.graph.add_vertex(v) - - elif tokens[0] == 'EDGE2': + + elif tokens[0] == "EDGE2": toroformat = True - + v1 = tokens[1] v2 = tokens[2] @@ -194,23 +228,23 @@ def __init__(self, filename, lidar=False, verbose=False): # X Y T # 3 4 5 mean = np.array([float(x) for x in tokens[3:6]]) - + # IXX IXY IYY ITT IXT IYT # 6 7 8 9 10 11 d = np.array([float(x) for x in tokens[6:12]]) - info = d[toro].reshape((3,3)) + info = d[toro].reshape((3, 3)) # create the edge v1 = self.graph[v1] v2 = self.graph[v2] e = PGEdge(v1, v2, mean, info) v1.connect(v2, edge=e) - - elif tokens[0] == 'ROBOTLASER1': + + elif tokens[0] == "ROBOTLASER1": if not lidar: continue - - # lidar records are associated with the immediately + + # lidar records are associated with the immediately # preceding VERTEX record # # not quite sure what all the fields are @@ -226,7 +260,6 @@ def __init__(self, filename, lidar=False, verbose=False): # 9+N+12 timestamp (*nix timestamp) # 9+N+13 lidar type (str) - if firstlidar: nbeams = int(tokens[8]) lidarmeta = tokens[2:6] @@ -234,10 +267,10 @@ def __init__(self, filename, lidar=False, verbose=False): # add attributes to the most recent vertex created v.theta = np.arange(0, nbeams) * float(tokens[4]) + float(tokens[2]) - v.range = np.array([float(x) for x in tokens[9:nbeams+9]]) - v.time = float(tokens[21+nbeams]) + v.range = np.array([float(x) for x in tokens[9 : nbeams + 9]]) + v.time = float(tokens[21 + nbeams]) self.vindex[v.index] = v - nlidar+= 1 + nlidar += 1 else: raise RuntimeError(f"Unexpected line {line} in {filename}") @@ -245,83 +278,90 @@ def __init__(self, filename, lidar=False, verbose=False): filetype = "TORO/LAGO" else: filetype = "g2o" - print(f"loaded {filetype} format file: {self.graph.n} vertices, {self.graph.ne} edges") - + print( + f"loaded {filetype} format file: {self.graph.n} vertices," + f" {self.graph.ne} edges" + ) + if nlidar > 0: lidarmeta = [float(x) for x in lidarmeta] - + self._angmin = lidarmeta[0] self._angmax = sum(lidarmeta[0:2]) self._maxrange = lidarmeta[3] fov = np.degrees([self._angmin, self._angmax]) - print(f" {nlidar} lidar scans: {nbeams} beams, fov {fov[0]:.1f}° to {fov[1]:.1f}°, max range {self._maxrange}") + print( + f" {nlidar} lidar scans: {nbeams} beams, fov {fov[0]:.1f}° to" + f" {fov[1]:.1f}°, max range {self._maxrange}" + ) - def scan(self, i): v = self.vindex[i] return v.range, v.theta - + def scanxy(self, i): - + range, theta = self.scan(i) range = np.where(range < self._maxrange, range, np.nan) x = range * np.cos(theta) y = range * np.sin(theta) - return np.c_[x, y].T - + def plot_scan(self, n): - n = base.getvector(n) + n = smb.getvector(n) for i in n: x, y = self.scanxy(i) - plt.plot(x, y, '.', 'MarkerSize', 10) + plt.plot(x, y, ".", "MarkerSize", 10) plt.pause(1) def pose(self, i): return self.vindex[i].coord - + def time(self, i): return self.vindex[i].time - + def plot(self, **kwargs): - if not 'vopt' in kwargs: - kwargs['vopt'] = dict(markersize=8, markerfacecolor='blue', markeredgecolor='None') - if not 'eopt' in kwargs: - kwargs['eopt'] = dict(linewidth=1) + if not "vopt" in kwargs: + kwargs["vopt"] = dict( + markersize=8, markerfacecolor="blue", markeredgecolor="None" + ) + if not "eopt" in kwargs: + kwargs["eopt"] = dict(linewidth=1) self.graph.plot(colorcomponents=False, force2d=True, **kwargs) - plt.xlabel('x') - plt.ylabel('y') + plt.xlabel("x") + plt.ylabel("y") plt.grid(True) def scanmatch(self, s1, s2): p1 = self.scanxy(s1) p2 = self.scanxy(s2) - T = base.ICP2d(p1, p2) + T = smb.ICP2d(p1, p2) return SE2(T) - + def scanmap(self, occgrid, maxrange=None): # note about maxrange timing - bar = FillingCirclesBar('Converting', max=self.graph.n, - suffix = '%(percent).1f%% - %(eta)ds') + bar = FillingCirclesBar( + "Converting", max=self.graph.n, suffix="%(percent).1f%% - %(eta)ds" + ) grid1d = occgrid.ravel for i in range(0, self.graph.n, 5): - + xy = self.scanxy(i) r, theta = self.scan(i) if maxrange is not None: toofar = np.where(r > maxrange)[0] xy = np.delete(xy, toofar, axis=1) xyt = self.vindex[i].coord - + xy = SE2(xyt) * xy - + # start of each ray p1 = occgrid.w2g(xyt[:2]) - + for p2 in occgrid.w2g(xy.T): # all cells along the ray @@ -330,7 +370,7 @@ def scanmap(self, occgrid, maxrange=None): except ValueError: # silently ignore rays to points outside the grid map continue - + # increment cells along the ray, these are free space grid1d[k[:-1]] += 1 # decrement target cell @@ -341,149 +381,118 @@ def scanmap(self, occgrid, maxrange=None): grid1d[grid1d < 0] = -1 grid1d[grid1d > 0] = 1 bar.finish() - + def w2g(self, w): return np.round((w - self._centre) / self._cellsize) + self._ngrid / 2 def g2w(self, g): - return ( np.r_[g] - self._ngrid / 2) * self._cellsize + self._centre + return (np.r_[g] - self._ngrid / 2) * self._cellsize + self._centre - def plot_occgrid(self, w, block=True): - - bl = self.g2w([0,0]) + def plot_occgrid(self, w, block=None): + + bl = self.g2w([0, 0]) tr = self.g2w([w.shape[1], w.shape[0]]) w = np.where(w < 0, -1, w) # free cells are -1 - w = np.where(w > 0, 1, w) # occupied cells are +1 - w = -w + w = np.where(w > 0, 1, w) # occupied cells are +1 + w = -w # plot it - plt.imshow(w, cmap='gray', extent=[bl[0], tr[0], bl[1], tr[1]]) - plt.xlabel('x') - plt.ylabel('y') - plt.show(block=block) - - -# This source code is part of the graph optimization package -# deveoped for the lectures of robotics2 at the University of Freiburg. -# -# Copyright (c) 2007 Giorgio Grisetti, Gian Diego Tipaldi -# -# It is licences under the Common Creative License, -# Attribution-NonCommercial-ShareAlike 3.0 -# -# You are free: -# - to Share - to copy, distribute and transmit the work -# - to Remix - to adapt the work -# -# Under the following conditions: -# -# - Attribution. You must attribute the work in the manner specified -# by the author or licensor (but not in any way that suggests that -# they endorse you or your use of the work). -# -# - Noncommercial. You may not use this work for commercial purposes. -# -# - Share Alike. If you alter, transform, or build upon this work, -# you may distribute the resulting work only under the same or -# similar license to this one. -# -# Any of the above conditions can be waived if you get permission -# from the copyright holder. Nothing in this license impairs or -# restricts the author's moral rights. -# -# This software is distributed in the hope that it will be useful, -# but WITHOUT ANY WARRANTY; without even the implied -# warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR -# PURPOSE. - - -# ls-slam.m -# this file is released under the creative common license - -# solves a graph-based slam problem via least squares -# vmeans: matrix containing the column vectors of the poses of the vertices -# the vertices are odrered such that vmeans[i] corresponds to the ith id -# eids: matrix containing the column vectors [idFrom, idTo]' of the ids of the vertices -# eids[k] corresponds to emeans[k] and einfs[k]. -# emeans: matrix containing the column vectors of the poses of the edges -# einfs: 3d matrix containing the information matrices of the edges -# einfs(:,:,k) refers to the information matrix of the k-th edge. -# n: number of iterations -# newmeans: matrix containing the column vectors of the updated vertices positions - - def optimize(self, iterations = 10, animate = False, retain = False, **kwargs): - - eprev = math.inf + plt.imshow(w, cmap="gray", extent=[bl[0], tr[0], bl[1], tr[1]]) + plt.xlabel("x") + plt.ylabel("y") + + if block is not None: + plt.show(block=block) + + # The following methods were inspired by, and modeled on, the MATLAB/Octave file + # ls-slam.m by Giorgio Grisetti and Gian Diego Tipaldi. + # + # The theory is covered in: + # G. Grisetti, R. Kümmerle, C. Stachniss and W. Burgard, + # "A Tutorial on Graph-Based SLAM,” in IEEE Intelligent Transportation Systems Magazine, + # vol. 2, no. 4, pp. 31-43, winter 2010, doi: 10.1109/MITS.2010.939925. + + def optimize(self, iterations=10, animate=False, retain=True, **kwargs): + + eprev = math.inf + eo = {} if animate and retain: colors = plt.cm.Greys(np.linspace(0.3, 1, iterations)) - if 'eopt' in kwargs: - eo = kwargs['eopt'] - kwargs = {k: v for (k, v) in kwargs.items() if k != 'eopt'} - else: - eo = {} + if "eopt" in kwargs: + eo = kwargs["eopt"] + kwargs = {k: v for (k, v) in kwargs.items() if k != "eopt"} + # else: + # eo = {} for i in range(iterations): if animate: + if retain: + if i == 0: + ax = smb.axes_logic(None, 2) + else: + ax = smb.axes_logic(None, 2, new=True) + if not retain: - plt.clf() eopt = eo else: eopt = {**eo, **dict(color=tuple(colors[i, :]), label=i)} - self.graph.plot(eopt=eopt, force2d=True, colorcomponents=False, **kwargs) + self.graph.plot( + eopt=eopt, force2d=True, colorcomponents=False, ax=ax, **kwargs + ) plt.pause(0.5) - + energy = self.linearize_and_solve() - + if energy >= eprev: break eprev = energy - # linearizes and solves one time the ls-slam problem specified by the input - # vmeans: vertices positions at the linearization point - # eids: edge ids - # emeans: edge means - # einfs: edge information matrices - # newmeans: new solution computed from the initial guess in vmeans def linearize_and_solve(self): - t0 = time.time() + t0 = time.time() g = self.graph - + # H and b are respectively the system matrix and the system vector H = np.zeros((g.n * 3, g.n * 3)) b = np.zeros((g.n * 3, 1)) # this loop constructs the global system by accumulating in H and b the contributions # of all edges (see lecture) - + etotal = 0 - for edge in g.edges(): + for edge in g.edges(): e, A, B = edge.linear_factors() - omega = edge.info + omega = edge.info # compute the blocks of H^k - bi = -A.T @ omega @ e - bj = -B.T @ omega @ e - Hii = A.T @ omega @ A - Hij = A.T @ omega @ B - Hjj = B.T @ omega @ B - + bi = -A.T @ omega @ e + bj = -B.T @ omega @ e + Hii = A.T @ omega @ A + Hij = A.T @ omega @ B + Hjj = B.T @ omega @ B + v = edge.endpoints i = v[0].index j = v[1].index - islice = slice(i*3, (i+1)*3) - jslice = slice(j*3, (j+1)*3) + islice = slice(i * 3, (i + 1) * 3) + jslice = slice(j * 3, (j + 1) * 3) # accumulate the blocks in H and b - H[islice, islice] += Hii - H[jslice, jslice] += Hjj - H[islice, jslice] += Hij - H[jslice, islice] += Hij.T - - b[islice,0] += bi - b[jslice,0] += bj + try: + H[islice, islice] += Hii + H[jslice, jslice] += Hjj + H[islice, jslice] += Hij + H[jslice, islice] += Hij.T + + b[islice, 0] += bi + b[jslice, 0] += bj + except ValueError: + print("linearize_and_solve failed") + print(v) + print(H.shape, b.shape, Hii.shape, Hjj.shape, Hij.shape) + print(islice, jslice) etotal += np.inner(e, e) @@ -496,60 +505,35 @@ def linearize_and_solve(self): # which is equivalent to the following H[0:3, 0:3] += np.eye(3) - + SH = sp.sparse.csr_matrix(H) deltax = sp.sparse.linalg.spsolve(SH, b) # SH \ b - + # split the increments in nice 3x1 vectors and sum them up to the original matrix for vertex in g: i = vertex.index * 3 - vertex.coord += deltax[i: i+3] + vertex.coord += deltax[i : i + 3] # normalize the angles between -PI and PI - vertex.coord[2] = base.angdiff(vertex.coord[2]) + vertex.coord[2] = smb.angdiff(vertex.coord[2]) - dt = time.time() - t0 + dt = time.time() - t0 print(f"done in {dt*1e3:0.2f} msec. Total cost {etotal:g}") return etotal - # This source code is part of the graph optimization package - # deveoped for the lectures of robotics2 at the University of Freiburg. - # - # Copyright (c) 2007 Giorgio Grisetti, Gian Diego Tipaldi - # - # It is licences under the Common Creative License, - # Attribution-NonCommercial-ShareAlike 3.0 - # - # You are free: - # - to Share - to copy, distribute and transmit the work - # - to Remix - to adapt the work - # - # Under the following conditions: - # - # - Attribution. You must attribute the work in the manner specified - # by the author or licensor (but not in any way that suggests that - # they endorse you or your use of the work). - # - # - Noncommercial. You may not use this work for commercial purposes. - # - # - Share Alike. If you alter, transform, or build upon this work, - # you may distribute the resulting work only under the same or - # similar license to this one. - # - # Any of the above conditions can be waived if you get permission - # from the copyright holder. Nothing in this license impairs or - # restricts the author's moral rights. - # - # This software is distributed in the hope that it will be useful, - # but WITHOUT ANY WARRANTY; without even the implied - # warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR - # PURPOSE. - if __name__ == "__main__": - scan = PoseGraph('killian.g2o', lidar=True, verbose=False) - print(scan.graph.nc) - # scan.plot() - w = scan.scanmap(maxrange=40) - scan.plot_occgrid(w) \ No newline at end of file + pg = PoseGraph("data/pg1.g2o") + pg.optimize(animate=True, retain=False) + + plt.show(block=True) + + # pg = PoseGraph("data/killian-small.toro"); + # pg.optimize() + + # scan = PoseGraph("killian.g2o", lidar=True, verbose=False) + # print(scan.graph.nc) + # # scan.plot() + # w = scan.scanmap(maxrange=40) + # scan.plot_occgrid(w) diff --git a/roboticstoolbox/mobile/QuinticPolyPlanner.py b/roboticstoolbox/mobile/QuinticPolyPlanner.py index 1900fa594..33b8e61c8 100644 --- a/roboticstoolbox/mobile/QuinticPolyPlanner.py +++ b/roboticstoolbox/mobile/QuinticPolyPlanner.py @@ -15,12 +15,12 @@ # The following code is modified from Python Robotics # https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning # Quintic Polynomial Planner -# Author: Atsushi Sakai +# Author: Atsushi Sakai # Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE -class _QuinticPolynomial: +class _QuinticPolynomial: def __init__(self, xs, vxs, axs, xe, vxe, axe, time): # calc coefficient of quintic polynomial # See jupyter notebook document for derivation of this equation. @@ -28,12 +28,20 @@ def __init__(self, xs, vxs, axs, xe, vxe, axe, time): self.a1 = vxs self.a2 = axs / 2.0 - A = np.array([[time ** 3, time ** 4, time ** 5], - [3 * time ** 2, 4 * time ** 3, 5 * time ** 4], - [6 * time, 12 * time ** 2, 20 * time ** 3]]) - b = np.array([xe - self.a0 - self.a1 * time - self.a2 * time ** 2, - vxe - self.a1 - 2 * self.a2 * time, - axe - 2 * self.a2]) + A = np.array( + [ + [time**3, time**4, time**5], + [3 * time**2, 4 * time**3, 5 * time**4], + [6 * time, 12 * time**2, 20 * time**3], + ] + ) + b = np.array( + [ + xe - self.a0 - self.a1 * time - self.a2 * time**2, + vxe - self.a1 - 2 * self.a2 * time, + axe - 2 * self.a2, + ] + ) x = np.linalg.solve(A, b) self.a3 = x[0] @@ -41,29 +49,47 @@ def __init__(self, xs, vxs, axs, xe, vxe, axe, time): self.a5 = x[2] def calc_point(self, t): - xt = self.a0 + self.a1 * t + self.a2 * t ** 2 + \ - self.a3 * t ** 3 + self.a4 * t ** 4 + self.a5 * t ** 5 + xt = ( + self.a0 + + self.a1 * t + + self.a2 * t**2 + + self.a3 * t**3 + + self.a4 * t**4 + + self.a5 * t**5 + ) return xt def calc_first_derivative(self, t): - xt = self.a1 + 2 * self.a2 * t + \ - 3 * self.a3 * t ** 2 + 4 * self.a4 * t ** 3 + 5 * self.a5 * t ** 4 + xt = ( + self.a1 + + 2 * self.a2 * t + + 3 * self.a3 * t**2 + + 4 * self.a4 * t**3 + + 5 * self.a5 * t**4 + ) return xt def calc_second_derivative(self, t): - xt = 2 * self.a2 + 6 * self.a3 * t + 12 * self.a4 * t ** 2 + 20 * self.a5 * t ** 3 + xt = ( + 2 * self.a2 + + 6 * self.a3 * t + + 12 * self.a4 * t**2 + + 20 * self.a5 * t**3 + ) return xt def calc_third_derivative(self, t): - xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t ** 2 + xt = 6 * self.a3 + 24 * self.a4 * t + 60 * self.a5 * t**2 return xt -def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt, MIN_T, MAX_T): +def quintic_polynomials_planner( + sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_accel, max_jerk, dt, MIN_T, MAX_T +): """ quintic polynomial planner @@ -134,16 +160,20 @@ def quintic_polynomials_planner(sx, sy, syaw, sv, sa, gx, gy, gyaw, gv, ga, max_ j *= -1 rj.append(j) - if max([abs(i) for i in ra]) <= max_accel and max([abs(i) for i in rj]) <= max_jerk: + if ( + max([abs(i) for i in ra]) <= max_accel + and max([abs(i) for i in rj]) <= max_jerk + ): print("find path!!") break return time, np.c_[rx, ry, ryaw], rv, ra, rj + # ====================== RTB wrapper ============================= # # Copyright (c) 2022 Peter Corke: https://github.com/petercorke/robotics-toolbox-python -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE class QuinticPolyPlanner(PlannerBase): r""" Quintic polynomial path planner @@ -158,7 +188,7 @@ class QuinticPolyPlanner(PlannerBase): :type goal_vel: float, optional :param goal_acc: goal acceleration, defaults to 0 :type goal_acc: float, optional - :param max_acc: [description], defaults to 1 + :param max_acc: maximum acceleration, defaults to 1 :type max_acc: int, optional :param max_jerk: maximum jerk, defaults to 0.5 :type min_t: float, optional @@ -187,6 +217,8 @@ class QuinticPolyPlanner(PlannerBase): x(t) &= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \\ y(t) &= b_0 + b_1 t + b_2 t^2 + b_3 t^3 + b_4 t^4 + b_5 t^5 + that meets the given constraints. Trajectory time is given as a range. + :reference: "Local Path Planning And Motion Control For AGV In Positioning", Takahashi, T. Hongo, Y. Ninomiya and G. Sugimoto; Proceedings. IEEE/RSJ International Workshop on @@ -212,8 +244,19 @@ class QuinticPolyPlanner(PlannerBase): :seealso: :class:`Planner` """ - def __init__(self, dt=0.1, start_vel=0, start_acc=0, goal_vel=0, goal_acc=0, - max_acc=1, max_jerk=0.5, min_t=5, max_t=100): + + def __init__( + self, + dt=0.1, + start_vel=0, + start_acc=0, + goal_vel=0, + goal_acc=0, + max_acc=1, + max_jerk=0.5, + min_t=5, + max_t=100, + ): super().__init__(ndims=3) self.dt = dt @@ -226,7 +269,6 @@ def __init__(self, dt=0.1, start_vel=0, start_acc=0, goal_vel=0, goal_acc=0, self.min_t = min_t self.max_t = max_t - def query(self, start, goal): r""" Find a quintic polynomial path @@ -248,23 +290,38 @@ def query(self, start, goal): ``accel`` acceleration profile along the path ``jerk`` jerk profile along the path ========== =================================================== - + :seealso: :meth:`Planner.query` """ self._start = start self._goal = goal time, path, v, a, j = quintic_polynomials_planner( - start[0], start[1], start[2], self.start_vel, self.start_acc, - goal[0], goal[1], goal[2], self.start_vel, self.start_acc, - self.max_acc, self.max_jerk, dt=self.dt, MIN_T=self.min_t, MAX_T=self.max_t) + start[0], + start[1], + start[2], + self.start_vel, + self.start_acc, + goal[0], + goal[1], + goal[2], + self.start_vel, + self.start_acc, + self.max_acc, + self.max_jerk, + dt=self.dt, + MIN_T=self.min_t, + MAX_T=self.max_t, + ) + + status = namedtuple("QuinticPolyStatus", ["t", "vel", "acc", "jerk"])( + time, v, a, j + ) - status = namedtuple('QuinticPolyStatus', ['t', 'vel', 'acc', 'jerk'])( - time, v, a, j) - return path, status -if __name__ == '__main__': + +if __name__ == "__main__": from math import pi start = (10, 10, np.deg2rad(10.0)) @@ -294,5 +351,3 @@ def query(self, start, goal): plt.grid(True) plt.show(block=True) - - diff --git a/roboticstoolbox/mobile/RRTPlanner.py b/roboticstoolbox/mobile/RRTPlanner.py index 713e28d4f..ec76179a4 100644 --- a/roboticstoolbox/mobile/RRTPlanner.py +++ b/roboticstoolbox/mobile/RRTPlanner.py @@ -3,9 +3,9 @@ # The following code is based on code from Python Robotics # https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning # RRTDubins planning -# Author: Atsushi Sakai +# Author: Atsushi Sakai # Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE import math from collections import namedtuple @@ -25,7 +25,7 @@ class RRTPlanner(PlannerBase): - """ + r""" Rapidly exploring tree planner :param map: occupancy grid @@ -36,8 +36,6 @@ class RRTPlanner(PlannerBase): :type curvature: float, optional :param stepsize: spacing between points on the path, defaults to 0.2 :type stepsize: float, optional - :param showsamples: shows vehicle polygons for all random samples, defaults to False - :type showsamples: bool, optional :param npoints: number of vertices in random tree, defaults to 50 :type npoints: int, optional @@ -57,7 +55,7 @@ class RRTPlanner(PlannerBase): the forward or backward direction. Polygons are used for obstacle avoidance: - + - the environment is defined by a set of polygons represented by a :class:`PolygonMap` - the vehicle is defined by a single polygon specified by the ``polygon`` argument to its constructor @@ -70,7 +68,7 @@ class RRTPlanner(PlannerBase): # create polygonal obstacles map = PolygonMap(workspace=[0, 10]) - map.add([(5, 50), (5, 6), (6, 6), (6, 50)]) + map.add([(5, 50), (5, 6), (6, 6), (6, 50)]) map.add([(5, 4), (5, -50), (6, -50), (6, 4)]) # create outline polygon for vehicle @@ -95,22 +93,20 @@ class RRTPlanner(PlannerBase): :seealso: :class:`DubinsPlanner` :class:`Vehicle` :class:`PlannerBase` """ + def __init__( self, map, vehicle, curvature=1.0, stepsize=0.2, - showsamples=False, npoints=50, - **kwargs + **kwargs, ): - super().__init__(ndims=2, **kwargs) self.npoints = npoints self.map = map - self.showsamples = showsamples self.g = DGraph(metric="SE2") @@ -127,12 +123,18 @@ def __init__( # self.goal_yaw_th = np.deg2rad(1.0) # self.goal_xy_th = 0.5 - def plan(self, goal, animate=True, search_until_npoints=True): + def plan(self, goal, showsamples=True, showvalid=True, animate=False): r""" Plan paths to goal using RRT :param goal: goal pose :math:`(x, y, \theta)`, defaults to previously set value :type goal: array_like(3), optional + :param showsamples: display position part of configurations overlaid on the map, defaults to True + :type showsamples: bool, optional + :param showvalid: display valid configurations as vehicle polygons overlaid on the map, defaults to False + :type showvalid: bool, optional + :param animate: update the display as configurations are tested, defaults to False + :type animate: bool, optional Compute a rapidly exploring random tree with its root at the ``goal``. The tree will have ``npoints`` vertices spread uniformly randomly over @@ -142,6 +144,11 @@ def plan(self, goal, animate=True, search_until_npoints=True): vertex already in the graph. Each configuration on that path, with spacing of ``stepsize``, is tested for obstacle intersection. + The configurations tested are displayed (translation only) if ``showsamples`` is + True. The valid configurations are displayed as vehicle polygones if ``showvalid`` + is True. If ``animate`` is True these points are displayed during the search + process, otherwise a single figure is displayed at the end. + :seealso: :meth:`query` """ # TODO use validate @@ -152,14 +159,18 @@ def plan(self, goal, animate=True, search_until_npoints=True): v = self.g.add_vertex(coord=goal) v.path = None + if showsamples or showvalid: + self.map.plot() + self.progress_start(self.npoints) count = 0 while count < self.npoints: - random_point = self.qrandom_free() - if self.showsamples: + if showsamples: plt.plot(random_point[0], random_point[1], "ok", markersize=2) + if animate: + plt.pause(0.02) vnearest, d = self.g.closest(random_point) @@ -191,8 +202,13 @@ def plan(self, goal, animate=True, search_until_npoints=True): self.g.add_edge(vnew, vnearest, cost=pstatus.length) vnew.path = path - self.vehicle.polygon(random_point).plot(color="b", alpha=0.1) - plt.show() + if showvalid: + self.vehicle.polygon(random_point).plot(color="b", alpha=0.1) + if animate: + plt.pause(0.02) + + if (showvalid or showsamples) and not animate: + plt.show(block=False) self.progress_end() @@ -306,7 +322,6 @@ def iscollision(self, q): if __name__ == "__main__": - from roboticstoolbox.mobile.Vehicle import Bicycle # start and goal configuration diff --git a/roboticstoolbox/mobile/ReedsSheppPlanner.py b/roboticstoolbox/mobile/ReedsSheppPlanner.py index 1d7724dfc..51c8aae91 100644 --- a/roboticstoolbox/mobile/ReedsSheppPlanner.py +++ b/roboticstoolbox/mobile/ReedsSheppPlanner.py @@ -11,12 +11,12 @@ # The following code is modified from Python Robotics # https://github.com/AtsushiSakai/PythonRobotics/tree/master/PathPlanning # D* grid planning -# Author: Atsushi Sakai +# Author: Atsushi Sakai # Copyright (c) 2016 - 2022 Atsushi Sakai and other contributors: https://github.com/AtsushiSakai/PythonRobotics/contributors -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE -class _Path: +class _Path: def __init__(self): self.lengths = [] self.ctypes = [] @@ -26,6 +26,13 @@ def __init__(self): self.yaw = [] self.directions = [] + def __repr__(self): + s = f"_Path: L={self.L:.2g}, " + s += f"[{', '.join(['{}={:.2g}'.format(c,l) for c, l in zip(self.ctypes, self.lengths)])}]" + if len(self.x) > 0: + s += f", {len(self.x)} points on path" + return s + def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): """ @@ -36,24 +43,32 @@ def plot_arrow(x, y, yaw, length=1.0, width=0.5, fc="r", ec="k"): for (ix, iy, iyaw) in zip(x, y, yaw): plot_arrow(ix, iy, iyaw) else: - plt.arrow(x, y, length * math.cos(yaw), length * math.sin(yaw), - fc=fc, ec=ec, head_width=width, head_length=width) + plt.arrow( + x, + y, + length * math.cos(yaw), + length * math.sin(yaw), + fc=fc, + ec=ec, + head_width=width, + head_length=width, + ) plt.plot(x, y) def straight_left_straight(x, y, phi): phi = base.wrap_0_2pi(phi) if y > 0.0 and 0.0 < phi < math.pi * 0.99: - xd = - y / math.tan(phi) + x + xd = -y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi - v = math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) + v = math.sqrt((x - xd) ** 2 + y**2) - math.tan(phi / 2.0) return True, t, u, v elif y < 0.0 < phi < math.pi * 0.99: - xd = - y / math.tan(phi) + x + xd = -y / math.tan(phi) + x t = xd - math.tan(phi / 2.0) u = phi - v = -math.sqrt((x - xd) ** 2 + y ** 2) - math.tan(phi / 2.0) + v = -math.sqrt((x - xd) ** 2 + y**2) - math.tan(phi / 2.0) return True, t, u, v return False, 0.0, 0.0, 0.0 @@ -66,7 +81,7 @@ def set_path(paths, lengths, ctypes): # check same path exist for tpath in paths: - typeissame = (tpath.ctypes == path.ctypes) + typeissame = tpath.ctypes == path.ctypes if typeissame: if sum(np.abs(tpath.lengths)) - sum(np.abs(path.lengths)) <= 0.01: return paths # not insert path @@ -93,7 +108,7 @@ def straight_curve_straight(x, y, phi, paths): def polar(x, y): - r = math.sqrt(x ** 2 + y ** 2) + r = math.sqrt(x**2 + y**2) theta = math.atan2(y, x) return r, theta @@ -200,7 +215,7 @@ def curve_straight_curve(x, y, phi, paths): def left_straight_right(x, y, phi): u1, t1 = polar(x + math.sin(phi), y - 1.0 - math.cos(phi)) - u1 = u1 ** 2 + u1 = u1**2 if u1 >= 4.0: u = math.sqrt(u1 - 4.0) theta = math.atan2(2.0, u) @@ -214,6 +229,18 @@ def left_straight_right(x, y, phi): def generate_path(q0, q1, max_curvature): + """ + Return a set of feasible paths + + :param q0: initial configuration + :type q0: array_like(3) + :param q1: final configuration + :type q1: array_like(3) + :param max_curvature: maximum path curvature + :type max_curvature: float + :return: set of paths + :rtype: list of _Path + """ dx = q1[0] - q0[0] dy = q1[1] - q0[1] dth = q1[2] - q0[2] @@ -222,6 +249,7 @@ def generate_path(q0, q1, max_curvature): x = (c * dx + s * dy) * max_curvature y = (-s * dx + c * dy) * max_curvature + # build a list of possible paths paths = [] paths = straight_curve_straight(x, y, dth, paths) paths = curve_straight_curve(x, y, dth, paths) @@ -230,7 +258,19 @@ def generate_path(q0, q1, max_curvature): return paths -def interpolate(ind, length, mode, max_curvature, origin_x, origin_y, origin_yaw, path_x, path_y, path_yaw, directions): +def interpolate( + ind, + length, + mode, + max_curvature, + origin_x, + origin_y, + origin_yaw, + path_x, + path_y, + path_yaw, + directions, +): if mode == "S": path_x[ind] = origin_x + length / max_curvature * math.cos(origin_yaw) path_y[ind] = origin_y + length / max_curvature * math.sin(origin_yaw) @@ -287,24 +327,26 @@ def generate_local_course(total_length, lengths, mode, max_curvature, step_size) ind -= 1 if i >= 1 and (lengths[i - 1] * lengths[i]) > 0: - pd = - d - ll + pd = -d - ll else: pd = d - ll while abs(pd) <= abs(l): ind += 1 px, py, pyaw, directions = interpolate( - ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) + ind, pd, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions + ) pd += d ll = l - pd - d # calc remain length ind += 1 px, py, pyaw, directions = interpolate( - ind, l, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions) + ind, l, m, max_curvature, ox, oy, oyaw, px, py, pyaw, directions + ) # remove unused data - while px[-1] == 0.0: + while abs(px[-1]) < 1e-10: px.pop() py.pop() pyaw.pop() @@ -321,16 +363,23 @@ def calc_paths(sx, sy, syaw, gx, gy, gyaw, maxc, step_size): q0 = [sx, sy, syaw] q1 = [gx, gy, gyaw] + # find set of feasible paths paths = generate_path(q0, q1, maxc) for path in paths: + # interpolate the path x, y, yaw, directions = generate_local_course( - path.L, path.lengths, path.ctypes, maxc, step_size * maxc) + path.L, path.lengths, path.ctypes, maxc, step_size * maxc + ) # convert global coordinate - path.x = [math.cos(-q0[2]) * ix + math.sin(-q0[2]) - * iy + q0[0] for (ix, iy) in zip(x, y)] - path.y = [-math.sin(-q0[2]) * ix + math.cos(-q0[2]) - * iy + q0[1] for (ix, iy) in zip(x, y)] + path.x = [ + math.cos(-q0[2]) * ix + math.sin(-q0[2]) * iy + q0[0] + for (ix, iy) in zip(x, y) + ] + path.y = [ + -math.sin(-q0[2]) * ix + math.cos(-q0[2]) * iy + q0[1] + for (ix, iy) in zip(x, y) + ] path.yaw = [pi_2_pi(iyaw + q0[2]) for iyaw in yaw] path.directions = directions path.lengths = [length / maxc for length in path.lengths] @@ -360,10 +409,11 @@ def reeds_shepp_path_planning(start, goal, maxc, step_size): return bpath + # ====================== RTB wrapper ============================= # # Copyright (c) 2022 Peter Corke: https://github.com/petercorke/robotics-toolbox-python -# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE +# Released under the MIT license: https://github.com/AtsushiSakai/PythonRobotics/blob/master/LICENSE class ReedsSheppPlanner(PlannerBase): r""" Reeds-Shepp path planner @@ -401,19 +451,23 @@ class ReedsSheppPlanner(PlannerBase): >>> print(status) :reference: Optimal paths for a car that goes both forwards and backwards, - Reeds, J.A. and L.A. Shepp, Pacific J. Math., 145 (1990), - pp. 367–393. + Reeds, J.A. and L.A. Shepp, Pacific J. Math., 145 (1990), + pp. 367–393. :thanks: based on Reeds-Shepp path planning from `Python Robotics `_ :seealso: :class:`DubinsPlanner` :class:`PlannerBase` """ + def __init__(self, curvature=1, stepsize=0.1, **kwargs): super().__init__(ndims=3, **kwargs) self._curvature = curvature self._stepsize = stepsize def __str__(self): - s = super().__str__() + f"\n curvature={self.curvature}, stepsize={self.stepsize}" + s = ( + super().__str__() + + f"\n curvature={self.curvature}, stepsize={self.stepsize}" + ) def query(self, start, goal, **kwargs): r""" @@ -444,33 +498,40 @@ def query(self, start, goal, **kwargs): +-------------+-----------------------------------------------------+ |``direction``| the direction of motion at each point on the path | +-------------+-----------------------------------------------------+ - - .. note:: The direction of turning is reversed when travelling + + .. note:: The direction of turning is reversed when travelling backwards. """ super().query(start=start, goal=goal, next=False, **kwargs) bpath = reeds_shepp_path_planning( - start=self.start, goal=self.goal, - maxc=self._curvature, step_size=self._stepsize) + start=self.start, + goal=self.goal, + maxc=self._curvature, + step_size=self._stepsize, + ) path = np.c_[bpath.x, bpath.y, bpath.yaw] - status = namedtuple('ReedsSheppStatus', - ['segments', 'length', 'seglengths', 'direction']) + status = namedtuple( + "ReedsSheppStatus", ["segments", "length", "seglengths", "direction"] + ) - return path, status(bpath.ctypes, sum([abs(l) for l in bpath.lengths]), - bpath.lengths, bpath.directions) + return path, status( + bpath.ctypes, + sum([abs(l) for l in bpath.lengths]), + bpath.lengths, + bpath.directions, + ) -if __name__ == '__main__': +if __name__ == "__main__": from math import pi # start = (-1, -4, -np.radians(20)) # goal = (5, 5, np.radians(25)) - start = (0, 0, 0) goal = (0, 0, pi) @@ -495,4 +556,4 @@ def query(self, start, goal, **kwargs): # plt.show(block=True) reedsshepp.plot(path=path, direction=status.direction, configspace=True) - plt.show(block=True) \ No newline at end of file + plt.show(block=True) diff --git a/roboticstoolbox/mobile/Vehicle.py b/roboticstoolbox/mobile/Vehicle.py index 18620cd2d..95c926a79 100644 --- a/roboticstoolbox/mobile/Vehicle.py +++ b/roboticstoolbox/mobile/Vehicle.py @@ -10,6 +10,8 @@ from scipy import interpolate import matplotlib.pyplot as plt +from matplotlib import animation + # from matplotlib import patches # import matplotlib.transforms as mtransforms @@ -19,10 +21,21 @@ class VehicleBase(ABC): - - def __init__(self, covar=None, speed_max=np.inf, accel_max=np.inf, x0=[0, 0, 0], dt=0.1, - control=None, seed=0, animation=None, verbose=False, plot=False, workspace=None, - polygon=None): + def __init__( + self, + covar=None, + speed_max=np.inf, + accel_max=np.inf, + x0=[0, 0, 0], + dt=0.1, + control=None, + seed=0, + animation=None, + verbose=False, + plot=False, + workspace=None, + polygon=None, + ): r""" Superclass for vehicle kinematic models @@ -64,6 +77,9 @@ def __init__(self, covar=None, speed_max=np.inf, accel_max=np.inf, x0=[0, 0, 0], [A, B, C, D] A:B C:D ============== ======= ======= + :note: Set ``seed=None`` to have it randomly initialized from the + operating system. + :seealso: :class:`Bicycle` :class:`Unicycle` :class:`VehicleAnimationBase` """ @@ -73,8 +89,8 @@ def __init__(self, covar=None, speed_max=np.inf, accel_max=np.inf, x0=[0, 0, 0], x0 = np.zeros((3,), dtype=float) else: x0 = base.getvector(x0) - if len(x0) not in (2,3): - raise ValueError('x0 must be length 2 or 3') + if len(x0) not in (2, 3): + raise ValueError("x0 must be length 2 or 3") self._x0 = x0 self._x = x0.copy() @@ -82,7 +98,7 @@ def __init__(self, covar=None, speed_max=np.inf, accel_max=np.inf, x0=[0, 0, 0], self._seed = seed self._speed_max = speed_max self._accel_max = accel_max - self._v_prev = 0 + self._v_prev = [0] self._polygon = polygon if isinstance(animation, str): @@ -138,7 +154,7 @@ def f(self, x, odo, v=None): :return: predicted vehicle state :rtype: ndarray(3) - Predict the next state based on current state and odometry + Predict the next state based on current state and odometry value. ``v`` is a random variable that represents additive odometry noise for simulation purposes. @@ -161,7 +177,12 @@ def f(self, x, odo, v=None): # used by particle filter dd, dth = odo theta = x[:, 2] - return np.array(x) + np.c_[dd * np.cos(theta), dd * np.sin(theta), np.full(theta.shape, dth)] + return ( + np.array(x) + + np.c_[ + dd * np.cos(theta), dd * np.sin(theta), np.full(theta.shape, dth) + ] + ) else: # x is a vector x = base.getvector(x, 3) @@ -185,7 +206,7 @@ def Fx(self, x, odo): :type odo: array_like(2) :return: Jacobian matrix :rtype: ndarray(3,3) - + Returns the Jacobian matrix :math:`\frac{\partial \vec{f}}{\partial \vec{x}}` for the given state and odometry. @@ -213,7 +234,7 @@ def Fv(self, x, odo): :type odo: array_like(2) :return: Jacobian matrix :rtype: ndarray(3,2) - + Returns the Jacobian matrix :math:`\frac{\partial \vec{f}}{\partial \vec{v}}` for the given state and odometry. @@ -230,7 +251,7 @@ def Fv(self, x, odo): ]) # fmt: on return J - + @property def control(self): """ @@ -257,7 +278,7 @@ def control(self): >>> bike.control = RandomPath(10) >>> print(bike) - :seealso: :meth:`run` :meth:`eval_control` :obj:`scipy.interpolate.interp1d` :class:`~roboticstoolbox.mobile.drivers.VehicleDriverBase` + :seealso: :meth:`run` :meth:`eval_control` :obj:`scipy.interpolate.interp1d` :class:`~roboticstoolbox.mobile.drivers.VehicleDriverBase` """ return self._control @@ -273,7 +294,6 @@ def control(self, control): # if this is a driver agent, connect it to the vehicle control.vehicle = self - def eval_control(self, control, x): """ Evaluate vehicle control input (superclass method) @@ -304,7 +324,7 @@ def eval_control(self, control, x): if base.isvector(control, 2): # control is a constant u = base.getvector(control, 2) - + elif isinstance(control, VehicleDriverBase): # vehicle has a driver object u = control.demand() @@ -318,42 +338,44 @@ def eval_control(self, control, x): u = control(self, self._t, x) else: - raise ValueError('bad control specified') + raise ValueError("bad control specified") # apply limits ulim = self.u_limited(u) return ulim - def limits_va(self, v): + def limits_va(self, v, v_prev): """ Apply velocity and acceleration limits (superclass) :param v: desired velocity :type v: float + :param v_prev: previous velocity, reference to list + :type v_prev: list with single element :return: allowed velocity :rtype: float Determine allowable velocity given desired velocity, speed and acceleration limits. - .. note:: This function is stateful, requires previous velocity, - ``_v_prev`` attribute, to enable acceleration limiting. This - is reset to zero at the start of each simulation. + .. note:: This function requires previous velocity, ``v_prev`` to enable + acceleration limiting. This is passed as a reference to a mutable value, + a single-element list. This is reset to zero at the start of each simulation. """ # acceleration limit + vp = v_prev[0] if self._accel_max is not None: - if (v - self._v_prev) / self._dt > self._accel_max: - v = self._v_prev + self._accelmax * self._dt; - elif (v - self._v_prev) / self._dt < -self._accel_max: - v = self._v_prev - self._accel_max * self._dt; - self._v_prev = v - + if (v - vp) / self._dt > self._accel_max: + v = vp + self._accelmax * self._dt + elif (v - vp) / self._dt < -self._accel_max: + v = vp - self._accel_max * self._dt + v_prev[0] = v + # speed limit if self._speed_max is not None: v = np.clip(v, -self._speed_max, self._speed_max) return v - def polygon(self, q): """ Bounding polygon at vehicle configuration @@ -388,30 +410,29 @@ def add_driver(self, driver): :seealso: :class:`VehicleDriverBase` :meth:`control` """ - warnings.warn('add_driver is deprecated, use veh.control=driver instead') + warnings.warn("add_driver is deprecated, use veh.control=driver instead") self._control = driver driver._veh = self - - def run(self, T=10, x0=None, control=None, animate=False): + def run(self, T=10, x0=None, control=None, animate=True): r""" Simulate motion of vehicle (superclass) - :param N: Number of simulation steps, defaults to 1000 - :type N: int, optional + :param T: Simulation time in seconds, defaults to 10 + :type T: float, optional :param x0: Initial state, defaults to value given to Vehicle constructor :type x0: array_like(3) or array_like(2) - :param animation: vehicle animation object, defaults to None - :type animation: VehicleAnimation subclass, optional - :param plot: Enable plotting, defaults to False - :type plot: bool, optional + :param control: vehicle control inputs, defaults to None + :type control: array_like(2), callable, driving agent + :param animate: Enable graphical animation of vehicle, defaults to False + :type animate: bool, optional :return: State trajectory, each row is :math:`(x,y,\theta)`. :rtype: ndarray(n,3) - Runs the vehicle simulation for ``N`` timesteps and optionally plots + Runs the vehicle simulation for ``T`` seconds and optionally plots an animation. The method :meth:`step` performs each time step. - The control inputs are providd by ``control`` which can be: + The control inputs are provided by ``control`` which can be: * a constant tuple as the control inputs to the vehicle * a function called as ``f(vehicle, t, x)`` that returns a tuple @@ -423,24 +444,129 @@ def run(self, T=10, x0=None, control=None, animate=False): acceleration and steering limits. The simulation can be stopped prematurely by the control function - calling :meth:`stopif`. - - :seealso: :meth:`init` :meth:`step` :meth:`control` + calling :meth:`stopsim`. + + .. note:: + * the simulation is fixed-time step with the step given by the ``dt`` + attribute set by the constructor. + * integration uses rectangular integration. + + :seealso: :meth:`init` :meth:`step` :meth:`control` :meth:`run_animation` """ - self.init(control=control, x0=x0) - + self.init(control=control, animate=animate, x0=x0) + for i in range(round(T / self.dt)): self.step(animate=animate) # check for user requested stop if self._stopsim: - print('USER REEQUESTED STOP AT time', self._t) + print("USER REEQUESTED STOP AT time", self._t) break return self.x_hist - def init(self, x0=None, control=None): + def run_animation(self, T=10, x0=None, control=None, format=None, file=None): + r""" + Simulate motion of vehicle (superclass) + + :param T: Simulation time in seconds, defaults to 10 + :type T: float, optional + :param x0: Initial state, defaults to value given to Vehicle constructor + :type x0: array_like(3) or array_like(2) + :param control: vehicle control inputs, defaults to None + :type control: array_like(2), callable, driving agent + :param format: Output format + :type format: str, optional + :param file: File name + :type file: str, optional + :return: Matplotlib animation object + :rtype: :meth:`matplotlib.animation.FuncAnimation` + + Runs the vehicle simulation for ``T`` seconds and returns an animation + in various formats:: + + ``format`` ``file`` description + ============ ========= ============================ + ``"html"`` str, None return HTML5 video + ``"jshtml"`` str, None return JS+HTML video + ``"gif"`` str return animated GIF + ``"mp4"`` str return MP4/H264 video + ``None`` return a ``FuncAnimation`` object + + The allowables types for ``file`` are given in the second column. A str + value is the file name. If ``None`` is an option then return the video as a string. + + For the last case, a reference to the animation object must be held if used for + animation in a Jupyter cell:: + + anim = robot.run_animation(T=20) + + The control inputs are provided by ``control`` which can be: + + * a constant tuple as the control inputs to the vehicle + * a function called as ``f(vehicle, t, x)`` that returns a tuple + * an interpolator called as ``f(t)`` that returns a tuple, see + SciPy interp1d + * a driver agent, subclass of :class:`VehicleDriverBase` + + This is evaluated by :meth:`eval_control` which applies velocity, + acceleration and steering limits. + + The simulation can be stopped prematurely by the control function + calling :meth:`stopsim`. + + .. note:: + * the simulation is fixed-time step with the step given by the ``dt`` + attribute set by the constructor. + * integration uses rectangular integration. + + :seealso: :meth:`init` :meth:`step` :meth:`control` :meth:`run_animation` + """ + + fig, ax = plt.subplots() + + nframes = round(T / self.dt) + anim = animation.FuncAnimation( + fig=fig, + func=lambda i: self.step(animate=True, pause=False), + init_func=lambda: self.init(animate=True), + frames=nframes, + interval=self.dt * 1000, + blit=False, + repeat=False, + ) + # anim._interval = self.dt*1000/2 + # anim._repeat = True + ret = None + if format == "html": + ret = anim.to_html5_video() # convert to embeddable HTML5 animation + elif format == "jshtml": + ret = anim.to_jshtml() # convert to embeddable Javascript/HTML animation + elif format == "gif": + anim.save( + file, writer=animation.PillowWriter(fps=1 / self.dt) + ) # convert to GIF + ret = None + elif format == "mp4": + anim.save( + file, writer=animation.FFMpegWriter(fps=1 / self.dt) + ) # convert to mp4/H264 + ret = None + elif format == None: + # return the anim object + return anim + else: + raise ValueError("unknown format") + + if ret is not None and file is not None: + with open(file, "w") as f: + f.write(ret) + ret = None + plt.close(fig) + return ret + + def init(self, x0=None, control=None, animate=True): """ Initialize for simulation (superclass) @@ -475,32 +601,37 @@ def init(self, x0=None, control=None): if control is not None: # override control self._control = control - - if self._control is not None: + + if isinstance(self._control, VehicleDriverBase): self._control.init() self._t = 0 + self._v_prev = [0] # initialize the graphics - if self._animation is not None: + if animate and self._animation is not None: # setup the plot self._ax = base.plotvol2(self.workspace) - - self._ax.set_xlabel('x') - self._ax.set_ylabel('y') - self._ax.set_aspect('equal') - self._ax.figure.canvas.manager.set_window_title( - f"Robotics Toolbox for Python (Figure {self._ax.figure.number})") + + self._ax.set_xlabel("x") + self._ax.set_ylabel("y") + self._ax.set_aspect("equal") + try: + self._ax.figure.canvas.manager.set_window_title( + f"Robotics Toolbox for Python (Figure {self._ax.figure.number})" + ) + except AttributeError: + pass self._animation.add(ax=self._ax) # add vehicle animation to axis - self._timer = plt.figtext(0.85, 0.95, '') # display time counter + self._timer = plt.figtext(0.85, 0.95, "") # display time counter # initialize the driver if isinstance(self._control, VehicleDriverBase): self._control.init(ax=self._ax) - def step(self, u=None, animate=False): + def step(self, u=None, animate=True, pause=True): r""" Step simulator by one time step (superclass) @@ -558,15 +689,21 @@ def step(self, u=None, animate=False): # do the graphics if animate and self._animation: self._animation.update(self._x) - if self._timer is not None: - self._timer.set_text(f"t = {self._t:.2f}") - plt.pause(self._dt) + # if self._timer is not None: + # self._timer.set_text(f"t = {self._t:.2f}") + if pause: + plt.pause(self._dt) + # plt.show(block=False) + # pass self._t += self._dt # be verbose if self._verbose: - print(f"{self._t:8.2f}: u=({u[0]:8.2f}, {u[1]:8.2f}), x=({self._x[0]:8.2f}, {self._x[1]:8.2f}, {self._x[2]:8.2f})") + print( + f"{self._t:8.2f}: u=({u[0]:8.2f}, {u[1]:8.2f}), x=({self._x[0]:8.2f}," + f" {self._x[1]:8.2f}, {self._x[2]:8.2f})" + ) return odo @@ -585,7 +722,7 @@ def workspace(self): # get workspace specified for Vehicle or from its driver if self._workspace is not None: return self._workspace - if self._control is not None: + if self._control is not None and hasattr(self._control, "_workspace"): return self._control._workspace @property @@ -758,42 +895,11 @@ def stopsim(self): """ self._stopsim = True - # def plot(self, x=None, shape='box', block=False, size=True, **kwargs): - # r""" - # Plot vehicle configuration (superclass) - - # :param x: configuration :math:`(x, y, \theta)`, defaults to None - # :type x: array_like(3), optional - # :param shape: [description], defaults to 'box' - # :type shape: str, optional - # :param block: [description], defaults to False - # :type block: bool, optional - # :param size: [description], defaults to True - # :type size: bool, optional - # :raises ValueError: [description] - # """ - # if shape == 'triangle': - # L = size - # W = 0.6 * size - # vertices = [(L, 0), (-L, -W), (-L, W)] - # elif shape == 'box': - # L1 = size - # L2 = size - # W = 0.6 * size - # vertices = [(-L1, W), (0.6*L2, W), (L2, 0.5*W), (L2, -0.5*W), (0.6*L2, -W), (-L1, -W)] - # elif isinstance(shape, np.ndarray): - # vertices = shape - # else: - # raise ValueError('bad vehicle shape specified') - - # vertices = np.array(vertices).T - # base.plot_poly(SE2(x) * vertices, close=True, **kwargs) - - def plot_xy(self, *args, block=False, **kwargs): + def plot_xy(self, *args, block=None, **kwargs): """ Plot xy-path from history - :param block: block until plot dismissed, defaults to False + :param block: block until plot dismissed, defaults to None :type block: bool, optional :param args: positional arguments passed to :meth:`~matplotlib.axes.Axes.plot` :param kwargs: keyword arguments passed to :meth:`~matplotlib.axes.Axes.plot` @@ -804,13 +910,14 @@ def plot_xy(self, *args, block=False, **kwargs): :seealso: :meth:`run` :meth:`plot_xyt` """ - if args is None and 'color' not in kwargs: - kwargs['color'] = 'b' + if args is None and "color" not in kwargs: + kwargs["color"] = "b" xyt = self.x_hist plt.plot(xyt[:, 0], xyt[:, 1], *args, **kwargs) - plt.show(block=block) + if block is not None: + plt.show(block=block) - def plot_xyt(self, block=False, **kwargs): + def plot_xyt(self, block=None, **kwargs): """ Plot configuration vs time from history @@ -826,79 +933,23 @@ def plot_xyt(self, block=False, **kwargs): """ xyt = self.x_hist t = np.arange(0, xyt.shape[0] * self._dt, self._dt) - plt.plot(xyt[:,0], xyt[:, :], **kwargs) - plt.legend(['x', 'y', '$\\theta$']) - plt.show(block=block) - - - # def path(self, t=10, u=None, x0=None): - # """ - # Compute path by integration (superclass) - - # :param t: integration time in seconds, defaults to 10 - # :type t: float, optional - # :param u: vehicle command, defaults to None - # :type u: array_like(2), optional - # :param x0: initial state, defaults to (0,0,0) - # :type x0: array_like(3), optional - # :return: time vector and state history - # :rtype: ndarray(1), ndarray(n,3) - - # XF = V.path(TF, U) is the final state of the vehicle (3x1) from the initial - # state (0,0,0) with the control inputs U (vehicle specific). TF is a scalar to - # specify the total integration time. - - # XP = V.path(TV, U) is the trajectory of the vehicle (Nx3) from the initial - # state (0,0,0) with the control inputs U (vehicle specific). T is a vector (N) of - # times for which elements of the trajectory will be computed. - - # XP = V.path(T, U, X0) as above but specify the initial state. - - # .. note:: - # - Integration is performed using ODE45. - # - The ODE being integrated is given by the deriv method of the vehicle object. - - # :seealso: :obj:`scipy.integrate.solve_ivp` - # """ - - # # t, x = veh.path(5, u=control) - # # print(t) - # if x0 is None: - # x0 = np.zeros(3) - - # def xdot(t, x, vehicle, u): - # # u = vehicle.control(demand, x) - # return vehicle.deriv(x, u) - - # if base.isscalar(t): - # t_span = (0, t) - # t_eval = np.linspace(0, t, 100) - # elif isinstance(t, np.ndarray): - # t_span = (t[0], t[-1]) - # t_eval = t - # else: - # raise ValueError('bad time argument') - # sol = integrate.solve_ivp(xdot, t_span, x0, t_eval=t_eval, method="RK45", args=(self, u)) - - # return (sol.t, sol.y.T) - + plt.plot(xyt[:, 0], xyt[:, :], **kwargs) + plt.legend(["x", "y", "$\\theta$"]) + if block is not None: + plt.show(block=block) # ========================================================================= # -class Bicycle(VehicleBase): - def __init__(self, - L=1, - steer_max=0.45 * pi, - **kwargs - ): +class Bicycle(VehicleBase): + def __init__(self, L=1, steer_max=0.45 * pi, **kwargs): r""" Create bicycle kinematic model :param L: wheel base, defaults to 1 :type L: float, optional - :param steer_max: [description], defaults to :math:`0.45\pi` + :param steer_max: maximum steering angle, defaults to :math:`0.45\pi` :type steer_max: float, optional :param kwargs: additional arguments passed to :class:`VehicleBase` constructor @@ -924,7 +975,10 @@ def __init__(self, def __str__(self): s = super().__str__() - s += f"\n L={self._l}, steer_max={self._steer_max:g}, speed_max={self._speed_max:g}, accel_max={self._accel_max:g}" + s += ( + f"\n L={self._l}, steer_max={self._steer_max:g}," + f" speed_max={self._speed_max:g}, accel_max={self._accel_max:g}" + ) return s @property @@ -989,6 +1043,8 @@ def deriv(self, x, u, limits=True): :type x: array_like(3) :param u: control input :math:`(v, \gamma)` :type u: array_like(2) + :param limits: limits are applied to input, default True + :type limits: bool :return: state derivative :math:`(\dot{x}, \dot{y}, \dot{\theta})` :rtype: ndarray(3) @@ -1000,9 +1056,12 @@ def deriv(self, x, u, limits=True): \dot{y} &= v \sin \theta \\ \dot{\theta} &= \frac{v}{L} \tan \gamma + If ``limits`` is True then speed, acceleration and steer-angle limits are + applied to ``u``. + :seealso: :meth:`f` """ - + # unpack some variables theta = x[2] @@ -1010,12 +1069,8 @@ def deriv(self, x, u, limits=True): u = self.u_limited(u) v = u[0] gamma = u[1] - - return v * np.r_[ - cos(theta), - sin(theta), - tan(gamma) / self.l - ] + + return v * np.r_[cos(theta), sin(theta), tan(gamma) / self.l] def u_limited(self, u): """ @@ -1026,24 +1081,22 @@ def u_limited(self, u): :return: Allowable vehicle inputs :math:`(v, \gamma)` :rtype: ndarray(2) - Velocity and acceleration limits are applied to :math:`v` and + Velocity and acceleration limits are applied to :math:`v` and steered wheel angle limits are applied to :math:`\gamma`. """ # limit speed and steer angle ulim = np.array(u) - ulim[0] = self.limits_va(u[0]) + ulim[0] = self.limits_va(u[0], self._v_prev) ulim[1] = np.clip(u[1], -self._steer_max, self._steer_max) return ulim + # ========================================================================= # -class Unicycle(VehicleBase): - def __init__(self, - W=1, - steer_max=np.inf, - **kwargs): +class Unicycle(VehicleBase): + def __init__(self, W=1, steer_max=np.inf, **kwargs): r""" Create unicycle kinematic model @@ -1068,16 +1121,19 @@ def __init__(self, :seealso: :meth:`f` :meth:`deriv` :meth:`Fx` meth:`Fv` :class:`Vehicle` """ super().__init__(**kwargs) - self._w = W + self._W = W + self._steer_max = steer_max def __str__(self): s = super().__str__() - s += f"\n W={self._w}, steer_max={self._steer_max}, vel_max={self._vel_max}, accel_max={self.accel_max}" + s += ( + f"\n W={self._W}, steer_max={self._steer_max:g}," + f" vel_max={self._speed_max}, accel_max={self._accel_max}" + ) return s - - def deriv(self, t, x, u): + def deriv(self, x, u, limits=True): r""" Time derivative of state @@ -1085,6 +1141,8 @@ def deriv(self, t, x, u): :type x: array_like(3) :param u: control input :math:`(v, \omega)` :type u: array_like(2) + :param limits: limits are applied to input, default True + :type limits: bool :return: state derivative :math:`(\dot{x}, \dot{y}, \dot{\theta})` :rtype: ndarray(3) @@ -1096,21 +1154,19 @@ def deriv(self, t, x, u): \dot{y} &= v \sin \theta \\ \dot{\theta} &= \omega - :seealso: :meth:`f` + If ``limits`` is True then speed, acceleration and steer-angle limits are + applied to ``u``. - .. note:: Vehicle speed and steering limits are not applied here + :seealso: :meth:`f` """ - + if limits: + u = self.u_limited(u) # unpack some variables theta = x[2] v = u[0] vdiff = u[1] - return np.r_[ - v * cos(theta), - v * sin(theta), - vdiff / self.w - ] + return np.r_[v * cos(theta), v * sin(theta), vdiff / self._W] def u_limited(self, u): """ @@ -1121,31 +1177,147 @@ def u_limited(self, u): :return: Allowable vehicle inputs :math:`(v, \omega)` :rtype: ndarray(2) - Velocity and acceleration limits are applied to :math:`v` and + Velocity and acceleration limits are applied to :math:`v` and turn rate limits are applied to :math:`\omega`. """ # limit speed and steer angle ulim = np.array(u) - ulim[0] = self.limits_va(u[0]) + ulim[0] = self.limits_va(u[0], self._v_prev) ulim[1] = np.maximum(-self._steer_max, np.minimum(self._steer_max, u[1])) return ulim + class DiffSteer(Unicycle): - pass + def __init__(self, W=1, **kwargs): + r""" + Create differential steering kinematic model + + :param W: vehicle width, defaults to 1 + :type W: float, optional + :param kwargs: additional arguments passed to :class:`VehicleBase` + constructor + + Model the motion of a unicycle model with equations of motion given by: + + .. math:: + + \dot{x} &= v \cos \theta \\ + \dot{y} &= v \sin \theta \\ + \dot{\theta} &= \omega + + where :math:`v = (v_R + v_L)/2` is the velocity in body frame x-direction, and + :math:`\omega = (v_R - v_L)/W` is the turn rate. + + :seealso: :meth:`f` :meth:`deriv` :meth:`Fx` meth:`Fv` :class:`Vehicle` + """ + super().__init__(**kwargs) + self._W = W + self._v_prev_L = [0] + self._v_prev_R = [0] + + def __str__(self): + + s = super().__str__() + return s + + def init(self): + super().init() + self._v_prev_L = [0] + self._v_prev_R = [0] + + + def u_limited(self, u): + """ + Apply vehicle velocity and acceleration limits + + :param u: Desired vehicle inputs :math:`(v_L, v_R)` + :type u: array_like(2) + :return: Allowable vehicle inputs :math:`(v_L, v_R)` + :rtype: ndarray(2) + + Velocity and acceleration limits are applied to :math:`v` and + turn rate limits are applied to :math:`\omega`. + """ + + # limit speed and acceleration of each wheel/track + ulim = np.array(u) + ulim[0] = self.limits_va(u[0], self._v_prev_L) + ulim[1] = self.limits_va(u[1], self._v_prev_R) + + return ulim + + def deriv(self, x, u, limits=True): + r""" + Time derivative of state + + :param x: vehicle state :math:`(x, y, \theta)` + :type x: array_like(3) + :param u: Desired vehicle inputs :math:`(v_L, v_R)` + :type u: array_like(2) + :param limits: limits are applied to input, default True + :type limits: bool + :return: state derivative :math:`(\dot{x}, \dot{y}, \dot{\theta})` + :rtype: ndarray(3) + + Returns the time derivative of state (3x1) at the state ``x`` with left and + right wheel speeds ``u``. + + .. math:: + + \dot{x} &= v \cos \theta \\ + \dot{y} &= v \sin \theta \\ + \dot{\theta} &= \omega + + where :math:`v = (v_R + v_L)/2` is the velocity in body frame x-direction, and + :math:`\omega = (v_R - v_L)/W` is the turn rate. + If ``limits`` is True then speed and acceleration limits are applied to the + wheel speeds ``u``. + + :seealso: :meth:`f` + """ + if limits: + u = self.u_limited(u) + # unpack some variables + theta = x[2] + vleft = u[0] + vright = u[1] + + # convert wheel speeds to forward and differential velocity + v = (vright + vleft) / 2.0 + vdiff = vright - vleft + + return np.r_[v * cos(theta), v * sin(theta), vdiff / self._W] + + + if __name__ == "__main__": from roboticstoolbox import RandomPath - V = np.eye(2) * 0.001 - robot = Bicycle(covar=V, animation="car") - odo = robot.step((1, 0.3), animate=False) + import roboticstoolbox as rtb + + a = rtb.VehicleMarker(marker="s", markerfacecolor="b") + veh = rtb.Bicycle(animation=a, workspace=10) + veh.control = (4, 0.5) + veh.run(20) - robot.control = RandomPath(workspace=10) + # V = np.eye(2) * 0.001 + # robot = Bicycle(covar=V, animation="car") + # odo = robot.step((1, 0.3), animate=False) - robot.run(T=10) + # robot.control = RandomPath(workspace=10) + + # # robot.run(T=10, animate=True) + # # plt.show(block=True) + + # anim = robot.run_animation(T=10, format="html", file="veh.html") + + # anim.save("veh.mp4", writer=animation.FFMpegWriter(fps=1/robot.dt)) # convert to mp4/H264 + # with open("veh.html", "w") as f: + # f.write(anim.to_html5_video()) # from math import pi @@ -1188,7 +1360,6 @@ class DiffSteer(Unicycle): # ax.set_xlim(-5, 5) # ax.set_ylim(-5, 5) - # v = VehicleAnimation.Polygon(shape='triangle', maxdim=0.1, color='r') # v = VehicleAnimation.Icon('car3.png', maxdim=2, centre=[0.3, 0.5]) # v = VehicleAnimation.Icon('/Users/corkep/Dropbox/code/robotics-toolbox-python/roboticstoolbox/data/car1.png', maxdim=2, centre=[0.3, 0.5]) @@ -1200,4 +1371,4 @@ class DiffSteer(Unicycle): # for theta in np.linspace(0, 2 * np.pi, 100): # v.update([0, 0, theta]) - # plt.pause(0.1) \ No newline at end of file + # plt.pause(0.1) diff --git a/roboticstoolbox/mobile/__init__.py b/roboticstoolbox/mobile/__init__.py index a2992fce0..77e47f802 100644 --- a/roboticstoolbox/mobile/__init__.py +++ b/roboticstoolbox/mobile/__init__.py @@ -16,13 +16,22 @@ from roboticstoolbox.mobile.Bug2 import Bug2 -from roboticstoolbox.mobile.OccGrid import BinaryOccupancyGrid, OccupancyGrid, PolygonMap +from roboticstoolbox.mobile.OccGrid import ( + BinaryOccupancyGrid, + OccupancyGrid, + PolygonMap, +) # localization and state estimation from roboticstoolbox.mobile.landmarkmap import LandmarkMap from roboticstoolbox.mobile.sensors import RangeBearingSensor from roboticstoolbox.mobile.drivers import * -from roboticstoolbox.mobile.Animations import VehicleAnimationBase, VehicleMarker, VehiclePolygon, VehicleIcon +from roboticstoolbox.mobile.Animations import ( + VehicleAnimationBase, + VehicleMarker, + VehiclePolygon, + VehicleIcon, +) from roboticstoolbox.mobile.PoseGraph import * from roboticstoolbox.mobile.EKF import EKF diff --git a/roboticstoolbox/mobile/drivers.py b/roboticstoolbox/mobile/drivers.py index 5072dbe5f..0d2e0df70 100644 --- a/roboticstoolbox/mobile/drivers.py +++ b/roboticstoolbox/mobile/drivers.py @@ -33,7 +33,7 @@ def demand(self): :return: speed and steering for :class:`VehicleBase` - When an instance of a :class:`VehicleDriverBase` class is attached as + When an instance of a :class:`VehicleDriverBase` class is attached as the control for an instance of a :class:`VehicleBase` class, this method is called at each time step to provide the control input. @@ -71,11 +71,21 @@ def vehicle(self, v): def __repr__(self): return str(self) + + # ========================================================================= # -class RandomPath(VehicleDriverBase): - def __init__(self, workspace, speed=1, dthresh=0.05, seed=0, headinggain=0.3, goalmarkerstyle=None): +class RandomPath(VehicleDriverBase): + def __init__( + self, + workspace, + speed=1, + dthresh=0.05, + seed=0, + headinggain=0.3, + goalmarkerstyle=None, + ): """ Driving agent for random path @@ -86,20 +96,20 @@ def __init__(self, workspace, speed=1, dthresh=0.05, seed=0, headinggain=0.3, go :param dthresh: distance threshold, defaults to 0.05 :type dthresh: float, optional - :raises ValueError: [description] + :raises ValueError: bad workspace specified - Returns a *driver* object that drives the attached vehicle to a + Returns a *driver* object that drives the attached vehicle to a sequence of random waypoints. The driver is connected to the vehicle by:: Vehicle(control=driver) - + or:: veh = Vehicle() veh.control = driver - + The waypoints are positioned inside a rectangular region defined by the vehicle that is specified by (see ``plotvol2``): @@ -119,14 +129,15 @@ def __init__(self, workspace, speed=1, dthresh=0.05, seed=0, headinggain=0.3, go - The vehicle chooses a new waypoint when it is closer than ``dthresh`` to the current waypoint. - Uses its own random number generator so as to not influence the performance - of other randomized algorithms such as path planning. + of other randomized algorithms such as path planning. Set ``seed=None`` to have it randomly initialized from the + operating system. :seealso: :class:`Bicycle` :class:`Unicycle` :func:`~spatialmath.base.graphics.plotvol2` """ - + # TODO options to specify region, maybe accept a Map object? - - if hasattr(workspace, 'workspace'): + + if hasattr(workspace, "workspace"): # workspace can be defined by an object with a workspace attribute self._workspace = base.expand_dims(workspace.workspace) else: @@ -137,35 +148,38 @@ def __init__(self, workspace, speed=1, dthresh=0.05, seed=0, headinggain=0.3, go self._goal_marker = None if goalmarkerstyle is None: self._goal_marker_style = { - 'marker': 'D', - 'markersize': 6, - 'color': 'r', - 'linestyle': 'None', - } + "marker": "D", + "markersize": 6, + "color": "r", + "linestyle": "None", + } else: self._goal_marker_style = goalmarkerstyle self._headinggain = headinggain - + self._d_prev = np.inf self._random = np.random.default_rng(seed) self._seed = seed self.verbose = True self._goal = None self._dthresh = dthresh * max( - self._workspace[1] - self._workspace[0], - self._workspace[3] - self._workspace[2] - ) + self._workspace[1] - self._workspace[0], + self._workspace[3] - self._workspace[2], + ) self._veh = None def __str__(self): """%RandomPath.char Convert to string % - % s = R.char() is a string showing driver parameters and state in in - % a compact human readable format. """ - - s = 'RandomPath driver object\n' - s += f" X {self._workspace[0]} : {self._workspace[1]}; Y {self._workspace[0]} : {self._workspace[1]}, dthresh={self._dthresh}\n" + % s = R.char() is a string showing driver parameters and state in in + % a compact human readable format.""" + + s = "RandomPath driver object\n" + s += ( + f" X {self._workspace[0]} : {self._workspace[1]}; Y {self._workspace[0]} :" + f" {self._workspace[1]}, dthresh={self._dthresh}\n" + ) s += f" current goal={self._goal}" return s @@ -189,19 +203,21 @@ def init(self, ax=None): :param ax: axes in which to draw via points, defaults to None :type ax: Axes, optional - Called at the start of a simulation run. Ensures that the - random number generated is reseeded to ensure that + Called at the start of a simulation run. Ensures that the + random number generated is reseeded to ensure that the sequence of random waypoints is repeatable. """ if self._seed is not None: self._random = np.random.default_rng(self._seed) - + self._goal = None # delete(driver.h_goal); % delete the goal # driver.h_goal = []; if ax is not None: - self._goal_marker = plt.plot(np.nan, np.nan, **self._goal_marker_style, label='random waypoint')[0] + self._goal_marker = plt.plot( + np.nan, np.nan, **self._goal_marker_style, label="random waypoint" + )[0] def demand(self): """ @@ -227,24 +243,23 @@ def demand(self): speed = self._speed goal_heading = atan2( - self._goal[1] - self._veh._x[1], - self._goal[0] - self._veh._x[0] - ) + self._goal[1] - self._veh._x[1], self._goal[0] - self._veh._x[0] + ) delta_heading = base.angdiff(goal_heading, self._veh._x[2]) return np.r_[speed, self._headinggain * delta_heading] ## private method, invoked from demand() to compute a new waypoint - + def _new_goal(self): - + # choose a uniform random goal within inner 80% of driving area while True: r = self._random.uniform(0.1, 0.9) - gx = self._workspace[0:2] @ np.r_[r, 1-r] + gx = self._workspace[0:2] @ np.r_[r, 1 - r] r = self._random.uniform(0.1, 0.9) - gy = self._workspace[2:4] @ np.r_[r, 1-r] + gy = self._workspace[2:4] @ np.r_[r, 1 - r] self._goal = np.r_[gx, gy] @@ -260,10 +275,11 @@ def _new_goal(self): self._goal_marker.set_xdata(self._goal[0]) self._goal_marker.set_ydata(self._goal[1]) + # ========================================================================= # -class PurePursuit(VehicleDriverBase): +class PurePursuit(VehicleDriverBase): def __init__(self, speed=1, radius=1): pass @@ -276,10 +292,9 @@ def init(self): def demand(self): pass + # ========================================================================= # if __name__ == "__main__": import unittest - - diff --git a/roboticstoolbox/mobile/landmarkmap.py b/roboticstoolbox/mobile/landmarkmap.py index 442f11baa..12bef3a56 100644 --- a/roboticstoolbox/mobile/landmarkmap.py +++ b/roboticstoolbox/mobile/landmarkmap.py @@ -6,6 +6,7 @@ from spatialmath import base import roboticstoolbox as rtb + class LandmarkMap: """ Map of planar point landmarks @@ -22,8 +23,8 @@ class LandmarkMap: :rtype: LandmarkMap A LandmarkMap object represents a rectangular 2D environment with a number - of point landmarks. - + of point landmarks. + The landmarks can be specified explicitly or be uniform randomly positioned inside a region defined by the workspace. The workspace can be numeric: @@ -45,7 +46,7 @@ class LandmarkMap: >>> map = LandmarkMap(20) >>> print(map) >>> print(map[3]) # coordinates of landmark 3 - + The object is an iterator that returns consecutive landmark coordinates. :Reference: @@ -58,7 +59,6 @@ class LandmarkMap: """ def __init__(self, map, workspace=10, verbose=True, seed=0): - try: self._workspace = workspace.workspace @@ -77,14 +77,13 @@ def __init__(self, map, workspace=10, verbose=True, seed=0): self._map = np.c_[x, y].T else: - raise ValueError('bad type for map') - - self._verbose = verbose + raise ValueError("bad type for map") + self._verbose = verbose def __str__(self): - # s = M.char() is a string showing map parameters in - # a compact human readable format. + # s = M.char() is a string showing map parameters in + # a compact human readable format. ws = self._workspace s = f"LandmarkMap object with {self._nlandmarks} landmarks, workspace=" s += f"({ws[0]}: {ws[1]}, {ws[2]}: {ws[3]})" @@ -102,7 +101,6 @@ def __len__(self): """ return self._nlandmarks - @property def landmarks(self): """ @@ -137,7 +135,7 @@ def __getitem__(self, k): """ return self._map[:, k] - def plot(self, labels=False, block=False, **kwargs): + def plot(self, labels=False, block=None, **kwargs): """ Plot landmark map @@ -152,21 +150,21 @@ def plot(self, labels=False, block=False, **kwargs): """ ax = base.plotvol2(self._workspace) - ax.set_aspect('equal') + ax.set_aspect("equal") - ax.set_xlabel('x') - ax.set_ylabel('y') + ax.set_xlabel("x") + ax.set_ylabel("y") if len(kwargs) == 0: kwargs = { - 'linewidth': 0, - 'marker': 'x', - 'color': 'black', - 'linestyle': 'None', + "linewidth": 0, + "marker": "x", + "color": "black", + "linestyle": "None", } - if 'label' not in kwargs: - kwargs['label'] = 'landmark' + if "label" not in kwargs: + kwargs["label"] = "landmark" # plt.plot(self._map[0,:], self._map[1,:], , **kwargs) if labels: @@ -175,8 +173,9 @@ def plot(self, labels=False, block=False, **kwargs): labels = None base.plot_point(self._map, text=labels, **kwargs) plt.grid(True) - plt.show(block=block) + if block is not None: + plt.show(block=block) + if __name__ == "__main__": import unittest - diff --git a/roboticstoolbox/mobile/sensors.py b/roboticstoolbox/mobile/sensors.py index e92be75f7..ba0936df7 100644 --- a/roboticstoolbox/mobile/sensors.py +++ b/roboticstoolbox/mobile/sensors.py @@ -31,22 +31,32 @@ """ + class SensorBase(ABC): # TODO, pose option, wrt vehicle # robot # map - + # verbose - + # ls # animate # animate sensor measurements # interval # measurement return subsample factor # fail # delay - - def __init__(self, robot, map, every=1, fail=[], animate=False, delay=0.1, seed=0, verbose=False): + def __init__( + self, + robot, + map, + every=1, + fail=[], + plot=False, + delay=0.1, + seed=0, + verbose=False, + ): """Sensor.Sensor Sensor object constructor % # S = Sensor(VEHICLE, MAP, OPTIONS) is a sensor mounted on a vehicle @@ -66,13 +76,13 @@ def __init__(self, robot, map, every=1, fail=[], animate=False, delay=0.1, seed= self._robot = robot self._map = map self._every = every - self._fail = fail - + self._fail = fail + self._verbose = verbose self.delay = 0.1 - self._animate = animate + self._animate = plot self._seed = seed self.init() @@ -84,8 +94,7 @@ def init(self): - reseed the random number generator - reset the counter for handling the ``every`` and ``fail`` options """ - if self._seed is not None: - self._random = np.random.default_rng(self._seed) + self._random = np.random.default_rng(self._seed) self._count = 0 def __str__(self): @@ -163,7 +172,7 @@ def plot(self, id): :type id: int Draws a line from the robot to landmark ``id``. - + .. note:: - The line is drawn using the ``line_style`` given at constructor time @@ -173,15 +182,15 @@ def plot(self, id): # if isempty(self.ls) # return # end - + # h = findobj(gca, 'tag', 'sensor') # if isempty(h) # # no sensor line, create one # h = plot(0, 0, self.ls, 'tag', 'sensor') # end - + # # there is a sensor line animate it - + # if lm_id == 0 # set(h, 'Visible', 'off') # else @@ -199,20 +208,22 @@ def plot(self, id): # covar can be 2x2 or (2,) # .W property class RangeBearingSensor(SensorBase): - - - def __init__(self, robot, map, - line_style=None, - poly_style=None, - covar=None, - range=None, - angle=None, - plot=False, - seed=0, - **kwargs): + def __init__( + self, + robot, + map, + line_style=None, + poly_style=None, + covar=None, + range=None, + angle=None, + plot=False, + seed=0, + **kwargs, + ): r""" - Range and bearing angke sensor + Range and bearing angle sensor :param robot: model of robot carrying the sensor :type robot: :class:`VehicleBase` subclass @@ -226,7 +237,7 @@ def __init__(self, robot, map, :type range: float or array_like(2), optional :param angle: angular field of view, from :math:`[-\theta, \theta]` defaults to None :type angle: float, optional - :param plot: [description], defaults to False + :param plot: animate the sensor beams, defaults to False :type plot: bool, optional :param seed: random number seed, defaults to 0 :type seed: int, optional @@ -257,6 +268,8 @@ def __init__(self, robot, map, :seealso: :class:`~roboticstoolbox.mobile.LandmarkMap` :class:`~roboticstoolbox.mobile.EKF` """ + # TODO change plot option to animate, but RVC3 uses plot + # call the superclass constructor super().__init__(robot, map, **kwargs) @@ -264,14 +277,14 @@ def __init__(self, robot, map, self._poly_style = poly_style if covar is None: - self._W = np.zeros((2,2)) + self._W = np.zeros((2, 2)) elif base.isvector(covar, 2): self._W = np.diag(covar) elif base.ismatrix(covar, (2, 2)): self._W = covar else: - raise ValueError('bad value for covar, must have shape (2,) or (2,2)') - + raise ValueError("bad value for covar, must have shape (2,) or (2,2)") + if range is None: self._r_range = None elif isinstance(range, Iterable): @@ -290,7 +303,7 @@ def __init__(self, robot, map, self._landmarklog = [] self._random = np.random.default_rng(seed) - + def __str__(self): s = super().__str__() s += f"\n W = {base.array2str(self._W)}\n" @@ -331,7 +344,6 @@ def W(self): the constructor. """ return self._covar - def reading(self): r""" @@ -344,7 +356,7 @@ def reading(self): the ``id`` of that landmark. The landmark is chosen randomly from the set of all visible landmarks, those within the angular field of view and range limit. - + If constructor argument ``every`` is set then only return a valid reading on every ``every`` calls. @@ -361,7 +373,7 @@ def reading(self): >>> map = LandmarkMap(20) >>> sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi/4) >>> print(sensor.reading()) - + .. note:: - Noise with covariance ``W`` (set by constructor) is added to the @@ -373,9 +385,9 @@ def reading(self): :seealso: :meth:`h` """ - + # TODO probably should return K=0 to indicated invalid - + # model a sensor that emits readings every interval samples self._count += 1 @@ -393,12 +405,12 @@ def reading(self): if any([start <= self._count < end for start, end in self._fail]): self._landmarklog.append(lm_id) return (None, None) - + # create a polygon to indicate the active sensing area based on range+angle limits # if self.animate && ~isempty(self.theta_range) && ~isempty(self.r_range) # h = findobj(gca, 'tag', 'sensor-area') # if isempty(h) - + # th=linspace(self.theta_range[0], self.theta_range[1], 20) # x = self.r_range[1] * cos(th) # y = self.r_range[1] * sin(th) @@ -415,7 +427,7 @@ def reading(self): # else # hg = get(h, 'Parent') # plot_poly(h, self.robot.x) - + zk = self.visible() if len(zk) > 1: # more than 1 visible landmark, pick a random one @@ -432,22 +444,22 @@ def reading(self): print(f"Sensor:: feature {lm_id}: ({z[0]}, {z[1]})") else: if self.verbose: - print('Sensor:: no features\n') + print("Sensor:: no features\n") self._landmarklog.append(lm_id) return (None, None) # compute the range and bearing from robot to feature - # z = self.h(self.robot.x, lm_id) - + # z = self.h(self.robot.x, lm_id) + if self._animate: self.plot(lm_id) - + # add the reading to the landmark log self._landmarklog.append(lm_id) # add noise with covariance W z += self._random.multivariate_normal((0, 0), self._W) - + return z, lm_id def visible(self): @@ -467,14 +479,16 @@ def visible(self): z = self.h(self.robot.x) zk = [(z, k) for k, z in enumerate(z)] # a list of tuples, each tuple is ((range, bearing), k) - + if self._r_range is not None: zk = filter(lambda zk: self._r_range[0] <= zk[0][0] <= self._r_range[1], zk) if self._theta_range is not None: # find all within angular range as well - zk = filter(lambda zk: self._theta_range[0] <= zk[0][1] <= self._theta_range[1], zk) - + zk = filter( + lambda zk: self._theta_range[0] <= zk[0][1] <= self._theta_range[1], zk + ) + return list(zk) def isvisible(self, id): @@ -492,10 +506,13 @@ def isvisible(self, id): :seealso: :meth:`visible` :meth:`h` """ z = self.h(self.robot.x, id) - - return ((self._r_range is None) or self._r_range[0] <= z[0] <= self._r_range[1]) and \ - ((self._theta_range is None) or self._theta_range[0] <= z[1] <= self._theta_range[1]) + return ( + (self._r_range is None) or self._r_range[0] <= z[0] <= self._r_range[1] + ) and ( + (self._theta_range is None) + or self._theta_range[0] <= z[1] <= self._theta_range[1] + ) def h(self, x, landmark=None): r""" @@ -513,7 +530,7 @@ def h(self, x, landmark=None): - ``.h(x)`` is range and bearing to all landmarks, one row per landmark - ``.h(x, id)`` is range and bearing to landmark ``id`` - ``.h(x, p)`` is range and bearing to landmark with coordinates ``p`` - + .. runblock:: pycon >>> from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor @@ -561,9 +578,8 @@ def h(self, x, landmark=None): # compute range and bearing (Vectorized code) z = np.c_[ - np.sqrt(dx**2 + dy**2), - base.angdiff(np.arctan2(dy, dx), t) - ] # range & bearing as columns + np.sqrt(dx**2 + dy**2), base.angdiff(np.arctan2(dy, dx), t) + ] # range & bearing as columns if z.shape[0] == 1: return z[0] @@ -586,7 +602,7 @@ def Hx(self, x, landmark): - ``sensor.Hx(q, id)`` is Jacobian for landmark ``id`` - ``sensor.h(q, p)`` is Jacobian for landmark with coordinates ``p`` - + :seealso: :meth:`h` :meth:`Hp` :meth:`Hw` """ @@ -622,7 +638,7 @@ def Hp(self, x, landmark): - ``sensor.Hp(x, id)`` is Jacobian for landmark ``id`` - ``sensor.Hp(x, p)`` is Jacobian for landmark with coordinates ``p`` - + :seealso: :meth:`h` :meth:`Hx` :meth:`Hw` """ if base.isinteger(landmark): @@ -658,7 +674,7 @@ def Hw(self, x, landmark): - ``sensor.Hw(x, p)`` is Jacobian for landmark with coordinates ``p`` .. note:: ``x`` and ``landmark`` are not used to compute this. - + :seealso: :meth:`h` :meth:`Hx` :meth:`Hp` """ return np.eye(2) @@ -680,7 +696,6 @@ def g(self, x, z): :seealso: :meth:`h` :meth:`Gx` :meth:`Gz` """ - range = z[0] bearing = z[1] + x[2] # bearing angle in vehicle frame @@ -742,15 +757,16 @@ def Gz(self, x, z): ]) # fmt: on + if __name__ == "__main__": - + from roboticstoolbox import Bicycle, LandmarkMap, RangeBearingSensor from math import pi + robot = Bicycle() map = LandmarkMap(20) - sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi/4) + sensor = RangeBearingSensor(robot, map, range=(0.5, 20), angle=pi / 4) print(sensor.reading()) print(sensor.visible()) print(sensor.isvisible(3)) print(sensor.isvisible(4)) - \ No newline at end of file diff --git a/roboticstoolbox/models/DH/Sawyer.py b/roboticstoolbox/models/DH/Sawyer.py index 205fd22e9..449ea6f7c 100644 --- a/roboticstoolbox/models/DH/Sawyer.py +++ b/roboticstoolbox/models/DH/Sawyer.py @@ -58,7 +58,7 @@ def __init__(self, symbolic=False): links = [] - for j in range(6): + for j in range(7): link = RevoluteDH(d=d[j], a=a[j], alpha=alpha[j]) links.append(link) @@ -73,8 +73,8 @@ def __init__(self, symbolic=False): symbolic=symbolic, ) - self.qr = np.zeros(6) - self.qz = np.zeros(6) + self.qr = np.zeros(7) + self.qz = np.zeros(7) self.addconfiguration("qr", self.qr) self.addconfiguration("qz", self.qz) diff --git a/roboticstoolbox/models/ETS/Frankie.py b/roboticstoolbox/models/ETS/Frankie.py index 9921df66d..3b7de44c7 100644 --- a/roboticstoolbox/models/ETS/Frankie.py +++ b/roboticstoolbox/models/ETS/Frankie.py @@ -3,11 +3,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -class Frankie(ERobot): +class Frankie(Robot): """ A class representing the Franka Emika Panda robot arm. ETS taken from [1] based on https://frankaemika.github.io/docs/control_parameters.html diff --git a/roboticstoolbox/models/ETS/GenericSeven.py b/roboticstoolbox/models/ETS/GenericSeven.py index eb1565621..91bb31276 100644 --- a/roboticstoolbox/models/ETS/GenericSeven.py +++ b/roboticstoolbox/models/ETS/GenericSeven.py @@ -3,11 +3,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -class GenericSeven(ERobot): +class GenericSeven(Robot): """ Create model of a generic seven degree-of-freedom robot diff --git a/roboticstoolbox/models/ETS/Omni.py b/roboticstoolbox/models/ETS/Omni.py index 2fa84ab40..f8836f42c 100644 --- a/roboticstoolbox/models/ETS/Omni.py +++ b/roboticstoolbox/models/ETS/Omni.py @@ -3,13 +3,13 @@ import numpy as np from roboticstoolbox.robot.ET import ET from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link import spatialgeometry as sg import spatialmath as sm -class Omni(ERobot): +class Omni(Robot): """ A class an omnidirectional mobile robot. diff --git a/roboticstoolbox/models/ETS/Panda.py b/roboticstoolbox/models/ETS/Panda.py index 6657251ce..592b94e3a 100644 --- a/roboticstoolbox/models/ETS/Panda.py +++ b/roboticstoolbox/models/ETS/Panda.py @@ -2,11 +2,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -class Panda(ERobot): +class Panda(Robot): """ Create model of Franka-Emika Panda manipulator diff --git a/roboticstoolbox/models/ETS/Planar2.py b/roboticstoolbox/models/ETS/Planar2.py index 946835fd1..1603e9c13 100644 --- a/roboticstoolbox/models/ETS/Planar2.py +++ b/roboticstoolbox/models/ETS/Planar2.py @@ -3,11 +3,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET2 from roboticstoolbox.robot.ETS import ETS2 -from roboticstoolbox.robot.ERobot import ERobot2 +from roboticstoolbox.robot.Robot import Robot2 from roboticstoolbox.robot.Link import Link2 -class Planar2(ERobot2): +class Planar2(Robot2): """ Create model of a branched planar manipulator:: @@ -35,11 +35,11 @@ def __init__(self): super().__init__([l0, l1, l2], name="Planar2", comment="Planar 2D manipulator") - self.qr = np.array([0, 0]) + self.qb = np.array([0, np.pi/2]) self.qz = np.zeros(2) - self.addconfiguration("qr", self.qr) self.addconfiguration("qz", self.qz) + self.addconfiguration("qb", self.qb) if __name__ == "__main__": # pragma nocover diff --git a/roboticstoolbox/models/ETS/Planar_Y.py b/roboticstoolbox/models/ETS/Planar_Y.py index 9f8c7cf03..92d5f9f9d 100644 --- a/roboticstoolbox/models/ETS/Planar_Y.py +++ b/roboticstoolbox/models/ETS/Planar_Y.py @@ -3,11 +3,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -class Planar_Y(ERobot): +class Planar_Y(Robot): """ Create model of a branched planar manipulator:: diff --git a/roboticstoolbox/models/ETS/Puma560.py b/roboticstoolbox/models/ETS/Puma560.py index be4b3d530..152f3f34d 100644 --- a/roboticstoolbox/models/ETS/Puma560.py +++ b/roboticstoolbox/models/ETS/Puma560.py @@ -1,9 +1,9 @@ from roboticstoolbox import ET as ET -from roboticstoolbox import ERobot +from roboticstoolbox import Robot import numpy as np -class Puma560(ERobot): +class Puma560(Robot): """ Create model of Franka-Emika Panda manipulator diff --git a/roboticstoolbox/models/ETS/XYPanda.py b/roboticstoolbox/models/ETS/XYPanda.py index a43361188..8181cc53f 100644 --- a/roboticstoolbox/models/ETS/XYPanda.py +++ b/roboticstoolbox/models/ETS/XYPanda.py @@ -3,11 +3,11 @@ import numpy as np from roboticstoolbox.robot.ET import ET from roboticstoolbox.robot.ETS import ETS -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link -class XYPanda(ERobot): +class XYPanda(Robot): """ Create model of Franka-Emika Panda manipulator on an XY platform diff --git a/roboticstoolbox/models/URDF/AL5D.py b/roboticstoolbox/models/URDF/AL5D.py index 47a3595ee..614aa5696 100644 --- a/roboticstoolbox/models/URDF/AL5D.py +++ b/roboticstoolbox/models/URDF/AL5D.py @@ -1,10 +1,11 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from math import pi -class AL5D(ERobot): + +class AL5D(Robot): """ Class that imports a AL5D URDF model @@ -47,6 +48,7 @@ def __init__(self): # reference pose robot pointing upwards self.addconfiguration("up", np.array([0.0000, 0.0000, 1.5707, 0.0000])) + if __name__ == "__main__": # pragma nocover robot = AL5D() diff --git a/roboticstoolbox/models/URDF/Fetch.py b/roboticstoolbox/models/URDF/Fetch.py index e7a53cb74..cd5066ff2 100644 --- a/roboticstoolbox/models/URDF/Fetch.py +++ b/roboticstoolbox/models/URDF/Fetch.py @@ -1,11 +1,12 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot + # from spatialmath import SE3 -class Fetch(ERobot): +class Fetch(Robot): """ Class that imports a Fetch URDF model diff --git a/roboticstoolbox/models/URDF/FetchCamera.py b/roboticstoolbox/models/URDF/FetchCamera.py index be562ae84..c98f9fcae 100644 --- a/roboticstoolbox/models/URDF/FetchCamera.py +++ b/roboticstoolbox/models/URDF/FetchCamera.py @@ -1,11 +1,12 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot + # from spatialmath import SE3 -class FetchCamera(ERobot): +class FetchCamera(Robot): """ Class that imports a FetchCamera URDF model diff --git a/roboticstoolbox/models/URDF/Frankie.py b/roboticstoolbox/models/URDF/Frankie.py index 731e64a98..34961a24d 100644 --- a/roboticstoolbox/models/URDF/Frankie.py +++ b/roboticstoolbox/models/URDF/Frankie.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from spatialmath import SE3 -class Frankie(ERobot): +class Frankie(Robot): """ Class that imports a Frankie URDF model diff --git a/roboticstoolbox/models/URDF/FrankieOmni.py b/roboticstoolbox/models/URDF/FrankieOmni.py index be2e2db20..12101d4ec 100644 --- a/roboticstoolbox/models/URDF/FrankieOmni.py +++ b/roboticstoolbox/models/URDF/FrankieOmni.py @@ -1,14 +1,14 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from roboticstoolbox.robot.Link import Link from roboticstoolbox.robot.ETS import ETS from roboticstoolbox.robot.ET import ET from spatialmath import SE3 -class FrankieOmni(ERobot): +class FrankieOmni(Robot): """ Class that imports an Omnidirectional Frankie URDF model diff --git a/roboticstoolbox/models/URDF/KinovaGen3.py b/roboticstoolbox/models/URDF/KinovaGen3.py index 7dc61ef29..5e6a79fda 100644 --- a/roboticstoolbox/models/URDF/KinovaGen3.py +++ b/roboticstoolbox/models/URDF/KinovaGen3.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class KinovaGen3(ERobot): +class KinovaGen3(Robot): """ Class that imports a KinovaGen3 URDF model diff --git a/roboticstoolbox/models/URDF/LBR.py b/roboticstoolbox/models/URDF/LBR.py index 80fec406f..c881a66ba 100644 --- a/roboticstoolbox/models/URDF/LBR.py +++ b/roboticstoolbox/models/URDF/LBR.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class LBR(ERobot): +class LBR(Robot): """ Class that imports a LBR URDF model diff --git a/roboticstoolbox/models/URDF/Mico.py b/roboticstoolbox/models/URDF/Mico.py index fbedf020c..f8fd2a46b 100644 --- a/roboticstoolbox/models/URDF/Mico.py +++ b/roboticstoolbox/models/URDF/Mico.py @@ -2,10 +2,10 @@ import numpy as np from roboticstoolbox.robot.Link import Link -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class Mico(ERobot): +class Mico(Robot): """ Class that imports a Mico URDF model diff --git a/roboticstoolbox/models/URDF/PR2.py b/roboticstoolbox/models/URDF/PR2.py index 60db8cdb0..e1ab35b42 100644 --- a/roboticstoolbox/models/URDF/PR2.py +++ b/roboticstoolbox/models/URDF/PR2.py @@ -1,10 +1,10 @@ #!/usr/bin/env python -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot import numpy as np -class PR2(ERobot): +class PR2(Robot): def __init__(self): links, name, urdf_string, urdf_filepath = self.URDF_read( @@ -30,6 +30,8 @@ def __init__(self): self.addconfiguration("qr", self.qr) self.addconfiguration("qz", self.qz) + self.qdlim = 2.0 * np.ones(31) + if __name__ == "__main__": # pragma nocover diff --git a/roboticstoolbox/models/URDF/Panda.py b/roboticstoolbox/models/URDF/Panda.py index c45a05d41..9eb010300 100644 --- a/roboticstoolbox/models/URDF/Panda.py +++ b/roboticstoolbox/models/URDF/Panda.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot, ERobot2 +from roboticstoolbox.robot.Robot import Robot from spatialmath import SE3 -class Panda(ERobot): +class Panda(Robot): """ Class that imports a Panda URDF model diff --git a/roboticstoolbox/models/URDF/Puma560.py b/roboticstoolbox/models/URDF/Puma560.py index 7256bed76..7e946ee03 100644 --- a/roboticstoolbox/models/URDF/Puma560.py +++ b/roboticstoolbox/models/URDF/Puma560.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from math import pi -class Puma560(ERobot): +class Puma560(Robot): """ Class that imports a Puma 560 URDF model @@ -94,3 +94,5 @@ def __init__(self): robot = Puma560() print(robot) + + print(robot.ee_links) diff --git a/roboticstoolbox/models/URDF/UR10.py b/roboticstoolbox/models/URDF/UR10.py index bc318af71..180b343c1 100644 --- a/roboticstoolbox/models/URDF/UR10.py +++ b/roboticstoolbox/models/URDF/UR10.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class UR10(ERobot): +class UR10(Robot): """ Class that imports a UR10 URDF model diff --git a/roboticstoolbox/models/URDF/UR3.py b/roboticstoolbox/models/URDF/UR3.py index 6a7bc1671..c59da8d16 100644 --- a/roboticstoolbox/models/URDF/UR3.py +++ b/roboticstoolbox/models/URDF/UR3.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class UR3(ERobot): +class UR3(Robot): """ Class that imports a UR3 URDF model diff --git a/roboticstoolbox/models/URDF/UR5.py b/roboticstoolbox/models/URDF/UR5.py index 7d4b67f5c..6bbfab9bc 100644 --- a/roboticstoolbox/models/URDF/UR5.py +++ b/roboticstoolbox/models/URDF/UR5.py @@ -1,11 +1,11 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot from math import pi -class UR5(ERobot): +class UR5(Robot): """ Class that imports a UR5 URDF model diff --git a/roboticstoolbox/models/URDF/Valkyrie.py b/roboticstoolbox/models/URDF/Valkyrie.py index 916555996..51b33b078 100644 --- a/roboticstoolbox/models/URDF/Valkyrie.py +++ b/roboticstoolbox/models/URDF/Valkyrie.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot, Link +from roboticstoolbox.robot.Robot import Robot, Link -class Valkyrie(ERobot): +class Valkyrie(Robot): """ Class that imports a NASA Valkyrie URDF model diff --git a/roboticstoolbox/models/URDF/YuMi.py b/roboticstoolbox/models/URDF/YuMi.py index 96a77854c..fa0414586 100644 --- a/roboticstoolbox/models/URDF/YuMi.py +++ b/roboticstoolbox/models/URDF/YuMi.py @@ -2,11 +2,11 @@ from roboticstoolbox.robot.Link import Link import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot import spatialmath as sm -class YuMi(ERobot): +class YuMi(Robot): """ Class that imports an ABB YuMi URDF model diff --git a/roboticstoolbox/models/URDF/px100.py b/roboticstoolbox/models/URDF/px100.py index e9fd8e0a7..243d02162 100644 --- a/roboticstoolbox/models/URDF/px100.py +++ b/roboticstoolbox/models/URDF/px100.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class px100(ERobot): +class px100(Robot): """ Class that imports a PX100 URDF model diff --git a/roboticstoolbox/models/URDF/px150.py b/roboticstoolbox/models/URDF/px150.py index 442d0ef1c..30299d93e 100644 --- a/roboticstoolbox/models/URDF/px150.py +++ b/roboticstoolbox/models/URDF/px150.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class px150(ERobot): +class px150(Robot): """ Class that imports a PX150 URDF model diff --git a/roboticstoolbox/models/URDF/rx150.py b/roboticstoolbox/models/URDF/rx150.py index aad7f98fb..3a40bff18 100644 --- a/roboticstoolbox/models/URDF/rx150.py +++ b/roboticstoolbox/models/URDF/rx150.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class rx150(ERobot): +class rx150(Robot): """ Class that imports a RX150 URDF model diff --git a/roboticstoolbox/models/URDF/rx200.py b/roboticstoolbox/models/URDF/rx200.py index be33450bf..1a38d6591 100644 --- a/roboticstoolbox/models/URDF/rx200.py +++ b/roboticstoolbox/models/URDF/rx200.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class rx200(ERobot): +class rx200(Robot): """ Class that imports a RX200 URDF model diff --git a/roboticstoolbox/models/URDF/vx300.py b/roboticstoolbox/models/URDF/vx300.py index e7008db41..f195a6450 100644 --- a/roboticstoolbox/models/URDF/vx300.py +++ b/roboticstoolbox/models/URDF/vx300.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class vx300(ERobot): +class vx300(Robot): """ Class that imports a VX300 URDF model diff --git a/roboticstoolbox/models/URDF/vx300s.py b/roboticstoolbox/models/URDF/vx300s.py index 8c7f9f8f8..41f8db13e 100644 --- a/roboticstoolbox/models/URDF/vx300s.py +++ b/roboticstoolbox/models/URDF/vx300s.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class vx300s(ERobot): +class vx300s(Robot): """ Class that imports a VX300s URDF model diff --git a/roboticstoolbox/models/URDF/wx200.py b/roboticstoolbox/models/URDF/wx200.py index b5328905c..337328f37 100755 --- a/roboticstoolbox/models/URDF/wx200.py +++ b/roboticstoolbox/models/URDF/wx200.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class wx200(ERobot): +class wx200(Robot): """ Class that imports a WX200 URDF model diff --git a/roboticstoolbox/models/URDF/wx250.py b/roboticstoolbox/models/URDF/wx250.py index 1af879866..882273226 100644 --- a/roboticstoolbox/models/URDF/wx250.py +++ b/roboticstoolbox/models/URDF/wx250.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class wx250(ERobot): +class wx250(Robot): """ Class that imports a WX250 URDF model diff --git a/roboticstoolbox/models/URDF/wx250s.py b/roboticstoolbox/models/URDF/wx250s.py index 9602f8d9c..3a43e465c 100644 --- a/roboticstoolbox/models/URDF/wx250s.py +++ b/roboticstoolbox/models/URDF/wx250s.py @@ -1,10 +1,10 @@ #!/usr/bin/env python import numpy as np -from roboticstoolbox.robot.ERobot import ERobot +from roboticstoolbox.robot.Robot import Robot -class wx250s(ERobot): +class wx250s(Robot): """ Class that imports a wx250s URDF model diff --git a/roboticstoolbox/robot/BaseRobot.py b/roboticstoolbox/robot/BaseRobot.py new file mode 100644 index 000000000..92277a4ea --- /dev/null +++ b/roboticstoolbox/robot/BaseRobot.py @@ -0,0 +1,3090 @@ +#!/usr/bin/env python + +""" +@author: Jesse Haviland +@author: Peter Corke +""" + +# import sys +from abc import ABC +from copy import deepcopy +from functools import lru_cache +from typing import ( + IO, + TYPE_CHECKING, + Any, + Callable, + Generic, + List, + TypeVar, + Union, + Dict, + Tuple, + Set, +) + +from typing_extensions import Literal as L + +import numpy as np + +from spatialmath import SE3 +from spatialmath.base.argcheck import ( + getvector, + getmatrix, + getunit, +) + +from ansitable import ANSITable, Column +from swift import Swift +from spatialgeometry import SceneNode + +from roboticstoolbox.fknm import Robot_link_T +import roboticstoolbox as rtb +from roboticstoolbox.robot.Gripper import Gripper +from roboticstoolbox.robot.Link import BaseLink, Link +from roboticstoolbox.robot.ETS import ETS +from roboticstoolbox.robot.ET import ET +from roboticstoolbox.robot.Dynamics import DynamicsMixin +from roboticstoolbox.tools.types import ArrayLike, NDArray +from roboticstoolbox.tools.params import rtb_get_param +from roboticstoolbox.backends.PyPlot import PyPlot, PyPlot2 +from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot + + +if TYPE_CHECKING: + from matplotlib.cm import Color # pragma nocover +else: + Color = None + +try: + from matplotlib import colors + from matplotlib import cm + + _mpl = True +except ImportError: # pragma nocover + cm = str + pass + +# _default_backend = None + +# A generic type variable representing any subclass of BaseLink +LinkType = TypeVar("LinkType", bound=BaseLink) + + +class BaseRobot(SceneNode, DynamicsMixin, ABC, Generic[LinkType]): + def __init__( + self, + links: List[LinkType], + gripper_links: Union[LinkType, List[LinkType], None] = None, + name: str = "", + manufacturer: str = "", + comment: str = "", + base: Union[NDArray, SE3, None] = None, + tool: Union[NDArray, SE3, None] = None, + gravity: ArrayLike = [0, 0, -9.81], + keywords: Union[List[str], Tuple[str]] = [], + symbolic: bool = False, + configs: Union[Dict[str, NDArray], None] = None, + check_jindex: bool = True, + ): + # Initialise the scene node + SceneNode.__init__(self) + + # Lets sort out links now + self._linkdict: Dict[str, LinkType] = {} + + # Sort links and set self.link, self.n, self.base_link, + # self.ee_links + self._sort_links(links, gripper_links, check_jindex) + + # Fix number of links for gripper links + self._nlinks = len(links) + + for gripper in self.grippers: + self._nlinks += len(gripper.links) + + # Set the pose of the robot in the world frame + # in the scenenode object to a numpy array + if isinstance(base, SE3): + self._T = base.A + elif isinstance(base, np.ndarray): + self._T = base + + # Set the robot tool transform + if isinstance(tool, SE3): + self._tool = tool.A + elif isinstance(tool, np.ndarray): + self._tool = tool + else: + self._tool = np.eye(4) + + # Set the keywords + if keywords is not None and not isinstance(keywords, (tuple, list)): + raise TypeError("keywords must be a list or tuple") + else: + self._keywords = list(keywords) + + # Gravity is in the negative-z direction. + self.gravity = np.array(gravity) + + # Basic arguments + self.name = name + self.manufacturer = manufacturer + self.comment = comment + self._symbolic = symbolic + self._reach = None + self._hasdynamics = False + self._hasgeometry = False + self._hascollision = False + self._urdf_string = "" + self._urdf_filepath = "" + + # Time to checkout the links for geometry information + for link in self.links: + # Add link back to robot object + link._robot = self + + if link.hasdynamics: + self._hasdynamics = True + if link.geometry: + self._hasgeometry = [] + if link.collision: + self._hascollision = True + + if isinstance(link, Link): + if len(link.geometry) > 0: + self._hasgeometry = True + + # Current joint configuraiton, velocity, acceleration + self.q = np.zeros(self.n) + self.qd = np.zeros(self.n) + self.qdd = np.zeros(self.n) + + # The current control mode of the robot + self.control_mode = "v" + + # Set up named configuration property + if configs is None: + configs = dict() + self._configs = configs + + # A flag for watching dynamics properties + self._dynchanged = False + + # Set up qlim + qlim = np.zeros((2, self.n)) + j = 0 + + for i in range(len(self.links)): + if self.links[i].isjoint: + qlim[:, j] = self.links[i].qlim + j += 1 + self._qlim = qlim + + self._valid_qlim = False + for i in range(self.n): + if any(qlim[:, i] != 0) and not any(np.isnan(qlim[:, i])): + self._valid_qlim = True + + # SceneNode, set a reference to the first link + self.scene_children = [self.links[0]] # type: ignore + + self._default_backend = None + + # --------------------------------------------------------------------- # + # --------- Private Methods ------------------------------------------- # + # --------------------------------------------------------------------- # + + def _sort_links( + self, + links: List[LinkType], + gripper_links: Union[LinkType, List[LinkType], None], + check_jindex: bool, + ): + """ + This method does several things for setting up the links of a robot + + - Gives each link a unique name if it doesn't have one + - Assigns each link a parent if it doesn't have one + - Finds and sets the base link + - Finds and sets the ee links + - sets the jindices + - sets n + - sets links + + """ + + # The ordered links + orlinks: List[LinkType] = [] + + # The end-effector links + self._ee_links: List[LinkType] = [] + + # Check all the incoming Link objects + n: int = 0 + + # Make sure each link has a name + # ------------------------------ + for k, link in enumerate(links): + if not isinstance(link, BaseLink): + raise TypeError("links should all be Link subclass") + + # If link has no name, give it one + if link.name is None or link.name == "": + link.name = f"link-{k}" + + link.number = k + 1 + + # Put it in the link dictionary, check for duplicates + if link.name in self._linkdict: + raise ValueError(f"link name {link.name} is not unique") + + self._linkdict[link.name] = link + + if link.isjoint: + n += 1 + + # Resolve parents given by name, within the context of + # this set of links + # ---------------------------------------------------- + for link in links: + if link.parent is None and link.parent_name is not None: + link.parent = self._linkdict[link.parent_name] + + if all([link.parent is None for link in links]): + # No parent links were given, assume they are sequential + for i in range(len(links) - 1): + # li = links[i] + links[i + 1].parent = links[i] + + # Set the base link + # ----------------- + for link in links: + # Is this a base link? + + if isinstance(link.parent, BaseLink): + # Update children of this link's parent + link.parent._children.append(link) + else: + try: + if self._base_link is not None: + raise ValueError("Multiple base links") + except AttributeError: + pass + + self._base_link = link + + if not hasattr(self, "_base_link"): + raise ValueError( + "Invalid link configuration provided, must have a base link" + ) + + # Scene node, set links between the links + # --------------------------------------- + for link in links: + if isinstance(link.parent, BaseLink): + link.scene_parent = link.parent + + # Set up the gripper, make a list containing the root of all + # grippers + # ---------------------------------------------------------- + if gripper_links is None: + gripper_links = [] + + if not isinstance(gripper_links, list): + gripper_links = [gripper_links] + + # An empty list to hold all grippers + self._grippers = [] + + # Make a gripper object for each gripper + for link in gripper_links: + g_links = self.dfs_links(link) + + # Remove gripper links from the robot + for g_link in g_links: + # print(g_link) + links.remove(g_link) + + # Save the gripper object + self._grippers.append(Gripper(g_links, name=link.name)) + + # Subtract the n of the grippers from the n of the robot + for gripper in self._grippers: + n -= gripper.n + + # Set the ee links + # ---------------- + ee_links: List[LinkType] = [] + + if len(gripper_links) == 0: + for link in links: + # Is this a leaf node? and do we not have any grippers + if link.children is None or len(link.children) == 0: + # No children, must be an end-effector + ee_links.append(link) + else: + for link in gripper_links: + # Use the passed in value + if link.parent is not None: + ee_links.append(link.parent) + + self._ee_links = ee_links + + # Assign the joint indices and sort the links + # ------------------------------------------- + if all([link.jindex is None or link.ets._auto_jindex for link in links]): + # No joints have an index + jindex = [0] # "mutable integer" hack + + def visit_link(link, jindex): + # if it's a joint, assign it a jindex and increment it + if link.isjoint and link in links: + link.jindex = jindex[0] + jindex[0] += 1 + + if link in links: + orlinks.append(link) + + # visit all links in DFS order + self.dfs_links(self.base_link, lambda link: visit_link(link, jindex)) + + elif all( + [ + link.jindex is not None and not link.ets._auto_jindex + for link in links + if link.isjoint + ] + ): + # Jindex set on all, check they are unique and contiguous + if check_jindex: + jset = set(range(n)) + for link in links: + if link.isjoint and link.jindex not in jset: + raise ValueError( + f"joint index {link.jindex} was repeated or out of range" + ) + jset -= set([link.jindex]) + if len(jset) > 0: # pragma nocover # is impossible + raise ValueError(f"joints {jset} were not assigned") + orlinks = links + else: + # must be a mixture of Links with/without jindex + raise ValueError("all links must have a jindex, or none have a jindex") + + # Set n + # ----- + self._n = n + + # Set links + # --------- + self._links = orlinks + + def dynchanged(self, what: Union[str, None] = None): + """ + Dynamic parameters have changed + + Called from a property setter to inform the robot that the cache of + dynamic parameters is invalid. + + See Also + -------- + :func:`roboticstoolbox.Link._listen_dyn` + + """ + + self._dynchanged = True + if what != "gravity": + self._hasdynamics = True + + # --------------------------------------------------------------------- # + # --------- Magic Methods --------------------------------------------- # + # --------------------------------------------------------------------- # + + def __iter__(self): + self._iter = 0 + return self + + def __next__(self) -> LinkType: + if self._iter < len(self.links): + link = self[self._iter] + self._iter += 1 + return link + else: + raise StopIteration + + def __getitem__(self, i: Union[int, str]) -> LinkType: + """ + Get link + + This also supports iterating over each link in the robot object, + from the base to the tool. + + Parameters + ---------- + i + link number or name + + Returns + ------- + link + i'th link or named link + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> print(robot[1]) # print the 2nd link + >>> print([link.a for link in robot]) # print all the a_j values + + Notes + ----- + ``Robot`` supports link lookup by name, + eg. ``robot['link1']`` + + """ + + if isinstance(i, int): + return self._links[i] + else: + return self._linkdict[i] + + def __str__(self) -> str: + """ + Pretty prints the ETS Model of the robot. + + Returns + ------- + str + Pretty print of the robot model + + Notes + ----- + - Constant links are shown in blue. + - End-effector links are prefixed with an @ + - Angles in degrees + - The robot base frame is denoted as ``BASE`` and is equal to the + robot's ``base`` attribute. + + """ + + unicode = rtb_get_param("unicode") + border = "thin" if unicode else "ascii" + + table = ANSITable( + Column("link", headalign="^", colalign=">"), + Column("link", headalign="^", colalign="<"), + Column("joint", headalign="^", colalign=">"), + Column("parent", headalign="^", colalign="<"), + Column("ETS: parent to link", headalign="^", colalign="<"), + border=border, + ) + + for k, link in enumerate(self.links): + color = "" if link.isjoint else "<>" + ee = "@" if link in self.ee_links else "" + ets = link.ets + if link.parent is None: + parent_name = "BASE" + else: + parent_name = link.parent.name + s = ets.__str__(f"q{link.jindex}") + # if len(s) > 0: + # op = " \u2295 " if unicode else " * " # \oplus + # s = op + s + + if link.isjoint: + jname = link.jindex + else: + jname = "" + table.row( + # link.jindex, + k, + color + ee + link.name, + jname, + parent_name, + f"{s}", + ) + + classname = "ERobot" + + s = f"{classname}: {self.name}" + if self.manufacturer is not None and len(self.manufacturer) > 0: + s += f" (by {self.manufacturer})" + s += f", {self.n} joints ({self.structure})" + if len(self.grippers) > 0: + s += ( + f", {len(self.grippers)} gripper{'s' if len(self.grippers) > 1 else ''}" + ) + if self.nbranches > 1: + s += f", {self.nbranches} branches" + if self._hasdynamics: + s += ", dynamics" + if any([len(link.geometry) > 0 for link in self.links]): + s += ", geometry" + if any([len(link.collision) > 0 for link in self.links]): + s += ", collision" + s += "\n" + + s += str(table) + s += self.configurations_str(border=border) + + return s + + def __repr__(self) -> str: + return str(self) + + # --------------------------------------------------------------------- # + # --------- Properties ------------------------------------------------ # + # --------------------------------------------------------------------- # + + @property + def links(self) -> List[LinkType]: + """ + Robot links + + Returns + ------- + links + A list of link objects + + Notes + ----- + It is probably more concise to index the robot object rather + than the list of links, ie. the following are equivalent: + - ``robot.links[i]`` + - ``robot[i]`` + + """ + + return self._links + + @property + def link_dict(self) -> Dict[str, LinkType]: + return self._linkdict + + @property + def grippers(self) -> List[Gripper]: + """ + Grippers attached to the robot + + Returns + ------- + grippers + A list of grippers + + """ + + return self._grippers + + @property + def base_link(self) -> LinkType: + """ + Get the robot base link + + - ``robot.base_link`` is the robot base link + + Returns + ------- + base_link + the first link in the robot tree + + """ + + return self._base_link + + @property + def ee_links(self) -> List[LinkType]: + return self._ee_links + + @property + def n(self) -> int: + """ + Number of joints + + Returns + ------- + n + Number of joints + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.n + + See Also + -------- + :func:`nlinks` + :func:`nbranches` + + """ + + return self._n + + @property + def nlinks(self): + """ + Number of links + + The returned number is the total of both variable joints and + static links + + Returns + ------- + nlinks + Number of links + + Examples + -------- + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.nlinks + + See Also + -------- + :func:`n` + :func:`nbranches` + + """ + + return self._nlinks + + @property + def nbranches(self) -> int: + """ + Number of branches + + Number of branches in this robot. Computed as the number of links with + zero children + + Returns + ------- + nbranches + number of branches in the robot's kinematic tree + + Examples + -------- + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.ETS.Panda() + >>> robot.nbranches + + See Also + -------- + :func:`n` + :func:`nlinks` + """ + + return sum([link.nchildren == 0 for link in self.links]) + len(self.grippers) + + # --------------------------------------------------------------------- # + + @property + def name(self) -> str: + """ + Get/set robot name + + - ``robot.name`` is the robot name + - ``robot.name = ...`` checks and sets the robot name + + Parameters + ---------- + name + the new robot name + + Returns + ------- + name + the current robot name + + """ + return self._name + + @name.setter + def name(self, name_new: str): + self._name = name_new + + @property + def comment(self) -> str: + """ + Get/set robot comment + + - ``robot.comment`` is the robot comment + - ``robot.comment = ...`` checks and sets the robot comment + + Parameters + ---------- + name + the new robot comment + + Returns + ------- + comment + robot comment + + """ + return self._comment + + @comment.setter + def comment(self, comment_new: str): + self._comment = comment_new + + @property + def manufacturer(self): + """ + Get/set robot manufacturer's name + + - ``robot.manufacturer`` is the robot manufacturer's name + - ``robot.manufacturer = ...`` checks and sets the manufacturer's name + + Returns + ------- + manufacturer + robot manufacturer's name + + """ + return self._manufacturer + + @manufacturer.setter + def manufacturer(self, manufacturer_new): + self._manufacturer = manufacturer_new + + @property + def configs(self) -> Dict[str, NDArray]: + return self._configs + + @property + def keywords(self) -> List[str]: + return self._keywords + + @property + def symbolic(self) -> bool: + return self._symbolic + + @property + def hasdynamics(self): + """ + Robot has dynamic parameters + + Returns + ------- + hasdynamics + Robot has dynamic parameters + + At least one link has associated dynamic parameters. + + Examples + -------- + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.hasdynamics: + + """ + + return self._hasdynamics + + @property + def hasgeometry(self): + """ + Robot has geometry model + + At least one link has associated mesh to describe its shape. + + Returns + ------- + hasgeometry + Robot has geometry model + + Examples + -------- + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.hasgeometry + + See Also + -------- + :func:`hascollision` + + """ + + return self._hasgeometry + + @property + def hascollision(self): + """ + Robot has collision model + + Returns + ------- + hascollision + Robot has collision model + + At least one link has associated collision model. + + Examples + -------- + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.hascollision + + See Also + -------- + :func:`hasgeometry` + + """ + + return self._hascollision + + @property + def default_backend(self): + """ + Get default graphical backend + + - ``robot.default_backend`` Get the default graphical backend, used when + no explicit backend is passed to ``Robot.plot``. + - ``robot.default_backend = ...`` Set the default graphical backend, used when + no explicit backend is passed to ``Robot.plot``. The default set here will + be overridden if the particular ``Robot`` subclass cannot support it. + + Returns + ------- + default_backend + backend name + + + """ + return self._default_backend + + @default_backend.setter + def default_backend(self, be): + self._default_backend = be + + @property + def gravity(self) -> NDArray: + """ + Get/set default gravitational acceleration (Robot superclass) + + - ``robot.name`` is the default gravitational acceleration + - ``robot.name = ...`` checks and sets default gravitational + acceleration + + + Parameters + ---------- + gravity + the new gravitational acceleration for this robot + + Returns + ------- + gravity + gravitational acceleration + + Notes + ----- + If the z-axis is upward, out of the Earth, this should be + a positive number. + + """ + + return self._gravity + + @gravity.setter + def gravity(self, gravity_new: ArrayLike): + self._gravity = np.array(getvector(gravity_new, 3)) + self.dynchanged() + + # --------------------------------------------------------------------- # + + @property + def q(self) -> NDArray: + """ + Get/set robot joint configuration + + - ``robot.q`` is the robot joint configuration + - ``robot.q = ...`` checks and sets the joint configuration + + Parameters + ---------- + q + the new robot joint configuration + + Returns + ------- + q + robot joint configuration + + """ + + return self._q + + @q.setter + def q(self, q_new: ArrayLike): + self._q = np.array(getvector(q_new, self.n)) + + @property + def qd(self) -> NDArray: + """ + Get/set robot joint velocity + + - ``robot.qd`` is the robot joint velocity + - ``robot.qd = ...`` checks and sets the joint velocity + + Returns + ------- + qd + robot joint velocity + + """ + + return self._qd + + @qd.setter + def qd(self, qd_new: ArrayLike): + self._qd = np.array(getvector(qd_new, self.n)) + + @property + def qdd(self) -> NDArray: + """ + Get/set robot joint acceleration + + - ``robot.qdd`` is the robot joint acceleration + - ``robot.qdd = ...`` checks and sets the robot joint acceleration + + Returns + ------- + qdd + robot joint acceleration + + + """ + return self._qdd + + @qdd.setter + def qdd(self, qdd_new: ArrayLike): + self._qdd = np.array(getvector(qdd_new, self.n)) + + @property + def qlim(self) -> NDArray: + r""" + Joint limits + + Limits are extracted from the link objects. If joints limits are + not set for: + + - a revolute joint [-𝜋. 𝜋] is returned + - a prismatic joint an exception is raised + + Attributes + ---------- + qlim + An array of joints limits (2, n) + + Raises + ------ + ValueError + unset limits for a prismatic joint + + Returns + ------- + qlim + Array of joint limit values + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.qlim + + """ + + limits = np.zeros((2, self.n)) + j = 0 + + for link in self.links: + if link.isrevolute: + if ( + link.qlim is None + or link.qlim[0] is None + or np.any(np.isnan(link.qlim)) + ): + v = [-np.pi, np.pi] + else: + v = link.qlim + elif link.isprismatic: + if link.qlim is None: + raise ValueError("Undefined prismatic joint limit") + else: + v = link.qlim + else: + # Fixed link + continue # pragma nocover + + limits[:, j] = v + j += 1 + + return limits + + @qlim.setter + def qlim(self, new_qlim: ArrayLike): + new_qlim = np.array(new_qlim) + + if new_qlim.shape != (2, self.n): + raise ValueError("new_qlim must be of shape (2, n)") + + j = 0 + for link in self.links: + if link.isjoint: + link.qlim = new_qlim[:, j] + j += 1 + + @property + def structure(self) -> str: + """ + Return the joint structure string + + A string with one letter per joint: ``R`` for a revolute + joint, and ``P`` for a prismatic joint. + + Returns + ------- + structure + joint configuration string + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.structure + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.structure + + Notes + ----- + Fixed joints, that maintain a constant link relative pose, + are not included. + ``len(self.structure) == self.n``. + + """ + + structure = [] + + for link in self.links: + if link.isrevolute: + structure.append("R") + elif link.isprismatic: + structure.append("P") + + return "".join(structure) + + @property + def prismaticjoints(self) -> List[bool]: + """ + Revolute joints as bool array + + Returns + ------- + prismaticjoints + array of joint type, True if prismatic + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.prismaticjoints() + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.prismaticjoints() + + Notes + ----- + Fixed joints, that maintain a constant link relative pose, + are not included. + + See Also + -------- + :func:`Link.isprismatic` + :func:`revolutejoints` + + """ + + return [link.isprismatic for link in self.links if link.isjoint] + + @property + def revolutejoints(self) -> List[bool]: + """ + Revolute joints as bool array + + Returns + ------- + revolutejoints + array of joint type, True if revolute + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.revolutejoints() + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.revolutejoints() + + Notes + ----- + Fixed joints, that maintain a constant link relative pose, + are not included. + + See Also + -------- + :func:`Link.isrevolute` + :func:`prismaticjoints` + + """ + + return [link.isrevolute for link in self.links if link.isjoint] + + @property + def control_mode(self) -> str: + """ + Get/set robot control mode + + - ``robot.control_type`` is the robot control mode + - ``robot.control_type = ...`` checks and sets the robot control mode + + Parameters + ---------- + control_mode + the new robot control mode + + Returns + ------- + control_mode + the current robot control mode + + """ + + return self._control_mode + + @control_mode.setter + def control_mode(self, cn: str): + if cn == "p" or cn == "v" or cn == "a": + self._control_mode = cn + else: + raise ValueError("Control type must be one of 'p', 'v', or 'a'") + + # --------------------------------------------------------------------- # + + @property + def urdf_string(self) -> str: + return self._urdf_string + + @property + def urdf_filepath(self) -> str: + return self._urdf_filepath + + # --------------------------------------------------------------------- # + + @property + def tool(self) -> SE3: + """ + Get/set robot tool transform + + - ``robot.tool`` is the robot tool transform as an SE3 object + - ``robot._tool`` is the robot tool transform as a numpy array + - ``robot.tool = ...`` checks and sets the robot tool transform + + Parameters + ---------- + tool + the new robot tool transform (as an SE(3)) + + Returns + ------- + tool + robot tool transform + + + + """ + return SE3(self._tool, check=False) + + @tool.setter + def tool(self, T: Union[SE3, NDArray]): + if isinstance(T, SE3): + self._tool = T.A + else: + self._tool = T + + @property + def base(self) -> SE3: + """ + Get/set robot base transform + + - ``robot.base`` is the robot base transform + - ``robot.base = ...`` checks and sets the robot base transform + + Parameters + ---------- + base + the new robot base transform + + Returns + ------- + base + the current robot base transform + + """ + + # return a copy, otherwise somebody with + # reference to the base can change it + + # This now returns the Scene Node transform + # self._T is a copy of SceneNode.__T + return SE3(self._T, check=False) + + @base.setter + def base(self, T: Union[NDArray, SE3]): + if isinstance(self, rtb.Robot): + # All 3D robots + # Set the SceneNode T + if isinstance(T, SE3): + self._T = T.A + else: + self._T = T + + # --------------------------------------------------------------------- # + + @lru_cache(maxsize=32) + def get_path( + self, + end: Union[Gripper, LinkType, str, None] = None, + start: Union[Gripper, LinkType, str, None] = None, + ) -> Tuple[List[LinkType], int, SE3]: + """ + Find a path from start to end + + Parameters + ---------- + end + end-effector or gripper to compute forward kinematics to + start + name or reference to a base link, defaults to None + + Raises + ------ + ValueError + link not known or ambiguous + + Returns + ------- + path + the path from start to end + n + the number of joints in the path + T + the tool transform present after end + + """ + + def search( + start, + end, + explored: Set[Union[LinkType, Link]], + path: List[Union[LinkType, Link]], + ) -> Union[List[Union[LinkType, Link]], None]: + link = self._getlink(start, self.base_link) + end = self._getlink(end, self.ee_links[0]) + + toplevel = len(path) == 0 + explored.add(link) + + if link == end: + return path + + # unlike regular DFS, the neighbours of the node are its children + # and its parent. + + # visit child nodes below start + if toplevel: + path = [link] + + if link.children is not None: + for child in link.children: + if child not in explored: + path.append(child) + p = search(child, end, explored, path) + if p is not None: + return p + + # We didn't find the node below, keep going up a level, and recursing + # down again + if toplevel: + path = [] + + if link.parent is not None: + parent = link.parent # go up one level toward the root + if parent not in explored: + if len(path) == 0: + p = search(parent, end, explored, [link]) + else: + path.append(link) + p = search(parent, end, explored, path) + + if p is not None and len(p) > 0: + return p + + end, start, tool = self._get_limit_links(end=end, start=start) + + path = search(start, end, set(), []) + + if path is None or len(path) == 0: + raise ValueError("No path found") # pragma nocover + elif path[-1] != end: + path.append(end) + + if tool is None: + tool = SE3() + + return path, len(path), tool # type: ignore + + @lru_cache(maxsize=32) + def _getlink( + self, + link: Union[LinkType, Gripper, str, None], + default: Union[LinkType, Gripper, str, None] = None, + ) -> Union[LinkType, Link]: + """ + Validate reference to Link + + ``robot._getlink(link)`` is a validated reference to a Link within + the ERobot ``robot``. If ``link`` is: + + - an ``Link`` reference it is validated as belonging to + ``robot``. + - a string, then it looked up in the robot's link name dictionary, and + a Link reference returned. + + Parameters + ---------- + link + link + + Raises + ------ + ValueError + link does not belong to this ERobot + TypeError + bad argument + + Returns + ------- + link + link reference + + """ + + if link is None: + link = default + + if isinstance(link, str): + if link in self.link_dict: + return self.link_dict[link] + + raise ValueError(f"no link named {link}") + + elif isinstance(link, BaseLink): + if link in self.links: + return link + else: + for gripper in self.grippers: + if link in gripper.links: + return link + + raise ValueError("link not in robot links") + elif isinstance(link, Gripper): + for gripper in self.grippers: + if link is gripper: + return gripper.links[0] + + raise ValueError("Gripper not in robot") + else: + raise TypeError("unknown argument") + + def _find_ets(self, start, end, explored, path) -> Union[ETS, None]: + """ + Privade method which will recursively find the ETS of a path + see ets() + """ + + link = self._getlink(start, self.base_link) + end = self._getlink(end, self.ee_links[0]) + + toplevel = path is None + explored.add(link) + + if link == end: + return path + + # unlike regular DFS, the neighbours of the node are its children + # and its parent. + + # visit child nodes below start + if toplevel: + path = link.ets + + if link.children is not None: + for child in link.children: + if child not in explored: + p = self._find_ets(child, end, explored, path * child.ets) + if p is not None: + return p + + # we didn't find the node below, keep going up a level, and recursing + # down again + if toplevel: + path = None + if link.parent is not None: + parent = link.parent # go up one level toward the root + if parent not in explored: + if path is None: + p = self._find_ets(parent, end, explored, link.ets.inv()) + else: + p = self._find_ets(parent, end, explored, path * link.ets.inv()) + if p is not None: + return p + + def _gripper_ets(self, gripper: Gripper) -> ETS: + """ + Privade method which will find the ETS of a gripper + + """ + + return ETS(ET.SE3(gripper.tool)) + + @lru_cache(maxsize=32) + def _get_limit_links( + self, + end: Union[Gripper, LinkType, str, None] = None, + start: Union[Gripper, LinkType, str, None] = None, + ) -> Tuple[LinkType, LinkType, Union[None, SE3]]: + """ + Get and validate an end-effector, and a base link + + Helper method to find or validate an end-effector and base link + + end + end-effector or gripper to compute forward kinematics to + start + name or reference to a base link, defaults to None + + ValueError + link not known or ambiguous + ValueError + [description] + TypeError + unknown type provided + + Returns + ------- + end + end-effector link + start + base link + tool + tool transform of gripper if applicable + + """ + + tool = None + if end is None: + if len(self.grippers) > 1: + end_ret = self.grippers[0].links[0] + tool = self.grippers[0].tool + if len(self.grippers) > 1: + # Warn user: more than one gripper + print("More than one gripper present, using robot.grippers[0]") + elif len(self.grippers) == 1: + end_ret = self.grippers[0].links[0] + tool = self.grippers[0].tool + + # no grippers, use ee link if just one + elif len(self.ee_links) > 1: + end_ret = self.ee_links[0] + if len(self.ee_links) > 1: + # Warn user: more than one EE + print("More than one end-effector present, using robot.ee_links[0]") + else: + end_ret = self.ee_links[0] + + else: + # Check if end corresponds to gripper + for gripper in self.grippers: + if end == gripper or end == gripper.name: + tool = gripper.tool + # end_ret = gripper.links[0] + + # otherwise check for end in the links + end_ret = self._getlink(end) + + if start is None: + start_ret = self.base_link + + # Cache result + self._cache_start = start + else: + # start effector is specified + start_ret = self._getlink(start) + + # because Gripper returns Link not LinkType + return end_ret, start_ret, tool # type: ignore + + @lru_cache(maxsize=32) + def ets( + self, + start: Union[LinkType, Gripper, str, None] = None, + end: Union[LinkType, Gripper, str, None] = None, + ) -> ETS: + """ + Robot to ETS + + ``robot.ets()`` is an ETS representing the kinematics from base to + end-effector. + + ``robot.ets(end=link)`` is an ETS representing the kinematics from + base to the link ``link`` specified as a Link reference or a name. + + ``robot.ets(start=l1, end=l2)`` is an ETS representing the kinematics + from link ``l1`` to link ``l2``. + + Parameters + ---------- + :param start: start of path, defaults to ``base_link`` + :param end: end of path, defaults to end-effector + + Raises + ------ + ValueError + a link does not belong to this ERobot + TypeError + a bad link argument + + Returns + ------- + ets + elementary transform sequence + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.ETS.Panda() + >>> panda.ets() + + """ + + # ets to stand and end incase of grippers + ets_init = None + ets_end = None + + if isinstance(start, Gripper): + ets_init = self._gripper_ets(start).inv() + link = start.links[0].parent + if link is None: # pragma nocover + raise ValueError("Invalid robot link configuration") + else: + link = self._getlink(start, self.base_link) + + if end is None: + if len(self.grippers) > 1: + end_link = self.grippers[0].links[0] + ets_end = self._gripper_ets(self.grippers[0]) + print("multiple grippers present, ambiguous, using self.grippers[0]") + elif len(self.grippers) == 1: + end_link = self.grippers[0].links[0] + ets_end = self._gripper_ets(self.grippers[0]) + elif len(self.ee_links) > 1: + end_link = self._getlink(end, self.ee_links[0]) + print( + "multiple end-effectors present, ambiguous, using self.ee_links[0]" + ) + else: + end_link = self._getlink(end, self.ee_links[0]) + else: + if isinstance(end, Gripper): + ets_end = self._gripper_ets(end) + end_link = end.links[0].parent # type: ignore + if end_link is None: # pragma nocover + raise ValueError("Invalid robot link configuration") + else: + end_link = self._getlink(end, self.ee_links[0]) + + explored = set() + + if link is end_link: + ets = link.ets + else: + ets = self._find_ets(link, end_link, explored, path=None) + + if ets is None: + raise ValueError( + "Could not find the requested ETS in this robot" + ) # pragma nocover + + if ets_init is not None: + ets = ets_init * ets + + if ets_end is not None: + ets = ets * ets_end + + return ets + + def copy(self): + return deepcopy(self) + + def __deepcopy__(self, memo): + links = [] + + # if isinstance(self, rtb.DHRobot): + # cls = rtb.DHRobot + if isinstance(self, rtb.Robot2): + cls = rtb.Robot2 + else: + cls = rtb.Robot + + for link in self.links: + links.append(deepcopy(link)) + + name = deepcopy(self.name) + manufacturer = deepcopy(self.manufacturer) + comment = deepcopy(self.comment) + base = deepcopy(self.base) + tool = deepcopy(self.tool) + gravity = deepcopy(self.gravity) + keywords = deepcopy(self.keywords) + symbolic = deepcopy(self.symbolic) + configs = deepcopy(self.configs) + + result = cls( + links, + name=name, + manufacturer=manufacturer, + comment=comment, + base=base, # type: ignore + tool=tool, + gravity=gravity, + keywords=keywords, + symbolic=symbolic, + configs=configs, + ) + + # if a configuration was an attribute of original robot, make it an + # attribute of the deep copy + for config in configs: + if hasattr(self, config): + setattr(result, config, configs[config]) + + memo[id(self)] = result + return result + + # --------------------------------------------------------------------- # + + def todegrees(self, q) -> NDArray: + """ + Convert joint angles to degrees + + Parameters + ---------- + q + The joint configuration of the robot + + Returns + ------- + q + a vector of joint coordinates in degrees and metres + + ``robot.todegrees(q)`` converts joint coordinates ``q`` to degrees + taking into account whether elements of ``q`` correspond to revolute + or prismatic joints, ie. prismatic joint values are not converted. + + If ``q`` is a matrix, with one column per joint, the conversion is + performed columnwise. + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> from math import pi + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.todegrees([pi/4, pi/8, 2, -pi/4, pi/6, pi/3]) + + """ + + q = getmatrix(q, (None, self.n)) + + for j, revolute in enumerate(self.revolutejoints): + if revolute: + q[:, j] *= 180.0 / np.pi + + if q.shape[0] == 1: + return q[0] + else: + return q + + def toradians(self, q) -> NDArray: + """ + Convert joint angles to radians + + ``robot.toradians(q)`` converts joint coordinates ``q`` to radians + taking into account whether elements of ``q`` correspond to revolute + or prismatic joints, ie. prismatic joint values are not converted. + + If ``q`` is a matrix, with one column per joint, the conversion is + performed columnwise. + + Parameters + ---------- + q + The joint configuration of the robot + + Returns + ------- + q + a vector of joint coordinates in radians and metres + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.toradians([10, 20, 2, 30, 40, 50]) + + """ + + q = getmatrix(q, (None, self.n)) + + for j, revolute in enumerate(self.revolutejoints): + if revolute: + q[:, j] *= np.pi / 180.0 + + if q.shape[0] == 1: + return q[0] + else: + return q + + def isrevolute(self, j) -> bool: + """ + Check if joint is revolute + + Returns + ------- + j + True if revolute + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.revolutejoints() + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.isrevolute(1) + + See Also + -------- + :func:`Link.isrevolute` + :func:`revolutejoints` + + """ + return self.revolutejoints[j] + + def isprismatic(self, j) -> bool: + """ + Check if joint is prismatic + + Returns + ------- + j + True if prismatic + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.prismaticjoints() + >>> stanford = rtb.models.DH.Stanford() + >>> stanford.isprismatic(1) + + See Also + -------- + :func:`Link.isprismatic` + :func:`prismaticjoints` + + """ + + return self.prismaticjoints[j] + + # --------------------------------------------------------------------- # + + def dfs_links( + self, + start: LinkType, + func: Union[None, Callable[[LinkType], Any]] = None, + ) -> List[LinkType]: + """ + A link search method + + Visit all links from start in depth-first order and will apply + func to each visited link + + Parameters + ---------- + start + The link to start at + func + An optional function to apply to each link as it is found + + Returns + ------- + links + A list of links + + """ + + visited = [] + + def vis_children(link): + visited.append(link) + if func is not None: + func(link) + + for li in link.children: + if li not in visited: + vis_children(li) + + vis_children(start) + + return visited + + def addconfiguration_attr(self, name: str, q: ArrayLike, unit: str = "rad"): + """ + Add a named joint configuration as an attribute + + Parameters + ---------- + name + Name of the joint configuration + q + Joint configuration + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.addconfiguration_attr("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) + >>> robot.mypos + >>> robot.configs["mypos"] + + Notes + ----- + - Used in robot model init method to store the ``qr`` configuration + - Dynamically adding attributes to objects can cause issues with + Python type checking. + - Configuration is also added to the robot instance's dictionary of + named configurations. + + See Also + -------- + :meth:`addconfiguration` + + """ + + v = getunit(q, unit, dim=self.n) + self._configs[name] = v + setattr(self, name, v) + + def addconfiguration(self, name: str, q: NDArray): + """ + Add a named joint configuration + + Add a named configuration to the robot instance's dictionary of named + configurations. + + Parameters + ---------- + name + Name of the joint configuration + q + Joint configuration + + + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.addconfiguration_attr("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) + >>> robot.configs["mypos"] + + See Also + -------- + :meth:`addconfiguration` + + """ + + self._configs[name] = q + + def configurations_str(self, border="thin"): + deg = 180 / np.pi + + # TODO: factor this out of DHRobot + def angle(theta, fmt=None): + if fmt is not None: + try: + return fmt.format(theta * deg) + "\u00b0" + except TypeError: # pragma nocover + pass + + return str(theta * deg) + "\u00b0" # pragma nocover + + # show named configurations + if len(self._configs) > 0: + table = ANSITable( + Column("name", colalign=">"), + *[ + Column(f"q{j:d}", colalign="<", headalign="<") + for j in range(self.n) + ], + border=border, + ) + + for name, q in self._configs.items(): + qlist = [] + for j, c in enumerate(self.structure): + if c == "P": + qlist.append(f"{q[j]: .3g}") + else: + qlist.append(angle(q[j], "{: .3g}")) + table.row(name, *qlist) + + return "\n" + str(table) + else: # pragma nocover + return "" + + def random_q(self): + """ + Return a random joint configuration + + The value for each joint is uniform randomly distributed between the + limits set for the robot. + + Note + ---- + The joint limit for all joints must be set. + + Returns + ------- + q + Random joint configuration :rtype: ndarray(n) + + See Also + -------- + :func:`Robot.qlim` + :func:`Link.qlim` + + """ + + qlim = self.qlim + if np.any(np.isnan(qlim)): + raise ValueError("some joint limits not defined") # pragma nocover + return np.random.uniform(low=qlim[0, :], high=qlim[1, :], size=(self.n,)) + + def linkcolormap(self, linkcolors: Union[List[Any], str] = "viridis"): + """ + Create a colormap for robot joints + + - ``cm = robot.linkcolormap()`` is an n-element colormap that gives a + unique color for every link. The RGBA colors for link ``j`` are + ``cm(j)``. + - ``cm = robot.linkcolormap(cmap)`` as above but ``cmap`` is the name + of a valid matplotlib colormap. The default, example above, is the + ``viridis`` colormap. + - ``cm = robot.linkcolormap(list of colors)`` as above but a + colormap is created from a list of n color names given as strings, + tuples or hexstrings. + + Parameters + ---------- + linkcolors + list of colors or colormap, defaults to "viridis" + + Returns + ------- + color map + the color map + + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> cm = robot.linkcolormap("inferno") + >>> print(cm(range(6))) # cm(i) is 3rd color in colormap + >>> cm = robot.linkcolormap( + >>> ['red', 'g', (0,0.5,0), '#0f8040', 'yellow', 'cyan']) + >>> print(cm(range(6))) + + Notes + ----- + - Colormaps have 4-elements: red, green, blue, alpha (RGBA) + - Names of supported colors and colormaps are defined in the + matplotlib documentation. + - `Specifying colors + `_ + - `Colormaps + `_ + + """ # noqa + + if isinstance(linkcolors, list) and len(linkcolors) == self.n: # pragma nocover + # provided a list of color names + return colors.ListedColormap(linkcolors) # type: ignore + else: # pragma nocover + # assume it is a colormap name + return cm.get_cmap(linkcolors, 6) # type: ignore + + def hierarchy(self) -> None: + """ + Pretty print the robot link hierachy + + Returns + ------- + Pretty print of the robot model + + Examples + -------- + Makes a robot and prints the heirachy + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.URDF.Panda() + >>> robot.hierarchy() + + """ + + def recurse(link, indent=0): + print(" " * indent * 2, link.name) + if link.children is not None: + for child in link.children: + recurse(child, indent + 1) + + recurse(self.base_link) + + def segments(self) -> List[List[Union[LinkType, None]]]: + """ + Segments of branched robot + + For a single-chain robot with structure:: + + L1 - L2 - L3 + + the return is ``[[None, L1, L2, L3]]`` + + For a robot with structure:: + + L1 - L2 +- L3 - L4 + +- L5 - L6 + + the return is ``[[None, L1, L2], [L2, L3, L4], [L2, L5, L6]]`` + + Returns + ------- + segments + Segment list + + Notes + ----- + - the length of the list is the number of segments in the robot + - the first segment always starts with ``None`` which represents + the base transform (since there is no base link) + - the last link of one segment is also the first link of subsequent + segments + """ + + def recurse(link: Link): + segs = [link.parent] + while True: + segs.append(link) + if link.nchildren == 0: + return [segs] + elif link.nchildren == 1: + link = link.children[0] # type: ignore + continue + elif link.nchildren > 1: + segs = [segs] + + for child in link.children: # type: ignore + segs.extend(recurse(child)) + + return segs + + return recurse(self.links[0]) # type: ignore + + # --------------------------------------------------------------------- # + # Scene Graph section + # --------------------------------------------------------------------- # + + def _update_link_tf(self, q: Union[ArrayLike, None] = None): + """ + This private method updates the local transform of each link within + this robot according to q (or self.q if q is none) + """ + + @lru_cache(maxsize=2) + def get_link_ets(): + return [link.ets._fknm for link in self.links] + + @lru_cache(maxsize=2) + def get_link_scene_node(): + return [link._T_reference for link in self.links] + + Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) + + [gripper._update_link_tf() for gripper in self.grippers] + + # --------------------------------------------------------------------- # + # --------- PyPlot Methods -------------------------------------------- # + # --------------------------------------------------------------------- # + + def _get_graphical_backend( + self, backend: Union[L["swift", "pyplot", "pyplot2"], None] = None + ) -> Union[Swift, PyPlot, PyPlot2]: + default = self.default_backend + + # figure out the right default + if backend is None: + if isinstance(self, rtb.DHRobot): + default = "pyplot" + elif isinstance(self, rtb.Robot2): + default = "pyplot2" + elif isinstance(self, rtb.Robot): + if self.hasgeometry: + default = "swift" + else: + default = "pyplot" + + if backend is not None: + using_backend = backend.lower() + else: + using_backend = backend + + # Find the right backend, modules are imported here on an as needs + # basis + if using_backend == "swift" or default == "swift": # pragma nocover + # swift was requested, is it installed? + if isinstance(self, rtb.DHRobot): + raise NotImplementedError( + "Plotting in Swift is not implemented for DHRobots yet" + ) + try: + # yes, use it + from roboticstoolbox.backends.swift import Swift + + env = Swift() + return env + except ModuleNotFoundError: + if using_backend == "swift": + print("Swift is not installed, install it using pip or conda") + using_backend = "pyplot" + + if using_backend is None: + using_backend = default + + if using_backend == "pyplot": + from roboticstoolbox.backends.PyPlot import PyPlot + + env = PyPlot() + + elif using_backend == "pyplot2": + from roboticstoolbox.backends.PyPlot import PyPlot2 + + env = PyPlot2() + + else: + raise ValueError("unknown backend", backend) # pragma nocover + + return env + + def plot( + self, + q: ArrayLike, + backend: Union[L["swift", "pyplot", "pyplot2"], None] = None, + block: bool = False, + dt: float = 0.050, + limits: Union[ArrayLike, None] = None, + vellipse: bool = False, + fellipse: bool = False, + fig: Union[str, None] = None, + movie: Union[str, None] = None, + loop: bool = False, + **kwargs, + ) -> Union[Swift, PyPlot, PyPlot2]: + """ + Graphical display and animation + + ``robot.plot(q, 'pyplot')`` displays a graphical view of a robot + based on the kinematic model and the joint configuration ``q``. + This is a stick figure polyline which joins the origins of the + link coordinate frames. The plot will autoscale with an aspect + ratio of 1. + + If ``q`` (m,n) representing a joint-space trajectory it will create an + animation with a pause of ``dt`` seconds between each frame. + + Attributes + ---------- + q + The joint configuration of the robot. + backend + The graphical backend to use, currently 'swift' + and 'pyplot' are implemented. Defaults to 'swift' of a ``Robot`` + and 'pyplot` for a ``DHRobot`` + block + Block operation of the code and keep the figure open + dt + if q is a trajectory, this describes the delay in + seconds between frames + limits + Custom view limits for the plot. If not supplied will + autoscale, [x1, x2, y1, y2, z1, z2] + (this option is for 'pyplot' only) + vellipse + (Plot Option) Plot the velocity ellipse at the + end-effector (this option is for 'pyplot' only) + fellipse + (Plot Option) Plot the force ellipse at the + end-effector (this option is for 'pyplot' only) + fig + (Plot Option) The figure label to plot in (this option is for + 'pyplot' only) + movie + (Plot Option) The filename to save the movie to (this option is for + 'pyplot' only) + loop + (Plot Option) Loop the movie (this option is for + 'pyplot' only) + jointaxes + (Plot Option) Plot an arrow indicating the axes in + which the joint revolves around(revolute joint) or translates + along (prosmatic joint) (this option is for 'pyplot' only) + eeframe + (Plot Option) Plot the end-effector coordinate frame + at the location of the end-effector. Uses three arrows, red, + green and blue to indicate the x, y, and z-axes. + (this option is for 'pyplot' only) + shadow + (Plot Option) Plot a shadow of the robot in the x-y + plane. (this option is for 'pyplot' only) + name + (Plot Option) Plot the name of the robot near its base + (this option is for 'pyplot' only) + + Returns + ------- + env + A reference to the environment object which controls the + figure + + Notes + ----- + - By default this method will block until the figure is dismissed. + To avoid this set ``block=False``. + - For PyPlot, the polyline joins the origins of the link frames, + but for some Denavit-Hartenberg models those frames may not + actually be on the robot, ie. the lines to not neccessarily + represent the links of the robot. + + See Also + -------- + :func:`teach` + + """ # noqa + + env = None + + env = self._get_graphical_backend(backend) + + q = np.array(getmatrix(q, (None, self.n))) + self.q = q[0, :] + + # Add the self to the figure in readonly mode + if q.shape[0] == 1: + env.launch(name=self.name + " Plot", limits=limits, fig=fig) + else: + env.launch(name=self.name + " Trajectory Plot", limits=limits, fig=fig) + + env.add(self, readonly=True, **kwargs) + + if vellipse: + vell = self.vellipse(q[0], centre="ee") + env.add(vell) + else: + vell = None + + if fellipse: + fell = self.fellipse(q[0], centre="ee") + env.add(fell) + else: + fell = None + + # List of images saved from each plot + images = [] + + if movie is not None: # pragma: nocover + loop = False + + while True: + for qk in q: + self.q = qk + if vell is not None: + vell.q = qk + if fell is not None: + fell.q = qk + env.step(dt) + + if movie is not None and isinstance(env, PyPlot): # pragma nocover + images.append(env.getframe()) + + if movie is not None: # pragma nocover + # save it as an animated GIF + images[0].save( + movie, + save_all=True, + append_images=images[1:], + optimize=False, + duration=dt, + loop=0, + ) + if not loop: + break + + # Keep the plot open + if block: # pragma nocover + env.hold() + + return env + + def fellipse( + self, + q: ArrayLike, + opt: L["trans", "rot"] = "trans", + unit: L["rad", "deg"] = "rad", + centre: Union[L["ee"], ArrayLike] = [0, 0, 0], + ) -> EllipsePlot: + """ + Create a force ellipsoid object for plotting with PyPlot + + ``robot.fellipse(q)`` creates a force ellipsoid for the robot at + pose ``q``. The ellipsoid is centered at the origin. + + Attributes + ---------- + q + The joint configuration of the robot. + opt + 'trans' or 'rot' will plot either the translational or + rotational force ellipsoid + unit + 'rad' or 'deg' will plot the ellipsoid in radians or + degrees + centre + The centre of the ellipsoid, either 'ee' for the end-effector + or a 3-vector [x, y, z] in the world frame + + Returns + ------- + env + An EllipsePlot object + + Notes + ----- + - By default the ellipsoid related to translational motion is + drawn. Use ``opt='rot'`` to draw the rotational velocity + ellipsoid. + - By default the ellipsoid is drawn at the origin. The option + ``centre`` allows its origin to set to set to the specified + 3-vector, or the string "ee" ensures it is drawn at the + end-effector position. + + """ + + if isinstance(self, rtb.ERobot): # pragma nocover + raise NotImplementedError("ERobot fellipse not implemented yet") + + q = getunit(q, unit) + ell = EllipsePlot(self, q, "f", opt, centre=centre) + return ell + + def vellipse( + self, + q: ArrayLike, + opt: L["trans", "rot"] = "trans", + unit: L["rad", "deg"] = "rad", + centre: Union[L["ee"], ArrayLike] = [0, 0, 0], + scale: float = 0.1, + ) -> EllipsePlot: + """ + Create a velocity ellipsoid object for plotting with PyPlot + + ``robot.vellipse(q)`` creates a force ellipsoid for the robot at + pose ``q``. The ellipsoid is centered at the origin. + + Attributes + ---------- + q + The joint configuration of the robot. + opt + 'trans' or 'rot' will plot either the translational or + rotational force ellipsoid + unit + 'rad' or 'deg' will plot the ellipsoid in radians or + degrees + centre + The centre of the ellipsoid, either 'ee' for the end-effector + or a 3-vector [x, y, z] in the world frame + scale + The scale factor for the ellipsoid + + Returns + ------- + env + An EllipsePlot object + + Notes + ----- + - By default the ellipsoid related to translational motion is + drawn. Use ``opt='rot'`` to draw the rotational velocity + ellipsoid. + - By default the ellipsoid is drawn at the origin. The option + ``centre`` allows its origin to set to set to the specified + 3-vector, or the string "ee" ensures it is drawn at the + end-effector position. + + """ + + if isinstance(self, rtb.ERobot): # pragma nocover + raise NotImplementedError("ERobot vellipse not implemented yet") + + q = getunit(q, unit) + ell = EllipsePlot(self, q, "v", opt, centre=centre, scale=scale) + return ell + + def plot_ellipse( + self, + ellipse: EllipsePlot, + block: bool = True, + limits: Union[ArrayLike, None] = None, + jointaxes: bool = True, + eeframe: bool = True, + shadow: bool = True, + name: bool = True, + ): + """ + Plot the an ellipsoid + + ``robot.plot_ellipse(ellipsoid)`` displays the ellipsoid. + + Attributes + ---------- + ellipse + the ellipsoid to plot + block + Block operation of the code and keep the figure open + limits + Custom view limits for the plot. If not supplied will + autoscale, [x1, x2, y1, y2, z1, z2] + jointaxes + (Plot Option) Plot an arrow indicating the axes in + which the joint revolves around(revolute joint) or translates + along (prosmatic joint) + eeframe + (Plot Option) Plot the end-effector coordinate frame + at the location of the end-effector. Uses three arrows, red, + green and blue to indicate the x, y, and z-axes. + shadow + (Plot Option) Plot a shadow of the robot in the x-y + plane + name + (Plot Option) Plot the name of the robot near its base + + Returns + ------- + env + A reference to the PyPlot object which controls the + matplotlib figure + + """ + + if not isinstance(ellipse, EllipsePlot): # pragma nocover + raise TypeError( + "ellipse must be of type roboticstoolbox.backend.PyPlot.EllipsePlot" + ) + + env = PyPlot() + + # Add the robot to the figure in readonly mode + env.launch(ellipse.robot.name + " " + ellipse.name, limits=limits) + + env.add(ellipse, jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, name=name) + + # Keep the plot open + if block: # pragma nocover + env.hold() + + return env + + def plot_fellipse( + self, + q: Union[ArrayLike, None], + block: bool = True, + fellipse: Union[EllipsePlot, None] = None, + limits: Union[ArrayLike, None] = None, + opt: L["trans", "rot"] = "trans", + centre: Union[L["ee"], ArrayLike] = [0, 0, 0], + jointaxes: bool = True, + eeframe: bool = True, + shadow: bool = True, + name: bool = True, + ) -> PyPlot: + """ + Plot the force ellipsoid for manipulator + + ``robot.plot_fellipse(q)`` displays the velocity ellipsoid for the + robot at pose ``q``. The plot will autoscale with an aspect ratio + of 1. + + ``robot.plot_fellipse(vellipse)`` specifies a custon ellipse to plot. + + Attributes + ---------- + q + The joint configuration of the robot + block + Block operation of the code and keep the figure open + fellipse + the vellocity ellipsoid to plot + limits + Custom view limits for the plot. If not supplied will + autoscale, [x1, x2, y1, y2, z1, z2] + opt + 'trans' or 'rot' will plot either the translational or + rotational force ellipsoid + centre + The coordinates to plot the fellipse [x, y, z] or "ee" + to plot at the end-effector location + jointaxes + (Plot Option) Plot an arrow indicating the axes in + which the joint revolves around(revolute joint) or translates + along (prosmatic joint) + eeframe + (Plot Option) Plot the end-effector coordinate frame + at the location of the end-effector. Uses three arrows, red, + green and blue to indicate the x, y, and z-axes. + shadow + (Plot Option) Plot a shadow of the robot in the x-y + plane + name + (Plot Option) Plot the name of the robot near its base + + Raises + ------ + ValueError + q or fellipse must be supplied + + Returns + ------- + env + A reference to the PyPlot object which controls the + matplotlib figure + + Notes + ----- + - By default the ellipsoid related to translational motion is + drawn. Use ``opt='rot'`` to draw the rotational velocity + ellipsoid. + - By default the ellipsoid is drawn at the origin. The option + ``centre`` allows its origin to set to set to the specified + 3-vector, or the string "ee" ensures it is drawn at the + end-effector position. + + """ + + if isinstance(self, rtb.ERobot): # pragma nocover + raise NotImplementedError( + "Ellipse Plotting of ERobot's not implemented yet" + ) + + if fellipse is None and q is not None: + fellipse = self.fellipse(q, opt=opt, centre=centre) + else: + raise ValueError("Must specify either q or fellipse") # pragma: nocover + + return self.plot_ellipse( + fellipse, + block=block, + limits=limits, + jointaxes=jointaxes, + eeframe=eeframe, + shadow=shadow, + name=name, + ) + + def plot_vellipse( + self, + q: Union[ArrayLike, None], + block: bool = True, + vellipse: Union[EllipsePlot, None] = None, + limits: Union[ArrayLike, None] = None, + opt: L["trans", "rot"] = "trans", + centre: Union[L["ee"], ArrayLike] = [0, 0, 0], + jointaxes: bool = True, + eeframe: bool = True, + shadow: bool = True, + name: bool = True, + ) -> PyPlot: + """ + Plot the velocity ellipsoid for manipulator + + ``robot.plot_vellipse(q)`` displays the velocity ellipsoid for the + robot at pose ``q``. The plot will autoscale with an aspect ratio + of 1. + + ``robot.plot_vellipse(vellipse)`` specifies a custon ellipse to plot. + + block + Block operation of the code and keep the figure open + q + The joint configuration of the robot + vellipse + the vellocity ellipsoid to plot + limits + Custom view limits for the plot. If not supplied will + autoscale, [x1, x2, y1, y2, z1, z2] + opt + 'trans' or 'rot' will plot either the translational or + rotational velocity ellipsoid + centre + The coordinates to plot the vellipse [x, y, z] or "ee" + to plot at the end-effector location + jointaxes + (Plot Option) Plot an arrow indicating the axes in + which the joint revolves around(revolute joint) or translates + along (prosmatic joint) + eeframe + (Plot Option) Plot the end-effector coordinate frame + at the location of the end-effector. Uses three arrows, red, + green and blue to indicate the x, y, and z-axes. + shadow + (Plot Option) Plot a shadow of the robot in the x-y + plane + name + (Plot Option) Plot the name of the robot near its base + + Raises + ------ + ValueError + q or fellipse must be supplied + + Returns + ------- + env + A reference to the PyPlot object which controls the + matplotlib figure + + Notes + ----- + - By default the ellipsoid related to translational motion is + drawn. Use ``opt='rot'`` to draw the rotational velocity + ellipsoid. + - By default the ellipsoid is drawn at the origin. The option + ``centre`` allows its origin to set to set to the specified + 3-vector, or the string "ee" ensures it is drawn at the + end-effector position. + + """ + + if isinstance(self, rtb.ERobot): # pragma nocover + raise NotImplementedError( + "Ellipse Plotting of ERobot's not implemented yet" + ) + + if vellipse is None and q is not None: + vellipse = self.vellipse(q=q, opt=opt, centre=centre) + else: + raise ValueError("Must specify either q or fellipse") # pragma: nocover + + return self.plot_ellipse( + vellipse, + block=block, + limits=limits, + jointaxes=jointaxes, + eeframe=eeframe, + shadow=shadow, + name=name, + ) + + def teach( + self, + q: Union[ArrayLike, None], + block: bool = True, + limits: Union[ArrayLike, None] = None, + vellipse: bool = False, + fellipse: bool = False, + backend: Union[L["pyplot", "pyplot2"], None] = None, + ) -> Union[PyPlot, PyPlot2]: + """ + Graphical teach pendant + + ``robot.teach(q)`` creates a matplotlib plot which allows the user to + "drive" a graphical robot using a graphical slider panel. The robot's + inital joint configuration is ``q``. The plot will autoscale with an + aspect ratio of 1. + + ``robot.teach()`` as above except the robot's stored value of ``q`` + is used. + + q + The joint configuration of the robot (Optional, + if not supplied will use the stored q values). + block + Block operation of the code and keep the figure open + limits + Custom view limits for the plot. If not supplied will + autoscale, [x1, x2, y1, y2, z1, z2] + vellipse + (Plot Option) Plot the velocity ellipse at the + end-effector (this option is for 'pyplot' only) + fellipse + (Plot Option) Plot the force ellipse at the + end-effector (this option is for 'pyplot' only) + + Returns + ------- + env + A reference to the PyPlot object which controls the + matplotlib figure + + Notes + ----- + - Program execution is blocked until the teach window is + dismissed. If ``block=False`` the method is non-blocking but + you need to poll the window manager to ensure that the window + remains responsive. + - The slider limits are derived from the joint limit properties. + If not set then: + - For revolute joints they are assumed to be [-pi, +pi] + - For prismatic joint they are assumed unknown and an error + occurs. + + """ + + if q is None: + q = np.zeros((self.n,)) + else: + q = getvector(q, self.n) + + # Make an empty 3D figure + env = self._get_graphical_backend(backend) + + if isinstance(env, Swift): # pragma: nocover + raise TypeError("teach() not supported for Swift backend") + + # Add the self to the figure in readonly mode + env.launch("Teach " + self.name, limits=limits) + env.add( + self, + readonly=True, + # jointaxes=jointaxes, + # jointlabels=jointlabels, + # eeframe=eeframe, + # shadow=shadow, + # name=name, + ) + + env._add_teach_panel(self, q) + + if vellipse: + vell = self.vellipse(q, centre="ee", scale=0.5) + env.add(vell) + + if fellipse: + fell = self.fellipse(q, centre="ee") + env.add(fell) + + # Keep the plot open + if block: # pragma nocover + env.hold() + + return env + + # --------------------------------------------------------------------- # + + # --------------------------------------------------------------------- # + # --------- Utility Methods ------------------------------------------- # + # --------------------------------------------------------------------- # + + def showgraph(self, display_graph: bool = True, **kwargs) -> Union[None, str]: + """ + Display a link transform graph in browser + + ``robot.showgraph()`` displays a graph of the robot's link frames + and the ETS between them. It uses GraphViz dot. + + The nodes are: + - Base is shown as a grey square. This is the world frame origin, + but can be changed using the ``base`` attribute of the robot. + - Link frames are indicated by circles + - ETS transforms are indicated by rounded boxes + + The edges are: + - an arrow if `jtype` is False or the joint is fixed + - an arrow with a round head if `jtype` is True and the joint is + revolute + - an arrow with a box head if `jtype` is True and the joint is + prismatic + + Edge labels or nodes in blue have a fixed transformation to the + preceding link. + + Parameters + ---------- + display_graph + Open the graph in a browser if True. Otherwise will return the + file path + etsbox + Put the link ETS in a box, otherwise an edge label + jtype + Arrowhead to node indicates revolute or prismatic type + static + Show static joints in blue and bold + + Examples + -------- + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.URDF.Panda() + >>> panda.showgraph() + + .. image:: ../figs/panda-graph.svg + :width: 600 + + See Also + -------- + :func:`dotfile` + + """ + + # Lazy import + import tempfile + import subprocess + import webbrowser + + # create the temporary dotfile + dotfile = tempfile.TemporaryFile(mode="w") + self.dotfile(dotfile, **kwargs) + + # rewind the dot file, create PDF file in the filesystem, run dot + dotfile.seek(0) + pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False) + subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile) + + # open the PDF file in browser (hopefully portable), then cleanup + if display_graph: # pragma nocover + webbrowser.open(f"file://{pdffile.name}") + else: + return pdffile.name + + def dotfile( + self, + filename: Union[str, IO[str]], + etsbox: bool = False, + ets: L["full", "brief"] = "full", + jtype: bool = False, + static: bool = True, + ): + """ + Write a link transform graph as a GraphViz dot file + + The file can be processed using dot: + % dot -Tpng -o out.png dotfile.dot + + The nodes are: + - Base is shown as a grey square. This is the world frame origin, + but can be changed using the ``base`` attribute of the robot. + - Link frames are indicated by circles + - ETS transforms are indicated by rounded boxes + + The edges are: + - an arrow if `jtype` is False or the joint is fixed + - an arrow with a round head if `jtype` is True and the joint is + revolute + - an arrow with a box head if `jtype` is True and the joint is + prismatic + + Edge labels or nodes in blue have a fixed transformation to the + preceding link. + + Note + ---- + If ``filename`` is a file object then the file will *not* + be closed after the GraphViz model is written. + + Parameters + ---------- + file + Name of file to write to + etsbox + Put the link ETS in a box, otherwise an edge label + ets + Display the full ets with "full" or a brief version with "brief" + jtype + Arrowhead to node indicates revolute or prismatic type + static + Show static joints in blue and bold + + See Also + -------- + :func:`showgraph` + + """ + + if isinstance(filename, str): + file = open(filename, "w") + else: + file = filename + + header = r"""digraph G { +graph [rankdir=LR]; +""" + + def draw_edge(link, etsbox, jtype, static): + # draw the edge + if jtype: + if link.isprismatic: + edge_options = 'arrowhead="box", arrowtail="inv", dir="both"' + elif link.isrevolute: + edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"' + else: + edge_options = 'arrowhead="normal"' + else: + edge_options = 'arrowhead="normal"' + + if link.parent is None: + parent = "BASE" + else: + parent = link.parent.name + + if etsbox: + # put the ets fragment in a box + if not link.isjoint and static: + node_options = ', fontcolor="blue"' + else: + node_options = "" + + try: + file.write( + ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format( + link.name, + link.ets.__str__(q=f"q{link.jindex}"), + node_options, + ) + ) + except UnicodeEncodeError: # pragma nocover + file.write( + ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format( + link.name, + link.ets.__str__(q=f"q{link.jindex}") + .encode("ascii", "ignore") + .decode("ascii"), + node_options, + ) + ) + + file.write(" {} -> {}_ets;\n".format(parent, link.name)) + file.write( + " {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options) + ) + else: + # put the ets fragment as an edge label + if not link.isjoint and static: + edge_options += 'fontcolor="blue"' + if ets == "full": + estr = link.ets.__str__(q=f"q{link.jindex}") + elif ets == "brief": + if link.jindex is None: + estr = "" + else: + estr = f"...q{link.jindex}" + else: + return + try: + file.write( + ' {} -> {} [label="{}", {}];\n'.format( + parent, + link.name, + estr, + edge_options, + ) + ) + except UnicodeEncodeError: # pragma nocover + file.write( + ' {} -> {} [label="{}", {}];\n'.format( + parent, + link.name, + estr.encode("ascii", "ignore").decode("ascii"), + edge_options, + ) + ) + + file.write(header) + + # add the base link + file.write(" BASE [shape=square, style=filled, fillcolor=gray]\n") + + # add the links + for link in self: + # draw the link frame node (circle) or ee node (doublecircle) + if link in self.ee_links: + # end-effector + node_options = 'shape="doublecircle", color="blue", fontcolor="blue"' + else: + node_options = 'shape="circle"' + + file.write(" {} [{}];\n".format(link.name, node_options)) + + draw_edge(link, etsbox, jtype, static) + + for gripper in self.grippers: + for link in gripper.links: + file.write(" {} [shape=cds];\n".format(link.name)) + draw_edge(link, etsbox, jtype, static) + + file.write("}\n") + + if isinstance(filename, str): + file.close() # noqa diff --git a/roboticstoolbox/robot/DHFactor.py b/roboticstoolbox/robot/DHFactor.py index b759530a4..6b838abc1 100644 --- a/roboticstoolbox/robot/DHFactor.py +++ b/roboticstoolbox/robot/DHFactor.py @@ -5,12 +5,11 @@ from spatialmath import base pi2 = base.pi() / 2 -deg = base.pi() / sympy.Integer('180') +deg = base.pi() / sympy.Integer("180") # PROGRESS # subs2z does a bad thing in first phase, 2 subs it shouldnt make class DHFactor(ET): - def __init__(self, axis=None, eta=None, **kwargs): super().__init__(axis=axis, eta=eta, **kwargs) @@ -25,7 +24,7 @@ def parse(cls, s): ets = DHFactor() for axis, eta in et_re.findall(s): - if eta[0] == 'q': + if eta[0] == "q": eta = None unit = None j = jointnum @@ -38,27 +37,27 @@ def parse(cls, s): except: # failing that, a symbolic variable eta = sympy.symbols(eta) - if axis[0] == 'R': + if axis[0] == "R": # convert to degrees, assumed unit in input string eta = eta * deg j = None - if axis == 'Rx': + if axis == "Rx": e = DHFactor.Rx(eta, j=j) - elif axis == 'Ry': + elif axis == "Ry": e = DHFactor.Ry(eta, j=j) - elif axis == 'Rz': + elif axis == "Rz": e = DHFactor.Rz(eta, j=j) - elif axis == 'Tx': + elif axis == "Tx": e = DHFactor.tx(eta, j=j) - elif axis == 'Ty': + elif axis == "Ty": e = DHFactor.ty(eta, j=j) # elif axis == 'Tz': else: e = DHFactor.tz(eta, j=j) ets *= e - + return cls(ets) # ---------------------------------------------------------------------- # @@ -72,8 +71,8 @@ def simplify(self, mdh=False): self.merge() # self.substitute_to_z() self.merge() - print('inital merge and swap\n ', self) - print('main loop') + print("inital merge and swap\n ", self) + print("main loop") for i in range(10): nchanges = 0 @@ -92,16 +91,16 @@ def simplify(self, mdh=False): break else: # too many iterations - print('too many iterations') + print("too many iterations") # ---------------------------------------------------------------------- # def float_right(self): """ Attempt to 'float' translational terms as far to the right as - * possible and across joint boundaries. + * possible and across joint boundaries. """ - + nchanges = 0 for i in range(len(self)): @@ -109,9 +108,9 @@ def float_right(self): this = self[i] if this.isjoint or this.isrevolute: continue - + crossed = False - for j in range(i+1, len(self)): + for j in range(i + 1, len(self)): next = self[j] if next.isprismatic: @@ -123,11 +122,12 @@ def float_right(self): if crossed: del self[i] - self.insert(j-1, this) + self.insert(j - 1, this) nchanges += 1 - print('floated') + print("floated") return nchanges + # ---------------------------------------------------------------------- # def swap(self, mdh=False): @@ -140,36 +140,57 @@ def swap(self, mdh=False): def do_swap(this, next): if mdh: # modified DH - return this.axis == 'Rx' and next.axis == 'tx' \ - or this.axis == 'Ry' and next.axis == 'ty' \ - or this.axis == 'Rz' and next.axis == 'tz' \ - or this.axis == 'tz' and next.axis == 'tx' + return ( + this.axis == "Rx" + and next.axis == "tx" + or this.axis == "Ry" + and next.axis == "ty" + or this.axis == "Rz" + and next.axis == "tz" + or this.axis == "tz" + and next.axis == "tx" + ) else: # standard DH # push constant translations through rotational joints - # of the same type + # of the same type - if this.axis == 'tz' and next.axis == 'tx': + if this.axis == "tz" and next.axis == "tx": return True if next.isjoint and ( - this.axis == 'tx' and next.axis == 'Rx' - or this.axis == 'ty' and next.axis == 'Ry' - or this.axis == 'tz' and next.axis == 'Rz'): + this.axis == "tx" + and next.axis == "Rx" + or this.axis == "ty" + and next.axis == "Ry" + or this.axis == "tz" + and next.axis == "Rz" + ): return True elif not this.isjoint and ( - this.axis == 'Rx' and next.axis == 'tx' - or this.axis == 'Ry' and next.axis == 'ty'): + this.axis == "Rx" + and next.axis == "tx" + or this.axis == "Ry" + and next.axis == "ty" + ): return True - if not this.isjoint and not next.isjoint and \ - this.axis == 'tz' and next.axis == 'Rz': + if ( + not this.isjoint + and not next.isjoint + and this.axis == "tz" + and next.axis == "Rz" + ): return True # move Ty terms to the right - if this.axis == 'ty' and next.axis == 'tz' \ - or this.axis == 'ty' and next.axis == 'tx': + if ( + this.axis == "ty" + and next.axis == "tz" + or this.axis == "ty" + and next.axis == "tx" + ): return True return False @@ -179,11 +200,11 @@ def do_swap(this, next): nchanges = 0 for i in range(len(self) - 1): this = self[i] - next = self[i+1] + next = self[i + 1] if do_swap(this, next): del self[i] - self.insert(i+1, this) + self.insert(i + 1, this) nchanges += 1 print(f"swapping {this} <--> {next}") if nchanges == 0: @@ -194,29 +215,20 @@ def do_swap(this, next): # ---------------------------------------------------------------------- # - def substitute_to_z(self): # substitute all non Z joint transforms according to rules nchanges = 0 out = DHFactor() def subs_z(this): - if this.axis == 'Rx': - return DHFactor.ry(pi2) \ - * DHFactor.rz(this.eta) \ - * DHFactor.ry(-pi2) - elif this.axis == 'Ry': - return DHFactor.rx(-pi2) \ - * DHFactor.rz(this.eta) \ - * DHFactor.rx(pi2) - elif this.axis == 'tx': - return DHFactor.ry(pi2) \ - * DHFactor.tz(this.eta) \ - * DHFactor.ry(-pi2) - elif this.axis == 'ty': - return DHFactor.rx(-pi2) \ - * DHFactor.tz(this.eta) \ - * DHFactor.rx(pi2) + if this.axis == "Rx": + return DHFactor.ry(pi2) * DHFactor.rz(this.eta) * DHFactor.ry(-pi2) + elif this.axis == "Ry": + return DHFactor.rx(-pi2) * DHFactor.rz(this.eta) * DHFactor.rx(pi2) + elif this.axis == "tx": + return DHFactor.ry(pi2) * DHFactor.tz(this.eta) * DHFactor.ry(-pi2) + elif this.axis == "ty": + return DHFactor.rx(-pi2) * DHFactor.tz(this.eta) * DHFactor.rx(pi2) for e in self: if not e.isjoint: @@ -241,30 +253,24 @@ def substitute_to_z2(self): jointyet = False def subs_z(prev, this): - if this.axis == 'Ry': - return DHFactor.rz(pi2) \ - * DHFactor.rx(this.eta) \ - * DHFactor.rz(-pi2) - elif this.axis == 'ty': - if prev.axis == 'Rz': - return DHFactor.rz(pi2) \ - * DHFactor.ty(this.eta) \ - * DHFactor.r(-pi2) + if this.axis == "Ry": + return DHFactor.rz(pi2) * DHFactor.rx(this.eta) * DHFactor.rz(-pi2) + elif this.axis == "ty": + if prev.axis == "Rz": + return DHFactor.rz(pi2) * DHFactor.ty(this.eta) * DHFactor.r(-pi2) else: - return DHFactor.rx(-pi2) \ - * DHFactor.ty(this.eta) \ - * DHFactor.rx(pi2) + return DHFactor.rx(-pi2) * DHFactor.ty(this.eta) * DHFactor.rx(pi2) for i in range(len(self)): this = self[i] if this.isjoint: jointyet = True continue - + if i == 0 or not jointyet: continue - prev = self[i-1] + prev = self[i - 1] new = subs_z(prev, this) @@ -277,10 +283,10 @@ def subs_z(prev, this): self.data = out.data return nchanges + # ---------------------------------------------------------------------- # def merge(self): - def can_merge(prev, this): return prev.axis == this.axis and not (prev.isjoint and this.isjoint) @@ -327,7 +333,7 @@ def add(this, that): else: out.eta += that.eta else: - raise ValueError('both ET cannot be joint variables') + raise ValueError("both ET cannot be joint variables") return out # ---------------------------------------------------------------------- # @@ -343,24 +349,24 @@ def subs_y(prev, this): return None new = None - if prev.axis == 'Rx' and this.axis == 'ty': # RX.TY -> TZ.RX + if prev.axis == "Rx" and this.axis == "ty": # RX.TY -> TZ.RX new = DHFactor.tx(prev.eta) * prev - elif prev.axis == 'Rx' and this.axis == 'tz': # RX.TZ -> TY.RX + elif prev.axis == "Rx" and this.axis == "tz": # RX.TZ -> TY.RX new = DHFactor.ty(-prev.eta) * prev - elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TX-> TZ.RY + elif prev.axis == "Ry" and this.axis == "tz": # RY.TX-> TZ.RY new = DHFactor.tz(-prev.eta) * prev - elif prev.axis == 'Ry' and this.axis == 'tz': # RY.TZ-> TX.RY + elif prev.axis == "Ry" and this.axis == "tz": # RY.TZ-> TX.RY new = DHFactor.tx(prev.eta) * prev - elif prev.axis == 'ty' and this.axis == 'Rx': # TY.RX -> RX.TZ + elif prev.axis == "ty" and this.axis == "Rx": # TY.RX -> RX.TZ new = this * DHFactor.tz(-this.eta) - elif prev.axis == 'tx' and this.axis == 'Rz': # TX.RZ -> RZ.TY + elif prev.axis == "tx" and this.axis == "Rz": # TX.RZ -> RZ.TY new = this * DHFactor.tz(this.eta) - elif prev.axis == 'Ry' and this.axis == 'Rx': # RY(Q).RX -> RX.RZ(-Q) + elif prev.axis == "Ry" and this.axis == "Rx": # RY(Q).RX -> RX.RZ(-Q) new = this * DHFactor.Rz(-prev.eta) - elif prev.axis == 'Rx' and this.axis == 'Ry': # RX.RY -> RZ.RX + elif prev.axis == "Rx" and this.axis == "Ry": # RX.RY -> RZ.RX new = DHFactor.Rz(this.eta) * prev - elif prev.axis == 'Rz' and this.axis == 'Rx': # RZ.RX -> RX.RY + elif prev.axis == "Rz" and this.axis == "Rx": # RZ.RX -> RX.RY new = this * DHFactor.Ry(this.eta) return new @@ -370,15 +376,14 @@ def subs_y(prev, this): if i == 0 or not jointyet: # leave initial const xform continue - prev = self[i-1] + prev = self[i - 1] new = subs_y(prev, this) if new is not None: - self[i-1:i+1] = new + self[i - 1 : i + 1] = new nchanges += 1 print(f"eliminate Y: {this} := {new}") - return nchanges # ---------------------------------------------------------------------- # @@ -448,7 +453,7 @@ def __str__(self, q=None): q = "q{0}" else: q = "q" - + # For each ET in the object, display it, data comes from properties # which come from the named tuple for et in self: @@ -460,7 +465,9 @@ def __str__(self, q=None): _j = j else: _j = et.jindex - qvar = q.format(_j, _j+1) # lgtm [py/str-format/surplus-argument] # noqa + qvar = q.format( + _j, _j + 1 + ) # lgtm [py/str-format/surplus-argument] # noqa else: qvar = "" if et.isflip: @@ -476,7 +483,7 @@ def __str__(self, q=None): s += f"{et.eta}" else: # adding to a previous value - if str(et.eta).startswith('-'): + if str(et.eta).startswith("-"): s += f"{et.eta}" else: s += f"+{et.eta}" @@ -494,9 +501,13 @@ def __str__(self, q=None): es.append(s) return " * ".join(es) - + + if __name__ == "__main__": # pragram: no cover - s = 'Tz(L1) Rz(q1) Ry(q2) Ty(L2) Tz(L3) Ry(q3) Tx(L4) Ty(L5) Tz(L6) Rz(q4) Ry(q5) Rz(q6)' + s = ( + "Tz(L1) Rz(q1) Ry(q2) Ty(L2) Tz(L3) Ry(q3) Tx(L4) Ty(L5) Tz(L6) Rz(q4) Ry(q5)" + " Rz(q6)" + ) ets = DHFactor.parse(s) print(ets) @@ -510,4 +521,3 @@ def __str__(self, q=None): print(ets) # ets.merge() print(ets) - diff --git a/roboticstoolbox/robot/DHLink.py b/roboticstoolbox/robot/DHLink.py index b94a4c9db..a50d1e6a3 100644 --- a/roboticstoolbox/robot/DHLink.py +++ b/roboticstoolbox/robot/DHLink.py @@ -127,7 +127,7 @@ class DHLink(Link): :type G: float :references: - - Robotics, Vision & Control, P. Corke, Springer 2011, Chap 7. + - Robotics, Vision & Control, P. Corke, Springer 2023, Chap 7. """ @@ -367,10 +367,7 @@ def __str__(self): qvar = f"q{self.id}" cls = self.__class__.__name__ if self.isrevolute: - s = ( - f"{cls}: θ={qvar}{offset}, d={self.d}, " - f" a={self.a}, ⍺={self.alpha}" - ) + s = f"{cls}: θ={qvar}{offset}, d={self.d}, a={self.a}, ⍺={self.alpha}" elif self.isprismatic: s = ( f"{cls}: θ={self.theta}, d={qvar}{offset}, " @@ -383,11 +380,11 @@ def __repr__(self): name = self.__class__.__name__ args = [] if self.isrevolute: - self._format(args, "d") + self._format_param(args, "d") else: - self._format(args, "theta", "θ") - self._format(args, "a") - self._format(args, "alpha", "⍺") + self._format_param(args, "theta", "θ") + self._format_param(args, "a") + self._format_param(args, "alpha", "⍺") args.extend(super()._params()) return name + "(" + ", ".join(args) + ")" @@ -749,7 +746,8 @@ class RevoluteDH(DHLink): :math:`\underbrace{\mathbf{T}_{rz}(q_i)}_{\mbox{variable}} \cdot \mathbf{T}_{tz}(d_i) \cdot \mathbf{T}_{tx}(a_i) \cdot \mathbf{T}_{rx}(\alpha_i)` where :math:`q_i` is the joint variable. :references: - - Robotics, Vision & Control, P. Corke, Springer 2011, Chap 7. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. + :seealso: :func:`PrismaticDH`, :func:`DHLink`, :func:`RevoluteMDH` """ # noqa @@ -813,7 +811,8 @@ class PrismaticDH(DHLink): :math:`\mathbf{T}_{rz}(\theta_i) \cdot \underbrace{\mathbf{T}_{tz}(q_i)}_{\mbox{variable}} \cdot \mathbf{T}_{tx}(a_i) \cdot \mathbf{T}_{rx}(\alpha_i)` where :math:`q_i` is the joint variable. :references: - - Robotics, Vision & Control, P. Corke, Springer 2011, Chap 7. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. + :seealso: :func:`RevoluteDH`, :func:`DHLink`, :func:`PrismaticMDH` """ # noqa @@ -883,7 +882,7 @@ class RevoluteMDH(DHLink): where :math:`q_i` is the joint variable. :references: - - Robotics, Vision & Control, P. Corke, Springer 2011, Chap 7. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. :seealso: :func:`PrismaticMDH`, :func:`DHLink`, :func:`RevoluteDH` """ # noqa @@ -956,7 +955,7 @@ class PrismaticMDH(DHLink): where :math:`q_i` is the joint variable. :references: - - Robotics, Vision & Control, P. Corke, Springer 2011, Chap 7. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. :seealso: :func:`RevoluteMDH`, :func:`DHLink`, :func:`PrismaticDH` """ # noqa diff --git a/roboticstoolbox/robot/DHRobot.py b/roboticstoolbox/robot/DHRobot.py index bcce1bd98..4fa75e7e3 100644 --- a/roboticstoolbox/robot/DHRobot.py +++ b/roboticstoolbox/robot/DHRobot.py @@ -69,15 +69,13 @@ class DHRobot(Robot): :reference: - - Robotics, Vision & Control, Chaps 7-9, - P. Corke, Springer 2011. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7-9. - Robot, Modeling & Control, M.Spong, S. Hutchinson & M. Vidyasagar, Wiley 2006. """ def __init__(self, links, meshdir=None, **kwargs): - # Verify L if not isinstance(links, list): raise TypeError("The links must be stored in a list.") @@ -115,9 +113,15 @@ def __init__(self, links, meshdir=None, **kwargs): else: raise TypeError("Input can be only DHLink or DHRobot") + for i, link in enumerate(all_links): + if i > 0: + link.parent = all_links[i - 1] + else: + link.parent = None + super().__init__(all_links, **kwargs) - self.ee_links = [self.links[-1]] + self._ee_links = [self.links[-1]] # Check the DH convention self._mdh = self.links[0].mdh @@ -201,6 +205,12 @@ def angle(theta, fmt=None): else: return str(theta * deg) + "\u00b0" + def format_attr(attr) -> str: + if isinstance(attr, float): + return f"{attr:.4g}" + else: + return str(attr) + has_qlim = any([link.qlim is not None for link in self]) if has_qlim: qlim_columns = [ @@ -245,6 +255,7 @@ def angle(theta, fmt=None): *qlim_columns, border=border, ) + for j, L in enumerate(self): if has_qlim: if L.isprismatic: @@ -255,11 +266,19 @@ def angle(theta, fmt=None): ql = [] if L.isprismatic: table.row( - angle(L.theta), qstr(j, L), f"{L.a:.4g}", angle(L.alpha), *ql + angle(L.theta), + qstr(j, L), + format_attr(L.a), + angle(L.alpha), + *ql, ) else: table.row( - qstr(j, L), f"{L.d:.4g}", f"{L.a:.4g}", angle(L.alpha), *ql + qstr(j, L), + format_attr(L.d), + format_attr(L.a), + angle(L.alpha), + *ql, ) s += str(table) @@ -280,7 +299,7 @@ def angle(theta, fmt=None): if tool is not None: table.row( "tool", - tool.printline(orient="rpy/xyz", fmt="{:.2g}", file=None), + tool.strline(orient="rpy/xyz", fmt="{:.2g}"), ) s += "\n" + str(table) @@ -304,9 +323,7 @@ def __add__(self, L): for j in range(L.n): nlinks.append(L.links[j]) else: - raise TypeError( - "Can only combine DHRobots with other " "DHRobots or DHLinks" - ) + raise TypeError("Can only combine DHRobots with other DHRobots or DHLinks") return DHRobot( nlinks, @@ -318,7 +335,6 @@ def __add__(self, L): ) def __deepcopy__(self, memo): - links = [] for link in self.links: @@ -420,7 +436,6 @@ def _set_link_fk(self, q): tall = self.fkine_all(q, old=True) for i, link in enumerate(self.links): - # Update the link model transforms for col in link.collision: col.wT = tall[i] @@ -722,7 +737,8 @@ def islimit(self, q=None): >>> robot.islimit([0, 0, -4, 4, 0, 0]) """ - q = self._getq(q) + if q is None: + q = self.q return [link.islimit(qk) for (link, qk) in zip(self, q)] @@ -946,7 +962,6 @@ def fkine(self, q, **kwargs): T = SE3.Empty() for qr in getmatrix(q, (None, self.n)): - first = True for q, L in zip(qr, self.links): if first: @@ -996,7 +1011,6 @@ def fkine_path(self, q, old=None): return T def segments(self): - segments = [None] segments.extend(self.links) return [segments] @@ -1037,7 +1051,9 @@ def fkine_all(self, q=None, old=True): - Joint offsets, if defined, are added to q before the forward kinematics are computed. """ - q = self._getq(q) + + if q is None: + q = self.q Tj = self.base.copy() Tall = Tj @@ -1610,7 +1626,6 @@ def removesmall(x): # ----------------- the forward recursion -------------------- # for j, link in enumerate(self.links): - Rt = Rm[j].T # transpose!! pstar = pstarm[:, j] r = link.r @@ -1742,7 +1757,8 @@ def removesmall(x): ) if debug: print( - f"j={j:}, G={link.G:}, Jm={link.Jm:}, friction={link.friction(qd_k[j], coulomb=False):}" + f"j={j:}, G={link.G:}, Jm={link.Jm:}," + f" friction={link.friction(qd_k[j], coulomb=False):}" ) # noqa print() @@ -2435,7 +2451,6 @@ def ik_gn( Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping ) - def ikine_LM( self, Tep: Union[np.ndarray, SE3], @@ -2474,7 +2489,6 @@ def _cross(a, b): if __name__ == "__main__": # pragma nocover - import roboticstoolbox as rtb # import spatialmath.base.symbolic as sym diff --git a/roboticstoolbox/robot/Dynamics.py b/roboticstoolbox/robot/Dynamics.py index e68ae8445..6a8c96802 100644 --- a/roboticstoolbox/robot/Dynamics.py +++ b/roboticstoolbox/robot/Dynamics.py @@ -11,11 +11,17 @@ :todo: perhaps these should be abstract properties, methods of this calss """ from collections import namedtuple +from typing import Any, Callable, Dict, Union import numpy as np from spatialmath.base import getvector, verifymatrix, isscalar, getmatrix, t2r, rot2jac from scipy import integrate, interpolate from spatialmath.base import symbolic as sym from roboticstoolbox import rtb_get_param +from roboticstoolbox.robot.RobotProto import RobotProto + +from roboticstoolbox.tools.types import ArrayLike, NDArray +from typing_extensions import Self +import roboticstoolbox as rtb from ansitable import ANSITable, Column import warnings @@ -24,46 +30,21 @@ class DynamicsMixin: # --------------------------------------------------------------------- # - - @property - def gravity(self): - """ - Get/set default gravitational acceleration (Robot superclass) - - - ``robot.name`` is the default gravitational acceleration - - :return: robot name - :rtype: ndarray(3,) - - - ``robot.name = ...`` checks and sets default gravitational - acceleration - - .. note:: If the z-axis is upward, out of the Earth, this should be - a positive number. - """ - return self._gravity - - @gravity.setter - def gravity(self, gravity_new): - self._gravity = getvector(gravity_new, 3) - self.dynchanged() - - # --------------------------------------------------------------------- # - def dynamics(self): + def dynamics(self: RobotProto): """ Pretty print the dynamic parameters (Robot superclass) The dynamic parameters (inertial and friction) are printed in a table, with one row per link. - Example: - + Examples + -------- .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.links[2].dyntable() + >>> robot.dyntable() - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.links[2].dyntable() - >>> robot.dyntable() """ unicode = rtb_get_param("unicode") table = ANSITable( @@ -78,16 +59,17 @@ def dynamics(self): border="thin" if unicode else "ascii", ) - for j, link in enumerate(self): + for j, link in enumerate(self.links): table.row(link.name, *link._dyn2list()) table.print() - def dynamics_list(self): + def dynamics_list(self: RobotProto): """ Print dynamic parameters (Robot superclass) Display the kinematic and dynamic parameters to the console in reable format + """ for j, link in enumerate(self.links): print("\nLink {:d}::".format(j), link) @@ -95,16 +77,10 @@ def dynamics_list(self): # --------------------------------------------------------------------- # - def friction(self, qd): + def friction(self: RobotProto, qd: NDArray) -> NDArray: r""" Manipulator joint friction (Robot superclass) - :param qd: The joint velocities of the robot - :type qd: ndarray(n) - - :return: The joint friction forces/torques for the robot - :rtype: ndarray(n,) - ``robot.friction(qd)`` is a vector of joint friction forces/torques for the robot moving with joint velocities ``qd``. @@ -119,32 +95,46 @@ def friction(self, qd): \tau_{C,j}^+ & \mbox{if $\dot{q}_j > 0$} \\ \tau_{C,j}^- & \mbox{if $\dot{q}_j < 0$} \end{array} \right. - .. note:: - - - The friction value should be added to the motor output torque to - determine the nett torque. It has a negative value when qd > 0. - - The returned friction value is referred to the output of the - gearbox. - - The friction parameters in the Link object are referred to the - motor. - - Motor viscous friction is scaled up by :math:`G^2`. - - Motor Coulomb friction is scaled up by math:`G`. - - The appropriate Coulomb friction value to use in the - non-symmetric case depends on the sign of the joint velocity, - not the motor velocity. - - Coulomb friction is zero for zero joint velocity, stiction is - not modeled. - - The absolute value of the gear ratio is used. Negative gear - ratios are tricky: the Puma560 robot has negative gear ratio for - joints 1 and 3. - - The absolute value of the gear ratio is used. Negative gear - ratios are tricky: the Puma560 has negative gear ratio for - joints 1 and 3. - - :seealso: :func:`Robot.nofriction`, :func:`Link.friction` + Parameters + ---------- + qd + The joint velocities of the robot + + Returns + ------- + friction + The joint friction forces/torques for the robot + + Notes + ----- + - The friction value should be added to the motor output torque to + determine the nett torque. It has a negative value when qd > 0. + - The returned friction value is referred to the output of the + gearbox. + - The friction parameters in the Link object are referred to the + motor. + - Motor viscous friction is scaled up by :math:`G^2`. + - Motor Coulomb friction is scaled up by math:`G`. + - The appropriate Coulomb friction value to use in the + non-symmetric case depends on the sign of the joint velocity, + not the motor velocity. + - Coulomb friction is zero for zero joint velocity, stiction is + not modeled. + - The absolute value of the gear ratio is used. Negative gear + ratios are tricky: the Puma560 robot has negative gear ratio for + joints 1 and 3. + - The absolute value of the gear ratio is used. Negative gear + ratios are tricky: the Puma560 has negative gear ratio for + joints 1 and 3. + + See Also + -------- + :func:`Robot.nofriction` + :func:`Link.friction` + """ - qd = getvector(qd, self.n) + qd = np.array(getvector(qd, self.n)) tau = np.zeros(self.n) for i in range(self.n): @@ -154,26 +144,37 @@ def friction(self, qd): # --------------------------------------------------------------------- # - def nofriction(self, coulomb=True, viscous=False): + def nofriction(self: RobotProto, coulomb: bool = True, viscous: bool = False): """ - Remove manipulator joint friction (Robot superclass) - - :param coulomb: set the Coulomb friction to 0 - :type coulomb: bool - :param viscous: set the viscous friction to 0 - :type viscous: bool - :return: A copy of the robot with dynamic parameters perturbed - :rtype: Robot subclass + Remove manipulator joint friction ``nofriction()`` copies the robot and returns a robot with the same link parameters except the Coulomb and/or viscous friction parameter are set to zero. - :seealso: :func:`Robot.friction`, :func:`Link.nofriction` + Parameters + ---------- + coulomb + set the Coulomb friction to 0 + viscous + set the viscous friction to 0 + + Returns + ------- + robot + A copy of the robot with dynamic parameters perturbed + + See Also + -------- + :func:`Robot.friction` + :func:`Link.nofriction` + """ # shallow copy the robot object - self.delete_rne() # remove the inherited C pointers + if isinstance(self, rtb.DHRobot): + self.delete_rne() # remove the inherited C pointers + nf = self.copy() nf.name = "NF/" + self.name @@ -183,53 +184,31 @@ def nofriction(self, coulomb=True, viscous=False): return nf def fdyn( - self, - T, - q0, - torque=None, - torque_args={}, - qd0=None, - solver="RK45", - solver_args={}, - dt=None, - progress=False, + self: RobotProto, + T: float, + q0: ArrayLike, + Q: Union[Callable[[Any, float, NDArray, NDArray], NDArray], None] = None, + Q_args: Dict = {}, + qd0: Union[ArrayLike, None] = None, + solver: str = "RK45", + solver_args: Dict = {}, + dt: Union[float, None] = None, + progress: bool = False, ): """ Integrate forward dynamics - :param T: integration time - :type T: float - :param q0: initial joint coordinates - :type q0: array_like - :param qd0: initial joint velocities, assumed zero if not given - :type qd0: array_like - :param torque: a function that computes torque as a function of time - and/or state - :type torque: callable - :param torque_args: positional arguments passed to ``torque`` - :type torque_args: dict - :type solver: name of scipy solver to use, RK45 is the default - :param solver: str - :type solver_args: arguments passed to the solver - :param solver_args: dict - :type dt: time step for results - :param dt: float - :param progress: show progress bar, default False - :type progress: bool - - :return: robot trajectory - :rtype: namedtuple - - - ``tg = R.fdyn(T, q)`` integrates the dynamics of the robot with zero - input torques over the time interval 0 to ``T`` and returns the - trajectory as a namedtuple with elements: - - - ``t`` the time vector (M,) - - ``q`` the joint coordinates (M,n) - - ``qd`` the joint velocities (M,n) - - - ``tg = R.fdyn(T, q, torqfun)`` as above but the torque applied to the - joints is given by the provided function:: + + ``tg = R.fdyn(T, q)`` integrates the dynamics of the robot with zero + input torques over the time interval 0 to ``T`` and returns the + trajectory as a namedtuple with elements: + + - ``t`` the time vector (M,) + - ``q`` the joint coordinates (M,n) + - ``qd`` the joint velocities (M,n) + + ``tg = R.fdyn(T, q, torqfun)`` as above but the torque applied to the + joints is given by the provided function:: tau = function(robot, t, q, qd, **args) @@ -240,57 +219,89 @@ def fdyn( - current joint coordinates (n,) - current joint velocity (n,) - args, optional keyword arguments can be specified, these are - passed in from the ``targs`` kewyword argument. + passed in from the ``targs`` kewyword argument. The function must return a Numpy array (n,) of joint forces/torques. - Examples: - - #. to apply zero joint torque to the robot without Coulomb - friction:: - - def myfunc(robot, t, q, qd): - return np.zeros((robot.n,)) - - tg = robot.nofriction().fdyn(5, q0, myfunc) - - plt.figure() - plt.plot(tg.t, tg.q) - plt.show() - - We could also use a lambda function:: - - tg = robot.nofriction().fdyn( - 5, q0, lambda r, t, q, qd: np.zeros((r.n,))) - - #. the robot is controlled by a PD controller. We first define a - function to compute the control which has additional parameters for - the setpoint and control gains (qstar, P, D):: - - def myfunc(robot, t, q, qd, qstar, P, D): - return (qstar - q) * P + qd * D # P, D are (6,) - - tg = robot.fdyn(10, q0, myfunc, torque_args=(qstar, P, D)) ) + Parameters + ---------- + T + integration time + q0 + initial joint coordinates + qd0 + initial joint velocities, assumed zero if not given + Q + a function that computes generalized joint force as a function of + time and/or state + Q_args + positional arguments passed to ``torque`` + solver + str + solver_args + dict + dt + float + progress + show progress bar, default False + + Returns + ------- + trajectory + robot trajectory + + Examples + -------- + + To apply zero joint torque to the robot without Coulomb + friction: + + >>> def myfunc(robot, t, q, qd): + >>> return np.zeros((robot.n,)) + + >>> tg = robot.nofriction().fdyn(5, q0, myfunc) + + >>> plt.figure() + >>> plt.plot(tg.t, tg.q) + >>> plt.show() + + We could also use a lambda function:: + + >>> tg = robot.nofriction().fdyn( + >>> 5, q0, lambda r, t, q, qd: np.zeros((r.n,))) + + The robot is controlled by a PD controller. We first define a + function to compute the control which has additional parameters for + the setpoint and control gains (qstar, P, D):: + + >>> def myfunc(robot, t, q, qd, qstar, P, D): + >>> return (qstar - q) * P + qd * D # P, D are (6,) + + >>> tg = robot.fdyn(10, q0, myfunc, torque_args=(qstar, P, D)) ) Many integrators have variable step length which is problematic if we want to animate the result. If ``dt`` is specified then the solver results are interpolated in time steps of ``dt``. - .. note:: - - - This function performs poorly with non-linear joint friction, + Notes + ----- + - This function performs poorly with non-linear joint friction, such as Coulomb friction. The R.nofriction() method can be used to set this friction to zero. - - If the function is not specified then zero force/torque is + - If the function is not specified then zero force/torque is applied to the manipulator joints. - - Interpolation is performed using `ScipY integrate.ode + - Interpolation is performed using `ScipY integrate.ode ` - - The SciPy RK45 integrator is used by default - - Interpolation is performed using `SciPy interp1 + - The SciPy RK45 integrator is used by default + - Interpolation is performed using `SciPy interp1 ` - :seealso: :func:`DHRobot.accel`, :func:`DHRobot.nofriction`, - :func:`DHRobot.rne`. + See Also + -------- + :func:`DHRobot.accel` + :func:`DHRobot.nofriction`, + :func:`DHRobot.rne`. + """ n = self.n @@ -302,9 +313,9 @@ def myfunc(robot, t, q, qd, qstar, P, D): qd0 = np.zeros((n,)) else: qd0 = getvector(qd0, n) - if torque is not None: - if not callable(torque): - raise ValueError("torque function must be callable") + if Q is not None: + if not callable(Q): + raise ValueError("generalized joint torque function must be callable") # concatenate q and qd into the initial state vector x0 = np.r_[q0, qd0] @@ -313,7 +324,7 @@ def myfunc(robot, t, q, qd, qstar, P, D): scipy_integrator = integrate.__dict__[solver] integrator = scipy_integrator( - lambda t, y: self._fdyn(t, y, torque, torque_args), + lambda t, y: self._fdyn(t, y, Q, Q_args), t0=0.0, y0=x0, t_bound=T, @@ -362,25 +373,36 @@ def myfunc(robot, t, q, qd, qstar, P, D): else: return namedtuple("fdyn", "t q qd")(tarray, xarray[:, :n], xarray[:, n:]) - def _fdyn(self, t, x, torqfun, targs): + def _fdyn( + self: RobotProto, + t: float, + x: NDArray, + Qfunc: Callable[[Any, float, NDArray, NDArray], NDArray], + Qargs: Dict, + ): """ Private function called by fdyn - :param t: current time - :type t: float - :param x: current state [q, qd] - :type x: numpy array (2n,) - :param torqfun: a function that computes torque as a function of time - and/or state - :type torqfun: callable - :param targs: argumments passed to ``torqfun`` - :type targs: dict - - :return: derivative of current state [qd, qdd] - :rtype: numpy array (2n,) - Called by ``fdyn`` to evaluate the robot velocity and acceleration for forward dynamics. + + Parameters + ---------- + t + current time + x + current state [q, qd] + Qfunc + a function that computes torque as a function of time + and/or state + Qargs : dict + argumments passed to ``Qfunc`` + + Returns + ------- + fdyn + derivative of current state [qd, qdd] + """ n = self.n @@ -388,10 +410,10 @@ def _fdyn(self, t, x, torqfun, targs): qd = x[n:] # evaluate the torque function if one is given - if torqfun is None: + if Qfunc is None: tau = np.zeros((n,)) else: - tau = torqfun(self, t, q, qd, **targs) + tau = Qfunc(self, t, q, qd, **Qargs) if len(tau) != n or not all(np.isreal(tau)): raise RuntimeError( "torque function must return vector with N real elements" @@ -401,22 +423,10 @@ def _fdyn(self, t, x, torqfun, targs): return np.r_[qd, qdd] - def accel(self, q, qd, torque, gravity=None): + def accel(self: RobotProto, q, qd, torque, gravity=None): r""" Compute acceleration due to applied torque - :param q: Joint coordinates - :type q: ndarray(n) - :param qd: Joint velocity - :type qd: ndarray(n) - :param torque: Joint torques of the robot - :type torque: ndarray(n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). - :type gravity: ndarray(3) - :return: Joint accelerations of the robot - :rtype: ndarray(n) - ``qdd = accel(q, qd, torque)`` calculates a vector (n) of joint accelerations that result from applying the actuator force/torque (n) to the manipulator in state `q` (n) and `qd` (n), and ``n`` is @@ -426,34 +436,51 @@ def accel(self, q, qd, torque, gravity=None): \ddot{q} = \mathbf{M}^{-1} \left(\tau - \mathbf{C}(q)\dot{q} - \mathbf{g}(q)\right) - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.accel(puma.qz, 0.5 * np.ones(6), np.zeros(6)) - **Trajectory operation** If `q`, `qd`, torque are matrices (m,n) then ``qdd`` is a matrix (m,n) where each row is the acceleration corresponding to the equivalent rows of q, qd, torque. - .. note:: - - Useful for simulation of manipulator dynamics, in - conjunction with a numerical integration function. - - Uses the method 1 of Walker and Orin to compute the forward - dynamics. - - Featherstone's method is more efficient for robots with large - numbers of joints. - - Joint friction is considered. - - :references: - - Efficient dynamic computer simulation of robotic mechanisms, - M. W. Walker and D. E. Orin, - ASME Journa of Dynamic Systems, Measurement and Control, vol. - 104, no. 3, pp. 205-211, 1982. + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + torque + Joint torques of the robot + gravity + Gravitational acceleration (Optional, if not supplied will + use the ``gravity`` attribute of self). + + Returns + ------- + ndarray(n) + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.accel(puma.qz, 0.5 * np.ones(6), np.zeros(6)) + + Notes + ----- + - Useful for simulation of manipulator dynamics, in + conjunction with a numerical integration function. + - Uses the method 1 of Walker and Orin to compute the forward + dynamics. + - Featherstone's method is more efficient for robots with large + numbers of joints. + - Joint friction is considered. + + References + ---------- + - Efficient dynamic computer simulation of robotic mechanisms, + M. W. Walker and D. E. Orin, + ASME Journa of Dynamic Systems, Measurement and Control, vol. + 104, no. 3, pp. 205-211, 1982. """ # noqa @@ -484,8 +511,16 @@ def accel(self, q, qd, torque, gravity=None): else: return qdd - def pay(self, W, q=None, J=None, frame=1): + def pay( + self: RobotProto, + W: ArrayLike, + q: Union[NDArray, None] = None, + J: Union[NDArray, None] = None, + frame: int = 1, + ): """ + Generalised joint force/torque due to a payload wrench + tau = pay(W, J) Returns the generalised joint force/torques due to a payload wrench W applied to the end-effector. Where the manipulator Jacobian is J (6xn), and n is the number of robot joints. @@ -502,44 +537,52 @@ def pay(self, W, q=None, J=None, frame=1): is the generalised force/torque at the pose given by corresponding row of q. - :param W: A wrench vector applied at the end effector, + Parameters + ---------- + W + A wrench vector applied at the end effector, W = [Fx Fy Fz Mx My Mz] - :type W: ndarray(6) - :param q: Joint coordinates - :type q: ndarray(n) - :param J: The manipulator Jacobian (Optional, if not supplied will + q + Joint coordinates + J + The manipulator Jacobian (Optional, if not supplied will use the q value). - :type J: ndarray(6,n) - :param frame: The frame in which to torques are expressed in when J + frame + The frame in which to torques are expressed in when J is not supplied. 0 means base frame of the robot, 1 means end- effector frame - :type frame: int - :return: Joint forces/torques due to w - :rtype: ndarray(n) + Returns + ------- + t + Joint forces/torques due to w - .. note:: - - Wrench vector and Jacobian must be from the same reference - frame. - - Tool transforms are taken into consideration when frame=1. - - Must have a constant wrench - no trajectory support for this - yet. + Notes + ----- + - Wrench vector and Jacobian must be from the same reference + frame. + - Tool transforms are taken into consideration when frame=1. + - Must have a constant wrench - no trajectory support for this + yet. """ try: - W = getvector(W, 6) + W = np.array(getvector(W, 6)) trajn = 0 except ValueError: - trajn = W.shape[0] - verifymatrix(W, (trajn, 6)) + if isinstance(W, NDArray): + trajn = W.shape[0] + verifymatrix(W, (trajn, 6)) + else: + raise ValueError("W is invalid") if trajn: # A trajectory if J is not None: # Jacobian supplied verifymatrix(J, (trajn, 6, self.n)) - else: + elif q is not None: # Use q instead verifymatrix(q, (trajn, self.n)) J = np.zeros((trajn, 6, self.n)) @@ -548,6 +591,8 @@ def pay(self, W, q=None, J=None, frame=1): J[i, :, :] = self.jacobe(q[i, :]) else: J[i, :, :] = self.jacob0(q[i, :]) + else: + raise ValueError("q of J is needed for trajectory") else: # Single configuration if J is not None: @@ -575,8 +620,10 @@ def pay(self, W, q=None, J=None, frame=1): return tau - def payload(self, m, p=np.zeros(3)): + def payload(self: RobotProto, m: float, p=np.zeros(3)): """ + Add a payload to the end-effector + payload(m, p) adds payload mass adds a payload with point mass m at position p in the end-effector coordinate frame. @@ -585,10 +632,12 @@ def payload(self, m, p=np.zeros(3)): payload(0) removes added payload. - :param m: mass (kg) - :type m: float - :param p: position in end-effector frame - :type p: ndarray(3,1) + Parameters + ---------- + m + mass (kg) + p + position in end-effector frame """ @@ -598,26 +647,32 @@ def payload(self, m, p=np.zeros(3)): lastlink.m = m lastlink.r = p - def jointdynamics(self, q, qd=None): + def jointdynamics(self: RobotProto, q, qd=None): """ Transfer function of joint actuator - :param q: Joint coordinates - :type q: ndarray(n) - :param qd: Joint velocity - :type qd: ndarray(n) - :return: transfer function denominators - :rtype: list of 2-tuples - - - ``tf = jointdynamics(qd, q)`` calculates a vector of n - continuous-time transfer functions that represent the transfer - function 1/(Js+B) for each joint based on the dynamic parameters - of the robot and the configuration q (n). n is the number of robot - joints. The result is a list of tuples (J, B) for each joint. - - - ``tf = jointdynamics(q, qd)`` as above but include the linearized - effects of Coulomb friction when operating at joint velocity QD - (1xN). + ``tf = jointdynamics(qd, q)`` calculates a vector of n + continuous-time transfer functions that represent the transfer + function 1/(Js+B) for each joint based on the dynamic parameters + of the robot and the configuration q (n). n is the number of robot + joints. The result is a list of tuples (J, B) for each joint. + + ``tf = jointdynamics(q, qd)`` as above but include the linearized + effects of Coulomb friction when operating at joint velocity QD + (1xN). + + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + + Returns + ------- + list of 2-tuples + transfer function denominators + """ tf = [] @@ -642,51 +697,56 @@ def jointdynamics(self, q, qd=None): return tf - def cinertia(self, q): + def cinertia(self: RobotProto, q): """ Deprecated, use ``inertia_x`` """ warnings.warn("cinertia is deprecated, use inertia_x", DeprecationWarning) - def inertia(self, q): - """ - Manipulator inertia matrix - - :param q: Joint coordinates - :type q: ndarray(n) or ndarray(m,n) - - :return: The inertia matrix - :rtype: ndarray(n,n) or ndarray(m,n,n) - + def inertia(self: RobotProto, q: NDArray) -> NDArray: + """Manipulator inertia matrix ``inertia(q)`` is the symmetric joint inertia matrix (n,n) which relates joint torque to joint acceleration for the robot at joint configuration q. - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.inertia(puma.qz) - **Trajectory operation** If ``q`` is a matrix (m,n), each row is interpretted as a joint state vector, and the result is a 3d-matrix (nxnxk) where each plane corresponds to the inertia for the corresponding row of q. - .. note:: - - The diagonal elements ``M[j,j]`` are the inertia seen by joint - actuator ``j``. - - The off-diagonal elements ``M[j,k]`` are coupling inertias that - relate acceleration on joint ``j`` to force/torque on - joint ``k``. - - The diagonal terms include the motor inertia reflected through - the gear ratio. + Parameters + ---------- + q + Joint coordinates + + Returns + ------- + inertia + The inertia matrix + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.inertia(puma.qz) + + Notes + ----- + - The diagonal elements ``M[j,j]`` are the inertia seen by joint + actuator ``j``. + - The off-diagonal elements ``M[j,k]`` are coupling inertias that + relate acceleration on joint ``j`` to force/torque on + joint ``k``. + - The diagonal terms include the motor inertia reflected through + the gear ratio. + + See Also + -------- + :func:`cinertia` - :seealso: :func:`cinertia` """ q = getmatrix(q, (None, self.n)) @@ -705,17 +765,10 @@ def inertia(self, q): else: return In - def coriolis(self, q, qd): + def coriolis(self: RobotProto, q, qd): r""" Coriolis and centripetal term - :param q: Joint coordinates - :type q: ndarray(n) or ndarray(m,n) - :param qd: Joint velocity - :type qd: ndarray(n) or ndarray(m,n) - :return: Velocity matrix - :rtype: ndarray(n,n) or ndarray(m,n,n) - ``coriolis(q, qd)`` calculates the Coriolis/centripetal matrix (n,n) for the robot in configuration ``q`` and velocity ``qd``, where ``n`` is the number of joints. @@ -727,24 +780,37 @@ def coriolis(self, q, qd): since it describes the disturbance forces on any joint due to velocity of all other joints. - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.coriolis(puma.qz, 0.5 * np.ones((6,))) - **Trajectory operation** If ``q`` and `qd` are matrices (m,n), each row is interpretted as a joint configuration, and the result (n,n,m) is a 3d-matrix where each plane corresponds to a row of ``q`` and ``qd``. - .. note:: - - Joint viscous friction is also a joint force proportional to - velocity but it is eliminated in the computation of this value. - - Computationally slow, involves :math:`n^2/2` invocations of RNE. + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + + Returns + ------- + coriolis + Velocity matrix + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.coriolis(puma.qz, 0.5 * np.ones((6,))) + + Notes + ----- + - Joint viscous friction is also a joint force proportional to + velocity but it is eliminated in the computation of this value. + - Computationally slow, involves :math:`n^2/2` invocations of RNE. + """ q = getmatrix(q, (None, self.n)) @@ -797,19 +863,14 @@ def coriolis(self, q, qd): else: return C - def gravload(self, q=None, gravity=None): + def gravload( + self: RobotProto, + q: Union[ArrayLike, None] = None, + gravity: Union[ArrayLike, None] = None, + ): """ Compute gravity load - :param q: Joint coordinates - :type q: ndarray(n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the stored gravity values). - :type gravity: ndarray(3) - - :return: The generalised joint force/torques due to gravity - :rtype: ndarray(n) - ``robot.gravload(q)`` calculates the joint gravity loading (n) for the robot in the joint configuration ``q`` and using the default gravitational acceleration specified in the DHRobot object. @@ -817,20 +878,32 @@ def gravload(self, q=None, gravity=None): ``robot.gravload(q, gravity=g)`` as above except the gravitational acceleration is explicitly specified as ``g``. - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.gravload(puma.qz) - **Trajectory operation** If q is a matrix (nxm) each column is interpreted as a joint configuration vector, and the result is a matrix (nxm) each column being the corresponding joint torques. + Parameters + ---------- + q + Joint coordinates + gravity : ndarray(3) + Gravitational acceleration (Optional, if not supplied will + use the stored gravity values). + + Returns + ------- + gravload + The generalised joint force/torques due to gravity + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.gravload(puma.qz) + """ q = getmatrix(q, (None, self.n)) @@ -851,20 +924,12 @@ def gravload(self, q=None, gravity=None): else: return taug - def inertia_x(self, q=None, pinv=False, representation="rpy/xyz", Ji=None): + def inertia_x( + self: RobotProto, q=None, pinv=False, representation="rpy/xyz", Ji=None + ): r""" Operational space inertia matrix - :param q: Joint coordinates - :type q: array_like(n) or ndarray(m,n) - :param pinv: use pseudo inverse rather than inverse - :type pinv: bool - :param analytical: the type of analytical Jacobian to use, default is - 'rpy/xyz' - :type analytical: str - :return: The inertia matrix - :rtype: ndarray(6,6) or ndarray(m,6,6) - ``robot.inertia_x(q)`` is the operational space (Cartesian) inertia matrix which relates Cartesian force/torque to Cartesian acceleration at the joint configuration q. @@ -885,14 +950,6 @@ def inertia_x(self, q=None, pinv=False, representation="rpy/xyz", Ji=None): ``'exp'`` exponential coordinate rates ============= ======================================== - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.inertia_x(puma.qz) - **Trajectory operation** If ``q`` is a matrix (m,n), each row is interpretted as a joint state @@ -900,14 +957,45 @@ def inertia_x(self, q=None, pinv=False, representation="rpy/xyz", Ji=None): corresponds to the Cartesian inertia for the corresponding row of ``q``. - .. note:: - - If the robot is not 6 DOF the ``pinv`` option is set True. - - ``pinv()`` is around 5x slower than ``inv()`` + Parameters + ---------- + q + Joint coordinates + pinv + use pseudo inverse rather than inverse (Default value = False) + analytical + the type of analytical Jacobian to use, default is + 'rpy/xyz' + representation + (Default value = "rpy/xyz") + Ji + The inverse analytical Jacobian (base-frame) + + Returns + ------- + inertia_x + The operational space inertia matrix + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.inertia_x(puma.qz) + + Notes + ----- + - If the robot is not 6 DOF the ``pinv`` option is set True. + - ``pinv()`` is around 5x slower than ``inv()`` .. warning:: Assumes that the operational space has 6 DOF. - :seealso: :func:`inertia` + See Also + -------- + :func:`inertia` + """ + q = getmatrix(q, (None, self.n)) if q.shape[1] != 6: pinv = True @@ -939,7 +1027,7 @@ def inertia_x(self, q=None, pinv=False, representation="rpy/xyz", Ji=None): return Mt def coriolis_x( - self, + self: RobotProto, q, qd, pinv=False, @@ -953,18 +1041,6 @@ def coriolis_x( r""" Operational space Coriolis and centripetal term - :param q: Joint coordinates - :type q: ndarray(n) or ndarray(m,n) - :param qd: Joint velocity - :type qd: ndarray(n) or ndarray(m,n) - :param pinv: use pseudo inverse rather than inverse - :type pinv: bool - :param analytical: the type of analytical Jacobian to use, default is - 'rpy/xyz' - :type analytical: str - :return: Operational space velocity matrix - :rtype: ndarray(6,6) or ndarray(m,6,6) - ``coriolis_x(q, qd)`` is the Coriolis/centripetal matrix (m,m) in operational space for the robot in configuration ``q`` and velocity ``qd``, where ``n`` is the number of joints. @@ -992,31 +1068,66 @@ def coriolis_x( ``'exp'`` exponential coordinate rates ============= ======================================== - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.coriolis_x(puma.qz, 0.5 * np.ones((6,))) - **Trajectory operation** If ``q`` and `qd` are matrices (m,n), each row is interpretted as a joint configuration, and the result (n,n,m) is a 3d-matrix where each plane corresponds to a row of ``q`` and ``qd``. - .. note:: - - Joint viscous friction is also a joint force proportional to - velocity but it is eliminated in the computation of this value. - - Computationally slow, involves :math:`n^2/2` invocations of RNE. - - If the robot is not 6 DOF the ``pinv`` option is set True. - - ``pinv()`` is around 5x slower than ``inv()`` + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + pinv + use pseudo inverse rather than inverse (Default value = False) + analytical + the type of analytical Jacobian to use, default is + 'rpy/xyz' + representation + (Default value = "rpy/xyz") + J + + Ji + + Jd + + C + + Mx + + + Returns + ------- + ndarray(6,6) or ndarray(m,6,6) + Operational space velocity matrix + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.coriolis_x(puma.qz, 0.5 * np.ones((6,))) + + Notes + ----- + - Joint viscous friction is also a joint force proportional to + velocity but it is eliminated in the computation of this value. + - Computationally slow, involves :math:`n^2/2` invocations of RNE. + - If the robot is not 6 DOF the ``pinv`` option is set True. + - ``pinv()`` is around 5x slower than ``inv()`` .. warning:: Assumes that the operational space has 6 DOF. - :seealso: :func:`coriolis`, :func:`inertia_x`, :func:`hessian0` + See Also + -------- + :func:`coriolis` + :func:`inertia_x` + :func:`hessian0` + """ + q = getmatrix(q, (None, self.n)) qd = getmatrix(qd, (None, self.n)) n = q.shape[1] @@ -1036,7 +1147,7 @@ def coriolis_x( if Mx is None: Mx = self.inertia_x(q[0, :], Ji=Ji) if Jd is None: - Jd = self.jacob_dot(q[0, :], qd[0, :], J0=Ja) + Jd = self.jacob0_dot(q[0, :], qd[0, :], J0=Ja) return Ji.T @ (C - Mx @ Jd) @ Ji else: # trajectory case @@ -1053,31 +1164,23 @@ def coriolis_x( C = self.coriolis(qk, qdk) Mx = self.inertia_x(qk, Ji=Ji) - Jd = self.jacob_dot(qk, qdk, J0=J) + Jd = self.jacob0_dot(qk, qdk, J0=J) Ct[k, :, :] = Ji.T @ (C - Mx @ Jd) @ Ji return Ct def gravload_x( - self, q=None, gravity=None, pinv=False, representation="rpy/xyz", Ji=None + self: RobotProto, + q=None, + gravity=None, + pinv=False, + representation="rpy/xyz", + Ji=None, ): - """ + r""" Operational space gravity load - :param q: Joint coordinates - :type q: ndarray(n) or ndarray(m,n) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). - :type gravity: ndarray(3) - :param pinv: use pseudo inverse rather than inverse - :type pinv: bool - :param analytical: the type of analytical Jacobian to use, default is - 'rpy/xyz' - :type analytical: str - :return: The operational space gravity wrench - :rtype: ndarray(6) or ndarray(m,6) - ``robot.gravload_x(q)`` calculates the gravity wrench for the robot in the joint configuration ``q`` and using the default gravitational acceleration specified in the robot object. @@ -1101,27 +1204,52 @@ def gravload_x( ``'exp'`` exponential coordinate rates ============= ======================================== - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.gravload_x(puma.qz) - **Trajectory operation** If q is a matrix (nxm) each column is interpreted as a joint configuration vector, and the result is a matrix (nxm) each column being the corresponding joint torques. - .. note:: - - If the robot is not 6 DOF the ``pinv`` option is set True. - - ``pinv()`` is around 5x slower than ``inv()`` + Parameters + ---------- + q + Joint coordinates + gravity + Gravitational acceleration (Optional, if not supplied will + use the ``gravity`` attribute of self). + pinv + use pseudo inverse rather than inverse (Default value = False) + analytical + the type of analytical Jacobian to use, default is + 'rpy/xyz' + representation : + (Default value = "rpy/xyz") + Ji : + + + Returns + ------- + gravload + The operational space gravity wrench + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.gravload_x(puma.qz) + + Notes + ----- + - If the robot is not 6 DOF the ``pinv`` option is set True. + - ``pinv()`` is around 5x slower than ``inv()`` .. warning:: Assumes that the operational space has 6 DOF. - :seealso: :func:`gravload` + See Also + -------- + :func:`gravload` + """ q = getmatrix(q, (None, self.n)) @@ -1136,7 +1264,7 @@ def gravload_x( if q.shape[0] == 1: # single q case if Ji is None: - Ja = self.jacob0(q[0, :], analytical=representation) + Ja = self.jacob0_analytical(q[0, :], representation=representation) if pinv: Ji = np.linalg.pinv(Ja) else: @@ -1150,7 +1278,7 @@ def gravload_x( # z = np.zeros(self.n) for k, qk in enumerate(q): - Ja = self.jacob0(qk, analytical=representation) + Ja = self.jacob0_analytical(qk, representation=representation) G = self.gravload(qk) if pinv: Ji = np.linalg.pinv(Ja) @@ -1162,28 +1290,17 @@ def gravload_x( return taug def accel_x( - self, q, xd, wrench, gravity=None, pinv=False, representation="rpy/xyz" + self: RobotProto, + q, + xd, + wrench, + gravity=None, + pinv=False, + representation="rpy/xyz", ): r""" Operational space acceleration due to applied wrench - :param q: Joint coordinates - :type q: ndarray(n) or ndarray(m,n) - :param qd: Joint velocity - :type qd: ndarray(n) or ndarray(m,n) - :param wrench: Wrench applied to the end-effector - :type torque: ndarray(6) or ndarray(m,6) - :param gravity: Gravitational acceleration (Optional, if not supplied will - use the ``gravity`` attribute of self). - :type gravity: ndarray(3) - :param pinv: use pseudo inverse rather than inverse - :type pinv: bool - :param analytical: the type of analytical Jacobian to use, default is - 'rpy/xyz' - :type analytical: str - :return: Operational space accelerations of the end-effector - :rtype: ndarray(6) or ndarray(m,6) - ``xdd = accel_x(q, qd, wrench)`` is the operational space acceleration due to ``wrench`` applied to the end-effector of a robot in joint configuration ``q`` and joint velocity ``qd``. @@ -1194,30 +1311,58 @@ def accel_x( \mathbf{J}(q)^T w - \mathbf{C}(q)\dot{q} - \mathbf{g}(q) \right) - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.accel_x(puma.qz, 0.5 * np.ones(6), np.zeros(6)) - **Trajectory operation** If `q`, `qd`, torque are matrices (m,n) then ``qdd`` is a matrix (m,n) where each row is the acceleration corresponding to the equivalent rows of q, qd, wrench. - .. note:: - - Useful for simulation of manipulator dynamics, in - conjunction with a numerical integration function. - - Uses the method 1 of Walker and Orin to compute the forward - dynamics. - - Featherstone's method is more efficient for robots with large - numbers of joints. - - Joint friction is considered. + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + wrench + Wrench applied to the end-effector + gravity + Gravitational acceleration (Optional, if not supplied will + use the ``gravity`` attribute of self). + pinv + use pseudo inverse rather than inverse + analytical + the type of analytical Jacobian to use, default is + 'rpy/xyz' + xd : + representation : + (Default value = "rpy/xyz") + + Returns + ------- + accel + Operational space accelerations of the end-effector + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.accel_x(puma.qz, 0.5 * np.ones(6), np.zeros(6)) + + Notes + ----- + - Useful for simulation of manipulator dynamics, in + conjunction with a numerical integration function. + - Uses the method 1 of Walker and Orin to compute the forward + dynamics. + - Featherstone's method is more efficient for robots with large + numbers of joints. + - Joint friction is considered. + + See Also + -------- + :func:`accel` - :seealso: :func:`accel` """ # noqa q = getmatrix(q, (None, self.n)) @@ -1230,7 +1375,7 @@ def accel_x( for k, (qk, xdk, wk) in enumerate(zip(q, xd, w)): - Ja = self.jacob0(qk, analytical=representation) + Ja = self.jacob0_analytical(qk, representation=representation) if pinv: Ji = np.linalg.pinv(Ja) else: @@ -1251,7 +1396,7 @@ def accel_x( # solve is faster than inv() which is faster than pinv() # tau_rne = C(q,qd) + G(q) # qdd = M^-1 (tau - C(q,qd) - G(q)) - qdd = np.linalg.solve(M, J.T @ wk - tau_rne) + qdd = np.linalg.solve(M, Ja.T @ wk - tau_rne) # xd = Ja qd # xdd = Jad qd + Ja qdd @@ -1262,7 +1407,7 @@ def accel_x( # need Jacobian dot qdk = Ji @ xdk - Jd = self.jacob_dot(qk, qdk, J0=J) + Jd = self.jacob0_dot(qk, qdk, J0=Ja) xdd[k, :] = T @ (Jd @ qdk + J @ qdd) @@ -1271,41 +1416,49 @@ def accel_x( else: return xdd - def itorque(self, q, qdd): + def itorque(self: RobotProto, q, qdd): r""" Inertia torque - :param q: Joint coordinates - :type q: ndarray(n) - :param qdd: Joint acceleration - :type qdd: ndarray(n) - - :return: The inertia torque vector - :rtype: ndarray(n) - ``itorque(q, qdd)`` is the inertia force/torque vector (n) at the specified joint configuration q (n) and acceleration qdd (n), and ``n`` is the number of robot joints. It is :math:`\mathbf{I}(q) \ddot{q}`. - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.itorque(puma.qz, 0.5 * np.ones((6,))) - **Trajectory operation** If ``q`` and ``qdd`` are matrices (m,n), each row is interpretted as a joint configuration, and the result is a matrix (m,n) where each row is the corresponding joint torques. - .. note:: If the robot model contains non-zero motor inertia then this - will be included in the result. + Parameters + ---------- + q + Joint coordinates + qdd + Joint acceleration + + Returns + ------- + itorque + The inertia torque vector + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.DH.Puma560() + >>> puma.itorque(puma.qz, 0.5 * np.ones((6,))) + + Notes + ----- + - If the robot model contains non-zero motor inertia then this + will be included in the result. + + See Also + -------- + :func:`inertia` - :seealso: :func:`inertia` """ q = getmatrix(q, (None, self.n)) @@ -1323,27 +1476,16 @@ def itorque(self, q, qdd): else: return taui - def paycap(self, w, tauR, frame=1, q=None): + def paycap( + self: RobotProto, + w: NDArray, + tauR: NDArray, + frame: int = 1, + q: Union[ArrayLike, None] = None, + ): """ Static payload capacity of a robot - :param w: The payload wrench - :type w: ndarray(n) - :param tauR: Joint torque matrix minimum and maximums - :type tauR: ndarray(n,2) - :param frame: The frame in which to torques are expressed in when J - is not supplied. 'base' means base frame of the robot, 'ee' means - end-effector frame - :type frame: str - :param q: Joint coordinates - :type q: ndarray(n) - - :return: The maximum permissible payload wrench - :rtype: ndarray(6) - :return: Joint index (zero indexed) which hits its - force/torque limit - :rtype: int - ``wmax, joint = paycap(q, w, f, tauR)`` returns the maximum permissible payload wrench ``wmax`` (6) applied at the end-effector, and the index of the joint (zero indexed) which hits its force/torque limit at that @@ -1356,19 +1498,42 @@ def paycap(self, w, tauR, frame=1, q=None): In the case q is nxm then wmax is Mx6 and J is Mx1 where the rows are the results at the pose given by corresponding row of q. - .. note:: - - Wrench vector and Jacobian must be from the same reference frame - - Tool transforms are taken into consideration for frame=1. + Parameters + ---------- + w + The payload wrench + tauR + Joint torque matrix minimum and maximums + frame + The frame in which to torques are expressed in when J + is not supplied. 'base' means base frame of the robot, 'ee' means + end-effector frame + q + Joint coordinates + + Returns + ------- + ndarray(6) + The maximum permissible payload wrench + + Notes + ----- + - Wrench vector and Jacobian must be from the same reference frame + - Tool transforms are taken into consideration for frame=1. + """ + # TODO rewrite trajn = 1 if q is None: q = self.q + else: + q = np.array(q) try: - q = getvector(q, self.n, "row") - w = getvector(w, 6, "row") + q = np.array(getvector(q, self.n, "row")) + w = np.array(getvector(w, 6, "row")) except ValueError: trajn = q.shape[1] verifymatrix(q, (trajn, self.n)) @@ -1412,7 +1577,7 @@ def paycap(self, w, tauR, frame=1, q=None): else: return wmax, joint - def perturb(self, p=0.1): + def perturb(self: RobotProto, p=0.1): """ Perturb robot parameters @@ -1426,11 +1591,15 @@ def perturb(self, p=0.1): schemes. For example to vary parameters in the range +/- 10 percent is: r2 = puma.perturb(0.1) - :param p: The percent (+/-) to be perturbed. Default 10% - :type p: float + Parameters + ---------- + p + The percent (+/-) to be perturbed. Default 10% - :return: A copy of the robot with dynamic parameters perturbed - :rtype: DHRobot + Returns + ------- + DHRobot + A copy of the robot with dynamic parameters perturbed """ @@ -1461,183 +1630,4 @@ def _printProgressBar( import roboticstoolbox as rtb - # from spatialmath.base import symbolic as sym - puma = rtb.models.DH.Puma560() - - # for j, link in enumerate(puma): - # print(f'joint {j:}::') - # print(link.dyn(indent=4)) - # print() - - # tau = puma.rne_dh(puma.qz, puma.qz, puma.qz) - # print(tau) - # tau = puma.rne_dh(np.r_[puma.qz, puma.qz, puma.qz]) - # print(tau) - # tau = puma.rne_dh([0,0,0,0,0,0], [0,0,0,0,0,0], [0,0,0,0,0,0]) - # print(tau) - # tau = puma.rne_dh([0,0,0,0,0,0, 0,0,0,0,0,0, 0,0,0,0,0,0]) - # print(tau) - - # puma = rtb.models.DH.Puma560(symbolic=True) - # print(puma) - # g = sym.symbol('g') - # puma.gravity = [0, 0, g] - # q = sym.symbol('q_:6') - # qd = sym.symbol('qd_:6') - # qdd = sym.symbol('qdd_:6') - - # tau = puma.rne_dh(q, qd, qdd, debug=False) - - # print(tau[0].coeff(qdd[0])) - # print(tau[0].expand().coeff(qdd[0])) - - q = puma.qz - # qd = puma.qz - # qdd = puma.qz - ones = np.ones((6,)) - qd = ones - qdd = ones - - print(puma.rne(q, qd, qdd)) - print(puma.rne_python(q, qd, qdd, debug=False)) - - print(puma.gravity) - print([link.isrevolute() for link in puma]) - - # NOT CONVINCED WE NEED THIS, AND IT'S ORPHAN CODE - # def gravjac(self, q, grav=None): - # """ - # Compute gravity load and Jacobian - - # :param q: The joint configuration of the robot - # :type q: ndarray(n) - # :param grav: The gravity vector (Optional, if not supplied will - # use the stored gravity values). - # :type grav: ndarray(3,) - - # :return tau: The generalised joint force/torques due to gravity - # :rtype tau: ndarray(n,) - - # ``tauB = gravjac(q, grav)`` calculates the generalised joint force/ - # torques due to gravity and the Jacobian - - # Trajectory operation: - # If q is nxm where n is the number of robot joints then a - # trajectory is assumed where each row of q corresponds to a robot - # configuration. tau (nxm) is the generalised joint torque, each row - # corresponding to an input pose, and jacob0 (6xnxm) where each - # plane is a Jacobian corresponding to an input pose. - - # .. note:: - # - The gravity vector is defined by the SerialLink property if not - # explicitly given. - # - Does not use inverse dynamics function RNE. - # - Faster than computing gravity and Jacobian separately. - - # Written by Bryan Moutrie - - # :seealso: :func:`gravload` - # """ - - # # TODO use np.cross instead - # def _cross3(self, a, b): - # c = np.zeros(3) - # c[2] = a[0] * b[1] - a[1] * b[0] - # c[0] = a[1] * b[2] - a[2] * b[1] - # c[1] = a[2] * b[0] - a[0] * b[2] - # return c - - # def makeJ(O, A, e, r): - # J[3:6,:] = A - # for j in range(r): - # if r[j]: - # J[0:3,j] = cross3(A(:,j),e-O(:,j)); - # else: - # J[:,j] = J[[4 5 6 1 2 3],j]; %J(1:3,:) = 0; - - # if grav is None: - # grav = np.copy(self.gravity) - # else: - # grav = getvector(grav, 3) - - # try: - # if q is not None: - # q = getvector(q, self.n, 'col') - # else: - # q = np.copy(self.q) - # q = getvector(q, self.n, 'col') - - # poses = 1 - # except ValueError: - # poses = q.shape[1] - # verifymatrix(q, (self.n, poses)) - - # if not self.mdh: - # baseAxis = self.base.a - # baseOrigin = self.base.t - - # tauB = np.zeros((self.n, poses)) - - # # Forces - # force = np.zeros((3, self.n)) - - # for joint in range(self.n): - # force[:, joint] = np.squeeze(self.links[joint].m * grav) - - # # Centre of masses (local frames) - # r = np.zeros((4, self.n)) - # for joint in range(self.n): - # r[:, joint] = np.r_[np.squeeze(self.links[joint].r), 1] - - # for pose in range(poses): - # com_arr = np.zeros((3, self.n)) - - # T = self.fkine_all(q[:, pose]) - - # jointOrigins = np.zeros((3, self.n)) - # jointAxes = np.zeros((3, self.n)) - # for i in range(self.n): - # jointOrigins[:, i] = T[i].t - # jointAxes[:, i] = T[i].a - - # if not self.mdh: - # jointOrigins = np.c_[ - # baseOrigin, jointOrigins[:, :-1] - # ] - # jointAxes = np.c_[ - # baseAxis, jointAxes[:, :-1] - # ] - - # # Backwards recursion - # for joint in range(self.n - 1, -1, -1): - # # C.o.M. in world frame, homog - # com = T[joint].A @ r[:, joint] - - # # Add it to the distal others - # com_arr[:, joint] = com[0:3] - - # t = np.zeros(3) - - # # for all links distal to it - # for link in range(joint, self.n): - # if not self.links[joint].sigma: - # # Revolute joint - # d = com_arr[:, link] - jointOrigins[:, joint] - # t = t + self._cross3(d, force[:, link]) - # # Though r x F would give the applied torque - # # and not the reaction torque, the gravity - # # vector is nominally in the positive z - # # direction, not negative, hence the force is - # # the reaction force - # else: - # # Prismatic joint - # # Force on prismatic joint - # t = t + force[:, link] - - # tauB[joint, pose] = t.T @ jointAxes[:, joint] - - # if poses == 1: - # return tauB[:, 0] - # else: - # return tauB diff --git a/roboticstoolbox/robot/ERobot.py b/roboticstoolbox/robot/ERobot.py index 145f1da0e..82803caa7 100644 --- a/roboticstoolbox/robot/ERobot.py +++ b/roboticstoolbox/robot/ERobot.py @@ -1,2899 +1,25 @@ #!/usr/bin/env python3 """ -Created on Tue Apr 24 15:48:52 2020 @author: Jesse Haviland """ -from os.path import splitext -import tempfile -import subprocess -import webbrowser -from numpy import ( - array, - ndarray, - isnan, - zeros, - eye, - expand_dims, - empty, - concatenate, - cross, - arccos, - dot, -) -from numpy.linalg import norm as npnorm, inv -from spatialmath import SE3, SE2 -from spatialgeometry import Cylinder -from spatialmath.base.argcheck import getvector, islistof -from roboticstoolbox.robot.Link import Link, Link2, BaseLink -from roboticstoolbox.robot.ETS import ETS, ETS2 -from roboticstoolbox.robot.ET import ET -from roboticstoolbox.robot.DHRobot import DHRobot -from roboticstoolbox.tools import xacro -from roboticstoolbox.tools import URDF -from roboticstoolbox.robot.Robot import Robot -from roboticstoolbox.robot.Gripper import Gripper -from roboticstoolbox.tools.data import rtb_path_to_datafile -from roboticstoolbox.tools.params import rtb_get_param -from pathlib import PurePosixPath -from ansitable import ANSITable, Column -from spatialmath import ( - SpatialAcceleration, - SpatialVelocity, - SpatialInertia, - SpatialForce, -) -from functools import lru_cache -from typing import Union, overload, Dict, List, Tuple, Optional -from copy import deepcopy +from roboticstoolbox.robot.Robot import Robot, Robot2 -ArrayLike = Union[list, ndarray, tuple, set] +class ERobot(Robot): + def __init__(self, *args, **kwargs): -class BaseERobot(Robot): + # warn("ERobot is deprecated, use iscollided instead", FutureWarning) - """ - Construct an ERobot object - :param et_list: List of elementary transforms which represent the robot - kinematics - :type et_list: ET list - :param name: Name of the robot - :type name: str, optional - :param manufacturer: Manufacturer of the robot - :type manufacturer: str, optional - :param base: Location of the base is the world frame - :type base: SE3, optional - :param tool: Offset of the flange of the robot to the end-effector - :type tool: SE3, optional - :param gravity: The gravity vector - :type n: ndarray(3) - An ERobot represents the kinematics of a serial-link manipulator with - one or more branches. - From ETS - -------- - Example: - .. runblock:: pycon - >>> from roboticstoolbox import ETS, ERobot - >>> ets = ETS.rz() * ETS.ry() * ETS.tz(1) * ETS.ry() * ETS.tz(1) - >>> robot = ERobot(ets) - >>> print(robot) - The ETS is partitioned such that a new link frame is created **after** every - joint variable. - From list of Links - ------------------- - Example: - .. runblock:: pycon - >>> from roboticstoolbox import ETS, ERobot - >>> link1 = Link(ETS.rz(), name='link1') - >>> link2 = Link(ETS.ry(), name='link2', parent=link1) - >>> link3 = Link(ETS.tz(1) * ETS.ry(), name='link3', parent=link2) - >>> link4 = Link(ETS.tz(1), name='ee', parent=link3) - >>> robot = ERobot([link1, link2, link3, link4]) - >>> print(robot) - A number of ``Link`` objects are created, each has a transform with - respect to the previous frame, and all except the first link have a parent. - The implicit parent of the first link is the base. - The parent also can be specified as a string, and its name is mapped to the - parent link by name in ``ERobot``. - If no ``parent`` arguments are given it is assumed the links are in - sequential order, and the parent hierarchy will be automatically - established. - .. runblock:: pycon - >>> from roboticstoolbox import ETS, ERobot - >>> robot = ERobot([ - >>> Link(ETS.rz(), name='link1'), - >>> Link(ETS.ry(), name='link2'), - >>> Link(ETS.tz(1) * ETS.ry(), name='link3'), - >>> Link(ETS.tz(1), name='ee') - >>> ]) - >>> print(robot) - Branched robots - --------------- - Example: - .. runblock:: pycon - >>> robot = ERobot([ - >>> Link(ETS.rz(), name='link1'), - >>> Link(ETS.tx(1) * ETS.ty(-0.5) * ETS.rz(), name='link2', parent='link1'), - >>> Link(ETS.tx(1), name='ee_1', parent='link2'), - >>> Link(ETS.tx(1) * ETS.ty(0.5) * ETS.rz(), name='link3', parent='link1'), - >>> Link(ETS.tx(1), name='ee_2', parent='link3') - >>> ]) - >>> print(robot) - :references: - - Kinematic Derivatives using the Elementary Transform Sequence, - J. Haviland and P. Corke - """ # noqa E501 - - def __init__(self, links, gripper_links=None, checkjindex=True, **kwargs): - self._path_cache_fknm = {} - self._path_cache = {} - self._eye_fknm = eye(4) - - self._linkdict = {} - self._n = 0 - self._ee_links = [] - - # Ordered links, we reorder the input elinks to be in depth first - # search order - orlinks = [] - - # check all the incoming Link objects - n = 0 - for k, link in enumerate(links): - # if link has no name, give it one - if link.name is None or link.name == "": - link.name = f"link-{k}" - link.number = k + 1 - - # put it in the link dictionary, check for duplicates - if link.name in self._linkdict: - raise ValueError(f"link name {link.name} is not unique") - self._linkdict[link.name] = link - - if link.isjoint: - n += 1 - - # resolve parents given by name, within the context of - # this set of links - for link in links: - if link.parent is None and link.parent_name is not None: - link._parent = self._linkdict[link.parent_name] - - if all([link.parent is None for link in links]): - # no parent links were given, assume they are sequential - for i in range(len(links) - 1): - links[i + 1]._parent = links[i] - - self._n = n - - # scan for base - for link in links: - # is this a base link? - if link._parent is None: - try: - if self._base_link is not None: - raise ValueError("Multiple base links") - except AttributeError: - pass - - self._base_link = link - else: - # no, update children of this link's parent - link._parent._children.append(link) - - if self.base_link is None: # Pragma: nocover - raise ValueError( - "Invalid link configuration provided, must have a base link" - ) - - # Scene node, set links between the links - for link in links: - if link.parent is not None: - link.scene_parent = link.parent - - # Set up the gripper, make a list containing the root of all - # grippers - if gripper_links is not None: - if isinstance(gripper_links, Link): - gripper_links = [gripper_links] - else: - gripper_links = [] - - # An empty list to hold all grippers - self._grippers = [] - - # Make a gripper object for each gripper - for link in gripper_links: - g_links = self.dfs_links(link) - - # Remove gripper links from the robot - for g_link in g_links: - # print(g_link) - links.remove(g_link) - - # Save the gripper object - self._grippers.append(Gripper(g_links, name=link.name)) - - # Subtract the n of the grippers from the n of the robot - for gripper in self._grippers: - self._n -= gripper.n - - # Set the ee links - self.ee_links = [] - if len(gripper_links) == 0: - for link in links: - # is this a leaf node? and do we not have any grippers - if len(link.children) == 0: - # no children, must be an end-effector - self.ee_links.append(link) - else: - for link in gripper_links: - # use the passed in value - self.ee_links.append(link.parent) # type: ignore - - # assign the joint indices - if all([link.jindex is None or link.ets._auto_jindex for link in links]): - # no joints have an index - jindex = [0] # "mutable integer" hack - - def visit_link(link, jindex): - # if it's a joint, assign it a jindex and increment it - if link.isjoint and link in links: - link.jindex = jindex[0] - jindex[0] += 1 - - if link in links: - orlinks.append(link) - - # visit all links in DFS order - self.dfs_links(self.base_link, lambda link: visit_link(link, jindex)) - - elif all([link.jindex is not None for link in links if link.isjoint]): - # jindex set on all, check they are unique and contiguous - if checkjindex: - jset = set(range(self._n)) - for link in links: - if link.isjoint and link.jindex not in jset: - raise ValueError( - f"joint index {link.jindex} was " "repeated or out of range" - ) - jset -= set([link.jindex]) - if len(jset) > 0: # pragma nocover # is impossible - raise ValueError(f"joints {jset} were not assigned") - orlinks = links - else: - # must be a mixture of Links with/without jindex - raise ValueError("all links must have a jindex, or none have a jindex") - - # self._nbranches = sum([link.nchildren == 0 for link in links]) - - # Set up qlim - qlim = zeros((2, self.n)) - j = 0 - - for i in range(len(orlinks)): - if orlinks[i].isjoint: - qlim[:, j] = orlinks[i].qlim - j += 1 - self._qlim = qlim - - self._valid_qlim = False - for i in range(self.n): - if any(qlim[:, i] != 0) and not any(isnan(qlim[:, i])): - self._valid_qlim = True - - # Initialise Robot object - super().__init__(orlinks, **kwargs) - - # Fix number of links for gripper links - self._nlinks = len(links) - - for gripper in self.grippers: - self._nlinks += len(gripper.links) - - # SceneNode, set a reference to the first link - self.scene_children = [self.links[0]] # type: ignore - - def __str__(self) -> str: - """ - Pretty prints the ETS Model of the robot. - :return: Pretty print of the robot model - :rtype: str - .. note:: - - Constant links are shown in blue. - - End-effector links are prefixed with an @ - - Angles in degrees - - The robot base frame is denoted as ``BASE`` and is equal to the - robot's ``base`` attribute. - """ - unicode = rtb_get_param("unicode") - border = "thin" if unicode else "ascii" - - table = ANSITable( - Column("link", headalign="^", colalign=">"), - Column("link", headalign="^", colalign="<"), - Column("joint", headalign="^", colalign=">"), - Column("parent", headalign="^", colalign="<"), - Column("ETS: parent to link", headalign="^", colalign="<"), - border=border, - ) - - for k, link in enumerate(self.links): - color = "" if link.isjoint else "<>" - ee = "@" if link in self.ee_links else "" - ets = link.ets - if link.parent is None: - parent_name = "BASE" - else: - parent_name = link.parent.name - s = ets.__str__(f"q{link.jindex}") - # if len(s) > 0: - # op = " \u2295 " if unicode else " * " # \oplus - # s = op + s - - if link.isjoint: - jname = link.jindex - else: - jname = "" - table.row( - # link.jindex, - k, - color + ee + link.name, - jname, - parent_name, - f"{s}", - ) - - if isinstance(self, ERobot2): - classname = "ERobot2" - else: - classname = "ERobot" - - s = f"{classname}: {self.name}" - if self.manufacturer is not None and len(self.manufacturer) > 0: - s += f" (by {self.manufacturer})" - s += f", {self.n} joints ({self.structure})" - if len(self.grippers) > 0: - s += ( - f", {len(self.grippers)} gripper{'s' if len(self.grippers) > 1 else ''}" - ) - if self.nbranches > 1: - s += f", {self.nbranches} branches" - if self._hasdynamics: - s += ", dynamics" - if any([len(link.geometry) > 0 for link in self.links]): - s += ", geometry" - if any([len(link.collision) > 0 for link in self.links]): - s += ", collision" - s += "\n" - - s += str(table) - s += self.configurations_str(border=border) - - return s - - @overload - def __getitem__(self: "ERobot", i: Union[int, str]) -> Link: - ... - - @overload - def __getitem__(self: "ERobot", i: slice) -> List[Link]: - ... - - @overload - def __getitem__(self: "ERobot2", i: Union[int, str]) -> Link2: - ... - - @overload - def __getitem__(self: "ERobot2", i: slice) -> List[Link2]: - ... - - def __getitem__(self, i): - """ - Get link - - :param i: link number or name - :type i: int, slice or str - :return: i'th link or named link - :rtype: Link - - This also supports iterating over each link in the robot object, - from the base to the tool. - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.Panda() - >>> print(robot[1]) # print the 2nd link - >>> print([link.a for link in robot]) # print all the a_j values - - .. note:: ``ERobot`` supports link lookup by name, - eg. ``robot['link1']`` - """ - if isinstance(i, str): - try: - return self.link_dict[i] - except KeyError: - raise KeyError(f"link {i} not in link dictionary") - except AttributeError: - raise AttributeError(f"robot has no link dictionary") - else: - return self._links[i] - - # --------------------------------------------------------------------- # - - @overload - def links(self: "ERobot") -> List[Link]: - ... - - @overload - def links(self: "ERobot2") -> List[Link2]: - ... - - @property - def links(self) -> List[Link]: - """ - Robot links - - :return: A list of link objects - """ - return self._links - - # --------------------------------------------------------------------- # - - @property - def n(self) -> int: - """ - Number of joints - :return: number of variable joint in the robot's kinematic tree - :rtype: int - The sum of the number of revolute and prismatic joints. - """ - return self._n - - # --------------------------------------------------------------------- # - - @property - def grippers(self) -> List[Gripper]: - """ - Grippers attached to the robot - - :return: A list of grippers - - """ - return self._grippers - - # --------------------------------------------------------------------- # - @property - def nbranches(self) -> int: - """ - Number of branches - - :return: number of branches in the robot's kinematic tree - :rtype: int - - Number of branches in this robot. Computed as the number of links with - zero children - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.ETS.Panda() - >>> robot.nbranches - - :seealso: :func:`n`, :func:`nlinks` - """ - # return self._nbranches - return sum([link.nchildren == 0 for link in self.links]) + len(self.grippers) - - # --------------------------------------------------------------------- # - - @overload - def elinks(self: "ERobot") -> List[Link]: - ... - - @overload - def elinks(self: "ERobot2") -> List[Link2]: - ... - - @property - def elinks(self) -> List[Link]: - return self._links - - # --------------------------------------------------------------------- # - - @overload - def link_dict(self: "ERobot") -> Dict[str, Link]: - ... - - @overload - def link_dict(self: "ERobot2") -> Dict[str, Link2]: - ... - - @property - def link_dict(self) -> Dict[str, Link]: - return self._linkdict - - # --------------------------------------------------------------------- # - - @overload - def base_link(self: "ERobot") -> Link: - ... - - @overload - def base_link(self: "ERobot2") -> Link2: - ... - - @property - def base_link(self) -> Link: - return self._base_link - - @base_link.setter - def base_link(self, link): - if isinstance(link, Link): - self._base_link = link - else: - raise TypeError("Must be a Link") - - # --------------------------------------------------------------------- # - - @overload - def ee_links(self: "ERobot2") -> List[Link2]: - ... - - @overload - def ee_links(self: "ERobot") -> List[Link]: - ... - - @property - def ee_links(self) -> List[Link]: - return self._ee_links - - @ee_links.setter - def ee_links(self, link: Union[List[Link], Link]): - if isinstance(link, Link): - self._ee_links = [link] - elif isinstance(link, list) and all([isinstance(x, Link) for x in link]): - self._ee_links = link - else: - raise TypeError("expecting a Link or list of Links") - - # --------------------------------------------------------------------- # - - @property - def reach(self) -> float: - r""" - Reach of the robot - :return: Maximum reach of the robot - :rtype: float - A conservative estimate of the reach of the robot. It is computed as - the sum of the translational ETs that define the link transform. - .. note:: - - Probably an overestimate of reach - - Used by numerical inverse kinematics to scale translational - error. - - For a prismatic joint, uses ``qlim`` if it is set - .. warning:: Computed on the first access. If kinematic parameters - subsequently change this will not be reflected. - """ - if self._reach is None: - d_all = [] - for link in self.ee_links: - d = 0 - while True: - for et in link.ets: - if et.istranslation: - if et.isjoint: - # the length of a prismatic joint depends on the - # joint limits. They might be set in the ET - # or in the Link depending on how the robot - # was constructed - if link.qlim is not None: - d += max(link.qlim) - elif et.qlim is not None: - d += max(et.qlim) - else: - d += abs(et.eta) - link = link.parent - if link is None or isinstance(link, str): - d_all.append(d) - break - - self._reach = max(d_all) - return self._reach - - # --------------------------------------------------------------------- # - - def hierarchy(self): - """ - Pretty print the robot link hierachy - :return: Pretty print of the robot model - :rtype: str - Example: - .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.URDF.Panda() - >>> robot.hierarchy() - """ - - def recurse(link, indent=0): - print(" " * indent * 2, link.name) - for child in link.child: - recurse(child, indent + 1) - - recurse(self.base_link) - - # --------------------------------------------------------------------- # - - def _find_ets(self, start, end, explored, path) -> Union[ETS, None]: - """ - Privade method which will recursively find the ETS of a path - see ets() - """ - - link = self._getlink(start, self.base_link) - end = self._getlink(end, self.ee_links[0]) - - toplevel = path is None - explored.add(link) - - if link == end: - return path - - # unlike regular DFS, the neighbours of the node are its children - # and its parent. - - # visit child nodes below start - if toplevel: - path = link.ets - - if link.children is not None: - for child in link.children: - if child not in explored: - p = self._find_ets(child, end, explored, path * child.ets) - if p is not None: - return p - - # we didn't find the node below, keep going up a level, and recursing - # down again - if toplevel: - path = None - if link.parent is not None: - parent = link.parent # go up one level toward the root - if parent not in explored: - if path is None: - p = self._find_ets(parent, end, explored, link.ets.inv()) - else: - p = self._find_ets(parent, end, explored, path * link.ets.inv()) - if p is not None: - return p - - def _gripper_ets(self, gripper: Gripper) -> ETS: - """ - Privade method which will find the ETS of a gripper - """ - # return gripper.links[0].ets * ET.SE3(gripper.tool) - return ETS(ET.SE3(gripper.tool)) - - @lru_cache(maxsize=32) - def ets( - self, - start: Union[Link, Gripper, str, None] = None, - end: Union[Link, Gripper, str, None] = None, - ) -> ETS: - """ - ERobot to ETS - - :param start: start of path, defaults to ``base_link`` - :type start: Link or str, optional - :param end: end of path, defaults to end-effector - :type end: Link or str, optional - :raises ValueError: a link does not belong to this ERobot - :raises TypeError: a bad link argument - :return: elementary transform sequence - :rtype: ETS instance - - - - ``robot.ets()`` is an ETS representing the kinematics from base to - end-effector. - - ``robot.ets(end=link)`` is an ETS representing the kinematics from - base to the link ``link`` specified as a Link reference or a name. - - ``robot.ets(start=l1, end=l2)`` is an ETS representing the kinematics - from link ``l1`` to link ``l2``. - - .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> panda = rtb.models.ETS.Panda() - >>> panda.ets() - """ - - # ets to stand and end incase of grippers - ets_init = None - ets_end = None - - if isinstance(start, Gripper): - ets_init = self._gripper_ets(start).inv() - link = start.links[0].parent - if link is None: # pragma: nocover - raise ValueError("Invalid robot link configuration") - else: - link = self._getlink(start, self.base_link) - - if end is None: - if len(self.grippers) > 1: - end_link = self.grippers[0].links[0] - ets_end = self._gripper_ets(self.grippers[0]) - print("multiple grippers present, ambiguous, using self.grippers[0]") - elif len(self.grippers) == 1: - end_link = self.grippers[0].links[0] - ets_end = self._gripper_ets(self.grippers[0]) - elif len(self.grippers) > 1: - end_link = self._getlink(end, self.ee_links[0]) - print( - "multiple end-effectors present, ambiguous, using self.ee_links[0]" - ) - else: - end_link = self._getlink(end, self.ee_links[0]) - else: - if isinstance(end, Gripper): - ets_end = self._gripper_ets(end) - end_link = end.links[0].parent # type: ignore - if end_link is None: # pragma: nocover - raise ValueError("Invalid robot link configuration") - else: - end_link = self._getlink(end, self.ee_links[0]) - - explored = set() - - if link is end_link: - ets = link.ets - else: - ets = self._find_ets(link, end_link, explored, path=None) - - if ets is None: - raise ValueError("Could not find the requested ETS in this robot") - - if ets_init is not None: - ets = ets_init * ets - - if ets_end is not None: - ets = ets * ets_end - - return ets - - # --------------------------------------------------------------------- # - - def segments(self) -> List[List[Union[Link, None]]]: - """ - Segments of branched robot - - :return: Segment list - :rtype: list of lists of Link - - For a single-chain robot with structure:: - - L1 - L2 - L3 - - the return is ``[[None, L1, L2, L3]]`` - - For a robot with structure:: - - L1 - L2 +- L3 - L4 - +- L5 - L6 - - the return is ``[[None, L1, L2], [L2, L3, L4], [L2, L5, L6]]`` - - .. note:: - - the length of the list is the number of segments in the robot - - the first segment always starts with ``None`` which represents - the base transform (since there is no base link) - - the last link of one segment is also the first link of subsequent - segments - """ - - def recurse(link: Link): - - segs = [link.parent] - while True: - segs.append(link) - if link.nchildren == 0: - return [segs] - elif link.nchildren == 1: - link = link.children[0] # type: ignore - continue - elif link.nchildren > 1: - segs = [segs] - - for child in link.children: # type: ignore - segs.extend(recurse(child)) - - return segs - - return recurse(self.links[0]) # type: ignore - - # --------------------------------------------------------------------- # - - def fkine_all(self, q): - """ - Compute the pose of every link frame - - :param q: The joint configuration - :type q: darray(n) - :return: Pose of all links - :rtype: SE3 instance - - ``T = robot.fkine_all(q)`` is an SE3 instance with ``robot.nlinks + - 1`` values: - - - ``T[0]`` is the base transform - - ``T[i]`` is the pose of link whose ``number`` is ``i`` - - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ - q = getvector(q) - - if isinstance(self, ERobot): - Tbase = SE3(self.base) # add base, also sets the type - else: - Tbase = SE2(self.base) # add base, also sets the type - - linkframes = Tbase.__class__.Alloc(self.nlinks + 1) - linkframes[0] = Tbase - - def recurse(Tall, Tparent, q, link): - # if joint?? - T = Tparent - while True: - if isinstance(self, ERobot): - T *= SE3(link.A(q[link.jindex])) - else: - T *= SE2(link.A(q[link.jindex])) - - Tall[link.number] = T - - if link.nchildren == 0: - # no children - return - elif link.nchildren == 1: - # one child - if link in self.ee_links: - # this link is an end-effector, go no further - return - link = link.children[0] - continue - else: - # multiple children - for child in link.children: - recurse(Tall, T, q, child) - return - - recurse(linkframes, Tbase, q, self.links[0]) - - return linkframes - - # --------------------------------------------------------------------- # - - def showgraph(self, **kwargs): - """ - Display a link transform graph in browser - :param etsbox: Put the link ETS in a box, otherwise an edge label - :type etsbox: bool - :param jtype: Arrowhead to node indicates revolute or prismatic type - :type jtype: bool - :param static: Show static joints in blue and bold - :type static: bool - ``robot.showgraph()`` displays a graph of the robot's link frames - and the ETS between them. It uses GraphViz dot. - The nodes are: - - Base is shown as a grey square. This is the world frame origin, - but can be changed using the ``base`` attribute of the robot. - - Link frames are indicated by circles - - ETS transforms are indicated by rounded boxes - The edges are: - - an arrow if `jtype` is False or the joint is fixed - - an arrow with a round head if `jtype` is True and the joint is - revolute - - an arrow with a box head if `jtype` is True and the joint is - prismatic - Edge labels or nodes in blue have a fixed transformation to the - preceding link. - Example:: - >>> import roboticstoolbox as rtb - >>> panda = rtb.models.URDF.Panda() - >>> panda.showgraph() - .. image:: ../figs/panda-graph.svg - :width: 600 - :seealso: :func:`dotfile` - """ - - # create the temporary dotfile - dotfile = tempfile.TemporaryFile(mode="w") - self.dotfile(dotfile, **kwargs) - - # rewind the dot file, create PDF file in the filesystem, run dot - dotfile.seek(0) - pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False) - subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile) - - # open the PDF file in browser (hopefully portable), then cleanup - webbrowser.open(f"file://{pdffile.name}") - - # --------------------------------------------------------------------- # - - def dotfile(self, filename, etsbox=False, ets="full", jtype=False, static=True): - """ - Write a link transform graph as a GraphViz dot file - :param file: Name of file to write to - :type file: str or file - :param etsbox: Put the link ETS in a box, otherwise an edge label - :type etsbox: bool - :param jtype: Arrowhead to node indicates revolute or prismatic type - :type jtype: bool - :param static: Show static joints in blue and bold - :type static: bool - The file can be processed using dot:: - % dot -Tpng -o out.png dotfile.dot - The nodes are: - - Base is shown as a grey square. This is the world frame origin, - but can be changed using the ``base`` attribute of the robot. - - Link frames are indicated by circles - - ETS transforms are indicated by rounded boxes - The edges are: - - an arrow if `jtype` is False or the joint is fixed - - an arrow with a round head if `jtype` is True and the joint is - revolute - - an arrow with a box head if `jtype` is True and the joint is - prismatic - Edge labels or nodes in blue have a fixed transformation to the - preceding link. - .. note:: If ``filename`` is a file object then the file will *not* - be closed after the GraphViz model is written. - :seealso: :func:`showgraph` - """ - if isinstance(filename, str): - file = open(filename, "w") - else: - file = filename - - header = r"""digraph G { -graph [rankdir=LR]; -""" - - def draw_edge(link, etsbox, jtype, static): - # draw the edge - if jtype: - if link.isprismatic: - edge_options = 'arrowhead="box", arrowtail="inv", dir="both"' - elif link.isrevolute: - edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"' - else: - edge_options = 'arrowhead="normal"' - else: - edge_options = 'arrowhead="normal"' - - if link.parent is None: - parent = "BASE" - else: - parent = link.parent.name - - if etsbox: - # put the ets fragment in a box - if not link.isjoint and static: - node_options = ', fontcolor="blue"' - else: - node_options = "" - file.write( - " {}_ets [shape=box, style=rounded, " - 'label="{}"{}];\n'.format( - link.name, link.ets.__str__(q=f"q{link.jindex}"), node_options - ) - ) - file.write(" {} -> {}_ets;\n".format(parent, link.name)) - file.write( - " {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options) - ) - else: - # put the ets fragment as an edge label - if not link.isjoint and static: - edge_options += 'fontcolor="blue"' - if ets == "full": - estr = link.ets.__str__(q=f"q{link.jindex}") - elif ets == "brief": - if link.jindex is None: - estr = "" - else: - estr = f"...q{link.jindex}" - else: - return - file.write( - ' {} -> {} [label="{}", {}];\n'.format( - parent, - link.name, - estr, - edge_options, - ) - ) - - file.write(header) - - # add the base link - file.write(" BASE [shape=square, style=filled, fillcolor=gray]\n") - - # add the links - for link in self: - # draw the link frame node (circle) or ee node (doublecircle) - if link in self.ee_links: - # end-effector - node_options = 'shape="doublecircle", color="blue", fontcolor="blue"' - else: - node_options = 'shape="circle"' - - file.write(" {} [{}];\n".format(link.name, node_options)) - - draw_edge(link, etsbox, jtype, static) - - for gripper in self.grippers: - for link in gripper.links: - file.write(" {} [shape=cds];\n".format(link.name)) - draw_edge(link, etsbox, jtype, static) - - file.write("}\n") - - if isinstance(filename, str): - file.close() # noqa - - # --------------------------------------------------------------------- # - - def dfs_links(self, start, func=None): - """ - Visit all links from start in depth-first order and will apply - func to each visited link - :param start: the link to start at - :type start: Link - :param func: An optional function to apply to each link as it is found - :type func: function - :returns: A list of links - :rtype: list of Link - """ - visited = [] - - def vis_children(link): - visited.append(link) - if func is not None: - func(link) - - for li in link.children: - if li not in visited: - vis_children(li) - - vis_children(start) - - return visited - - def _get_limit_links( - self, - end: Union[Gripper, Link, str, None] = None, - start: Union[Gripper, Link, str, None] = None, - ) -> Tuple[Link, Union[Link, Gripper], Union[None, SE3]]: - """ - Get and validate an end-effector, and a base link - :param end: end-effector or gripper to compute forward kinematics to - :type end: str or Link or Gripper, optional - :param start: name or reference to a base link, defaults to None - :type start: str or Link, optional - :raises ValueError: link not known or ambiguous - :raises ValueError: [description] - :raises TypeError: unknown type provided - :return: end-effector link, base link, and tool transform of gripper - if applicable - :rtype: Link, Elink, SE3 or None - Helper method to find or validate an end-effector and base link. - """ - - # Try cache - # if self._cache_end is not None: - # return self._cache_end, self._cache_start, self._cache_end_tool - - tool = None - if end is None: - - if len(self.grippers) > 1: - end_ret = self.grippers[0].links[0] - tool = self.grippers[0].tool - if len(self.grippers) > 1: - # Warn user: more than one gripper - print("More than one gripper present, using robot.grippers[0]") - elif len(self.grippers) == 1: - end_ret = self.grippers[0].links[0] - tool = self.grippers[0].tool - - # no grippers, use ee link if just one - elif len(self.ee_links) > 1: - end_ret = self.ee_links[0] - if len(self.ee_links) > 1: - # Warn user: more than one EE - print("More than one end-effector present, using robot.ee_links[0]") - else: - end_ret = self.ee_links[0] - - # Cache result - self._cache_end = end - self._cache_end_tool = tool - else: - - # Check if end corresponds to gripper - for gripper in self.grippers: - if end == gripper or end == gripper.name: - tool = gripper.tool - # end_ret = gripper.links[0] - - # otherwise check for end in the links - end_ret = self._getlink(end) - - if start is None: - start_ret = self.base_link - - # Cache result - self._cache_start = start - else: - # start effector is specified - start_ret = self._getlink(start) - - return end_ret, start_ret, tool - - def _getlink( - self, - link: Union[Link, Gripper, str, None], - default: Union[Link, Gripper, str, None] = None, - ) -> Link: - """ - Validate reference to Link - - :param link: link - - :raises ValueError: link does not belong to this ERobot - :raises TypeError: bad argument - - :return: link reference - - ``robot._getlink(link)`` is a validated reference to a Link within - the ERobot ``robot``. If ``link`` is: - - - an ``Link`` reference it is validated as belonging to - ``robot``. - - a string, then it looked up in the robot's link name dictionary, and - a Link reference returned. - """ - if link is None: - link = default - - if isinstance(link, str): - if link in self.link_dict: - return self.link_dict[link] - - raise ValueError(f"no link named {link}") - - elif isinstance(link, BaseLink): - if link in self.links: - return link - else: - for gripper in self.grippers: - if link in gripper.links: - return link - - raise ValueError("link not in robot links") - elif isinstance(link, Gripper): - for gripper in self.grippers: - if link is gripper: - return gripper.links[0] - - raise ValueError("Gripper not in robot") - - else: - raise TypeError("unknown argument") + super().__init__(*args, **kwargs) # =========================================================================== # -class ERobot(BaseERobot): - def __init__(self, arg, urdf_string=None, urdf_filepath=None, **kwargs): - - if isinstance(arg, ERobot): - # We're passed an ERobot, clone it - # We need to preserve the parent link as we copy - - # Copy each link within the robot - links = [deepcopy(link) for link in arg.links] - gripper_links = [] - - for gripper in arg.grippers: - glinks = [] - for link in gripper.links: - glinks.append(deepcopy(link)) - - gripper_links.append(glinks[0]) - links = links + glinks - - # print(links[9] is gripper_links[0]) - # print(gripper_links) - - # Sever parent connection, but save the string - # The constructor will piece this together for us - for link in links: - link._children = [] - if link.parent is not None: - link._parent_name = link.parent.name - link._parent = None - - # gripper_parents = [] - - # # Make a list of old gripper links - # for gripper in arg.grippers: - # gripper_parents.append(gripper.links[0].name) - - # gripper_links = [] - - # def dfs(node, node_copy): - # for child in node.children: - # child_copy = child.copy(node_copy) - # links.append(child_copy) - - # # If this link was a gripper link, add to the list - # if child_copy.name in gripper_parents: - # gripper_links.append(child_copy) - - # dfs(child, child_copy) - - # link0 = arg.links[0] - # links.append(arg.links[0].copy()) - # dfs(link0, links[0]) - - # print(gripper_links[0].jindex) - - super().__init__(links, gripper_links=gripper_links, **kwargs) - - for i, gripper in enumerate(self.grippers): - gripper.tool = arg.grippers[i].tool.copy() - - # if arg.qdlim is not None: - # self.qdlim = arg.qdlim - - self._urdf_string = arg.urdf_string - self._urdf_filepath = arg.urdf_filepath - - else: - self._urdf_string = urdf_string - self._urdf_filepath = urdf_filepath - - if isinstance(arg, DHRobot): - # we're passed a DHRobot object - # TODO handle dynamic parameters if given - arg = arg.ets - - if isinstance(arg, ETS): - # we're passed an ETS string - links = [] - # chop it up into segments, a link frame after every joint - parent = None - for j, ets_j in enumerate(arg.split()): - elink = Link(ETS(ets_j), parent=parent, name=f"link{j:d}") - if ( - elink.qlim is None - and elink.v is not None - and elink.v.qlim is not None - ): - elink.qlim = elink.v.qlim - parent = elink - links.append(elink) - - elif islistof(arg, Link): - links = arg - - else: - raise TypeError("constructor argument must be ETS or list of Link") - - super().__init__(links, **kwargs) - - @classmethod - def URDF(cls, file_path, gripper=None): - """ - Construct an ERobot object from URDF file - :param file_path: [description] - :type file_path: [type] - :param gripper: index or name of the gripper link(s) - :type gripper: int or str or list - :return: [description] - :rtype: [type] - If ``gripper`` is specified, links from that link outward are removed - from the rigid-body tree and folded into a ``Gripper`` object. - """ - links, name, _, _ = ERobot.URDF_read(file_path) - - if gripper is not None: - if isinstance(gripper, int): - gripper = links[gripper] - elif isinstance(gripper, str): - for link in links: - if link.name == gripper: - gripper = link - break - else: - raise ValueError(f"no link named {gripper}") - else: - raise TypeError("bad argument passed as gripper") - - links, name, urdf_string, urdf_filepath = ERobot.URDF_read(file_path) - print(cls) - return cls( - links, - name=name, - gripper_links=gripper, - urdf_string=urdf_string, - urdf_filepath=urdf_filepath, - ) - - @property - def urdf_string(self): - return self._urdf_string - - @property - def urdf_filepath(self): - return self._urdf_filepath - - # --------------------------------------------------------------------- # - - def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0): - - # self._set_link_fk(self.q) - - ob = [] - - for link in self.links: - - if robot_alpha > 0: - for gi in link.geometry: - gi.set_alpha(robot_alpha) - ob.append(gi.to_dict()) - if collision_alpha > 0: - for gi in link.collision: - gi.set_alpha(collision_alpha) - ob.append(gi.to_dict()) - - # Do the grippers now - for gripper in self.grippers: - for link in gripper.links: - - if robot_alpha > 0: - for gi in link.geometry: - gi.set_alpha(robot_alpha) - ob.append(gi.to_dict()) - if collision_alpha > 0: - for gi in link.collision: - gi.set_alpha(collision_alpha) - ob.append(gi.to_dict()) - - # for o in ob: - # print(o) - - return ob - - def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0): - ob = [] - - # Do the robot - for link in self.links: - - if robot_alpha > 0: - for gi in link.geometry: - ob.append(gi.fk_dict()) - if collision_alpha > 0: - for gi in link.collision: - ob.append(gi.fk_dict()) - - # Do the grippers now - for gripper in self.grippers: - for link in gripper.links: - if robot_alpha > 0: - for gi in link.geometry: - ob.append(gi.fk_dict()) - if collision_alpha > 0: - for gi in link.collision: - ob.append(gi.fk_dict()) - - return ob - - # --------------------------------------------------------------------- # - - @staticmethod - def URDF_read(file_path, tld=None, xacro_tld=None): - """ - Read a URDF file as Links - :param file_path: File path relative to the xacro folder - :type file_path: str, in Posix file path fprmat - :param tld: A custom top-level directory which holds the xacro data, - defaults to None - :type tld: str, optional - :param xacro_tld: A custom top-level within the xacro data, - defaults to None - :type xacro_tld: str, optional - :return: Links and robot name - :rtype: tuple(Link list, str) - File should be specified relative to ``RTBDATA/URDF/xacro`` - - .. note:: If ``tld`` is not supplied, filepath pointing to xacro data should - be directly under ``RTBDATA/URDF/xacro`` OR under ``./xacro`` relative - to the model file calling this method. If ``tld`` is supplied, then - ```file_path``` needs to be relative to ``tld`` - """ - - # get the path to the class that defines the robot - if tld is None: - base_path = rtb_path_to_datafile("xacro") - else: - base_path = PurePosixPath(tld) - # print("*** urdf_to_ets_args: ", classpath) - # add on relative path to get to the URDF or xacro file - # base_path = PurePath(classpath).parent.parent / 'URDF' / 'xacro' - file_path = base_path / PurePosixPath(file_path) - name, ext = splitext(file_path) - - if ext == ".xacro": - # it's a xacro file, preprocess it - if xacro_tld is not None: - xacro_tld = base_path / PurePosixPath(xacro_tld) - urdf_string = xacro.main(file_path, xacro_tld) - try: - urdf = URDF.loadstr(urdf_string, file_path, base_path) - except BaseException as e: - print("error parsing URDF file", file_path) - raise e - else: # pragma nocover - urdf_string = open(file_path).read() - urdf = URDF.loadstr(urdf_string, file_path, base_path) - - return urdf.elinks, urdf.name, urdf_string, file_path - - # --------------------------------------------------------------------- # - - def get_path(self, end=None, start=None): - """ - Find a path from start to end. The end must come after - the start (ie end must be further away from the base link - of the robot than start) in the kinematic chain and both links - must be a part of the same branch within the robot structure. This - method is a work in progress while an approach which generalises - to all applications is designed. - :param end: end-effector or gripper to compute forward kinematics to - :type end: str or Link or Gripper, optional - :param start: name or reference to a base link, defaults to None - :type start: str or Link, optional - :raises ValueError: link not known or ambiguous - :return: the path from start to end - :rtype: list of Link - """ - path = [] - n = 0 - - end, start, tool = self._get_limit_links(end=end, start=start) - - # This is way faster than doing if x in y method - try: - return self._path_cache[start.name][end.name] - except KeyError: - pass - - if start.name not in self._path_cache: - self._path_cache[start.name] = {} - # self._path_cache_fknm[start.name] = {} - - link = end - - path.append(link) - if link.isjoint: - n += 1 - - while link != start: - link = link.parent - if link is None: - raise ValueError( - f"cannot find path from {start.name} to" f" {end.name}" - ) - path.append(link) - if link.isjoint: - n += 1 - - path.reverse() - # path_fknm = [x._fknm for x in path] - - if tool is None: - tool = SE3() - - self._path_cache[start.name][end.name] = (path, n, tool) - # self._path_cache_fknm[start.name][end.name] = (path_fknm, n, tool.A) - - return path, n, tool - - def fkine( - self, - q: ArrayLike, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - tool: Union[ndarray, SE3, None] = None, - include_base: bool = True, - ) -> SE3: - """ - Forward kinematics - - :param q: Joint coordinates - :type q: ArrayLike - :param end: end-effector or gripper to compute forward kinematics to - :param start: the link to compute forward kinematics from - :param tool: tool transform, optional - - :return: The transformation matrix representing the pose of the - end-effector - - - ``T = robot.fkine(q)`` evaluates forward kinematics for the robot at - joint configuration ``q``. - **Trajectory operation**: - - If ``q`` has multiple rows (mxn), it is considered a trajectory and the - result is an ``SE3`` instance with ``m`` values. - .. note:: - - For a robot with a single end-effector there is no need to - specify ``end`` - - For a robot with multiple end-effectors, the ``end`` must - be specified. - - The robot's base tool transform, if set, is incorporated - into the result. - - A tool transform, if provided, is incorporated into the result. - - Works from the end-effector link to the base - - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ - return SE3( - self.ets(start, end).fkine( - q, base=self._T, tool=tool, include_base=include_base - ), - check=False, - ) - - def jacob0( - self, - q: ArrayLike, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: - r""" - Manipulator geometric Jacobian in the base frame - - :param q: Joint coordinate vector - :type q: ArrayLike - :param end: the particular link or gripper whose velocity the Jacobian - describes, defaults to the end-effector if only one is present - :param start: the link considered as the base frame, defaults to the robots's base frame - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return J: Manipulator Jacobian in the base frame - - - ``robot.jacobo(q)`` is the manipulator Jacobian matrix which maps - joint velocity to end-effector spatial velocity expressed in the - end-effector frame. - - End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` - is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. - - Example: - .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.ETS.Puma560() - >>> puma.jacobe([0, 0, 0, 0, 0, 0]) - - .. warning:: This is the geometric Jacobian as described in texts by - Corke, Spong etal., Siciliano etal. The end-effector velocity is - described in terms of translational and angular velocity, not a - velocity twist as per the text by Lynch & Park. - - .. warning:: ``start`` and ``end`` must be on the same branch, - with ``start`` closest to the base. - """ # noqa - return self.ets(start, end).jacob0(q, tool=tool) - - def jacobe( - self, - q: ArrayLike, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: - r""" - Manipulator geometric Jacobian in the end-effector frame - - :param q: Joint coordinate vector - :type q: ArrayLike - :param end: the particular link or Gripper whose velocity the Jacobian - describes, defaults to the end-effector if only one is present - :param start: the link considered as the base frame, defaults to the robots's base frame - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return J: Manipulator Jacobian in the end-effector frame - - - ``robot.jacobe(q)`` is the manipulator Jacobian matrix which maps - joint velocity to end-effector spatial velocity expressed in the - end-effector frame. - End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` - is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. - - Example: - .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.ETS.Puma560() - >>> puma.jacobe([0, 0, 0, 0, 0, 0]) - - .. warning:: This is the **geometric Jacobian** as described in texts by - Corke, Spong etal., Siciliano etal. The end-effector velocity is - described in terms of translational and angular velocity, not a - velocity twist as per the text by Lynch & Park. - - .. warning:: ``start`` and ``end`` must be on the same branch, - with ``start`` closest to the base. - """ # noqa - return self.ets(start, end).jacobe(q, tool=tool) - - def hessian0( - self, - q: Union[ArrayLike, None] = None, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - J0: Union[ndarray, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: - r""" - Manipulator Hessian - - The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the world-coordinate frame. This - function calulcates this based on the ETS of the robot. One of J0 or q - is required. Supply J0 if already calculated to save computation time - - :param q: The joint angles/configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: ArrayLike - :param end: the final link/Gripper which the Hessian represents - :param start: the first link which the Hessian represents - :param J0: The manipulator Jacobian in the 0 frame - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return: The manipulator Hessian in 0 frame - - This method computes the manipulator Hessian in the base frame. If - we take the time derivative of the differential kinematic relationship - .. math:: - \nu &= \mat{J}(\vec{q}) \dvec{q} \\ - \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q} - where - .. math:: - \dmat{J} = \mat{H} \dvec{q} - and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the - Hessian tensor. - - The elements of the Hessian are - .. math:: - \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} - where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements - of the spatial velocity vector. - Similarly, we can write - .. math:: - \mat{J}_{i,j} = \frac{d u_i}{d q_j} - - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ - return self.ets(start, end).hessian0(q, J0=J0, tool=tool) - - def hessiane( - self, - q: Union[ArrayLike, None] = None, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - Je: Union[ndarray, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: - r""" - Manipulator Hessian - - The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the ee frame. This - function calulcates this based on the ETS of the robot. One of Je or q - is required. Supply Je if already calculated to save computation time - - :param q: The joint angles/configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: ArrayLike - :param end: the final link/Gripper which the Hessian represents - :param start: the first link which the Hessian represents - :param Je: The manipulator Jacobian in the ee frame - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return: The manipulator Hessian in ee frame - - This method computes the manipulator Hessian in the ee frame. If - we take the time derivative of the differential kinematic relationship - .. math:: - \nu &= \mat{J}(\vec{q}) \dvec{q} \\ - \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q} - where - .. math:: - \dmat{J} = \mat{H} \dvec{q} - and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the - Hessian tensor. - - The elements of the Hessian are - .. math:: - \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} - where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements - of the spatial velocity vector. - Similarly, we can write - .. math:: - \mat{J}_{i,j} = \frac{d u_i}{d q_j} - - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ - return self.ets(start, end).hessiane(q, Je=Je, tool=tool) - - def partial_fkine0( - self, - q: ArrayLike, - n: int = 3, - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - ): - r""" - Manipulator Forward Kinematics nth Partial Derivative - - The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the ee frame. This - function calulcates this based on the ETS of the robot. One of Je or q - is required. Supply Je if already calculated to save computation time - - :param q: The joint angles/configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: ArrayLike - :param end: the final link/Gripper which the Hessian represents - :param start: the first link which the Hessian represents - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return: The nth Partial Derivative of the forward kinematics - - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ - - return self.ets(start, end).partial_fkine0(q, n=n) - - def link_collision_damper( - self, - shape, - q=None, - di=0.3, - ds=0.05, - xi=1.0, - end=None, - start=None, - collision_list=None, - ): - """ - Formulates an inequality contraint which, when optimised for will - make it impossible for the robot to run into a collision. Requires - See examples/neo.py for use case - :param ds: The minimum distance in which a joint is allowed to - approach the collision object shape - :type ds: float - :param di: The influence distance in which the velocity - damper becomes active - :type di: float - :param xi: The gain for the velocity damper - :type xi: float - :param from_link: The first link to consider, defaults to the base - link - :type from_link: Link - :param to_link: The last link to consider, will consider all links - between from_link and to_link in the robot, defaults to the - end-effector link - :type to_link: Link - :returns: Ain, Bin as the inequality contraints for an omptimisor - :rtype: ndarray(6), ndarray(6) - """ - - end, start, _ = self._get_limit_links(start=start, end=end) - - links, n, _ = self.get_path(start=start, end=end) - - # if q is None: - # q = copy(self.q) - # else: - # q = getvector(q, n) - - j = 0 - Ain = None - bin = None - - def indiv_calculation(link, link_col, q): - d, wTlp, wTcp = link_col.closest_point(shape, di) - - if d is not None: - lpTcp = -wTlp + wTcp - - norm = lpTcp / d - norm_h = expand_dims(concatenate((norm, [0, 0, 0])), axis=0) - - # tool = (self.fkine(q, end=link).inv() * SE3(wTlp)).A[:3, 3] - - # Je = self.jacob0(q, end=link, tool=tool) - # Je[:3, :] = self._T[:3, :3] @ Je[:3, :] - - # n_dim = Je.shape[1] - # dp = norm_h @ shape.v - # l_Ain = zeros((1, self.n)) - - Je = self.jacobe(q, start=self.base_link, end=link, tool=link_col.T) - n_dim = Je.shape[1] - dp = norm_h @ shape.v - l_Ain = zeros((1, n)) - - l_Ain[0, :n_dim] = norm_h @ Je - l_bin = (xi * (d - ds) / (di - ds)) + dp - else: - l_Ain = None - l_bin = None - - return l_Ain, l_bin - - for link in links: - if link.isjoint: - j += 1 - - if collision_list is None: - col_list = link.collision - else: - col_list = collision_list[j - 1] - - for link_col in col_list: - l_Ain, l_bin = indiv_calculation(link, link_col, q) - - if l_Ain is not None and l_bin is not None: - if Ain is None: - Ain = l_Ain - else: - Ain = concatenate((Ain, l_Ain)) - - if bin is None: - bin = array(l_bin) - else: - bin = concatenate((bin, l_bin)) - - return Ain, bin - - def vision_collision_damper( - self, - shape, - camera=None, - camera_n=0, - q=None, - di=0.3, - ds=0.05, - xi=1.0, - end=None, - start=None, - collision_list=None, - ): - """ - Formulates an inequality contraint which, when optimised for will - make it impossible for the robot to run into a line of sight. - See examples/fetch_vision.py for use case - :param camera: The camera link, either as a robotic link or SE3 - pose - :type camera: ERobot or SE3 - :param camera_n: Degrees of freedom of the camera link - :type camera_n: int - :param ds: The minimum distance in which a joint is allowed to - approach the collision object shape - :type ds: float - :param di: The influence distance in which the velocity - damper becomes active - :type di: float - :param xi: The gain for the velocity damper - :type xi: float - :param from_link: The first link to consider, defaults to the base - link - :type from_link: ELink - :param to_link: The last link to consider, will consider all links - between from_link and to_link in the robot, defaults to the - end-effector link - :type to_link: ELink - :returns: Ain, Bin as the inequality contraints for an omptimisor - :rtype: ndarray(6), ndarray(6) - """ - - if start is None: - start = self.base_link - - if end is None: - end = self.ee_link - - links, n, _ = self.get_path(start=start, end=end) - - j = 0 - Ain = None - bin = None - - def rotation_between_vectors(a, b): - a = a / npnorm(a) - b = b / npnorm(b) - - angle = arccos(dot(a, b)) - axis = cross(a, b) - - return SE3.AngleAxis(angle, axis) - - if isinstance(camera, ERobot): - wTcp = camera.fkine(camera.q).A[:3, 3] - elif isinstance(camera, SE3): - wTcp = camera.t - - wTtp = shape.T[:3, -1] - - # Create line of sight object - los_mid = SE3((wTcp + wTtp) / 2) - los_orientation = rotation_between_vectors(array([0.0, 0.0, 1.0]), wTcp - wTtp) - - los = Cylinder( - radius=0.001, - length=npnorm(wTcp - wTtp), - base=(los_mid * los_orientation), - ) - - def indiv_calculation(link, link_col, q): - d, wTlp, wTvp = link_col.closest_point(los, di) - - if d is not None: - lpTvp = -wTlp + wTvp - - norm = lpTvp / d - norm_h = expand_dims(concatenate((norm, [0, 0, 0])), axis=0) - - tool = SE3((inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3]) - - Je = self.jacob0(q, end=link, tool=tool.A) - Je[:3, :] = self._T[:3, :3] @ Je[:3, :] - n_dim = Je.shape[1] - - if isinstance(camera, ERobot): - Jv = camera.jacob0(camera.q) - Jv[:3, :] = self._T[:3, :3] @ Jv[:3, :] - - Jv *= npnorm(wTvp - shape.T[:3, -1]) / los.length - - dpc = norm_h @ Jv - dpc = concatenate( - ( - dpc[0, :-camera_n], - zeros(self.n - (camera.n - camera_n)), - dpc[0, -camera_n:], - ) - ) - else: - dpc = zeros((1, self.n + camera_n)) - - dpt = norm_h @ shape.v - dpt *= npnorm(wTvp - wTcp) / los.length - - l_Ain = zeros((1, self.n + camera_n)) - l_Ain[0, :n_dim] = norm_h @ Je - l_Ain -= dpc - l_bin = (xi * (d - ds) / (di - ds)) + dpt - else: - l_Ain = None - l_bin = None - - return l_Ain, l_bin - - for link in links: - if link.isjoint: - j += 1 - - if collision_list is None: - col_list = link.collision - else: - col_list = collision_list[j - 1] - - for link_col in col_list: - l_Ain, l_bin = indiv_calculation(link, link_col, q) - - if l_Ain is not None and l_bin is not None: - if Ain is None: - Ain = l_Ain - else: - Ain = concatenate((Ain, l_Ain)) - - if bin is None: - bin = array(l_bin) - else: - bin = concatenate((bin, l_bin)) - - return Ain, bin - - # inverse dynamics (recursive Newton-Euler) using spatial vector notation - def rne(self, q, qd, qdd, symbolic=False, gravity=None): - - n = self.n - - # allocate intermediate variables - Xup = SE3.Alloc(n) - Xtree = SE3.Alloc(n) - - v = SpatialVelocity.Alloc(n) - a = SpatialAcceleration.Alloc(n) - f = SpatialForce.Alloc(n) - I = SpatialInertia.Alloc(n) # noqa - s = [] # joint motion subspace - - if symbolic: - Q = empty((n,), dtype="O") # joint torque/force - else: - Q = empty((n,)) # joint torque/force - - # TODO Should the dynamic parameters of static links preceding joint be - # somehow merged with the joint? - - # A temp variable to handle static joints - Ts = SE3() - - # A counter through joints - j = 0 - - # initialize intermediate variables - for link in self.links: - if link.isjoint: - I[j] = SpatialInertia(m=link.m, r=link.r) - if symbolic and link.Ts is None: - Xtree[j] = SE3(eye(4, dtype="O"), check=False) - else: - Xtree[j] = Ts * SE3(link.Ts, check=False) - - if link.v is not None: - s.append(link.v.s) - - # Increment the joint counter - j += 1 - - # Reset the Ts tracker - Ts = SE3() - else: - # TODO Keep track of inertia and transform??? - Ts *= SE3(link.Ts, check=False) - - if gravity is None: - a_grav = -SpatialAcceleration(self.gravity) - else: - a_grav = -SpatialAcceleration(gravity) - - # forward recursion - for j in range(0, n): - vJ = SpatialVelocity(s[j] * qd[j]) - - # transform from parent(j) to j - Xup[j] = SE3(self.links[j].A(q[j])).inv() - - if self.links[j].parent is None: - v[j] = vJ - a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qdd[j]) - else: - jp = self.links[j].parent.jindex # type: ignore - v[j] = Xup[j] * v[jp] + vJ - a[j] = Xup[j] * a[jp] + SpatialAcceleration(s[j] * qdd[j]) + v[j] @ vJ - - f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j]) - - # backward recursion - for j in reversed(range(0, n)): - - # next line could be dot(), but fails for symbolic arguments - Q[j] = sum(f[j].A * s[j]) - - if self.links[j].parent is not None: - jp = self.links[j].parent.jindex # type: ignore - f[jp] = f[jp] + Xup[j] * f[j] - - return Q - - # --------------------------------------------------------------------- # - - def ik_lm_chan( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ndarray, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param end: the particular link or gripper to compute the pose of - :param start: the link considered as the base frame, defaults to the robots's base frame - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== - - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. - - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: - - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. - - :references: - TODO - - :seealso: - TODO - """ - - return self.ets(start, end).ik_lm_chan( - Tep, q0, ilimit, slimit, tol, reject_jl, we, λ - ) - - def ik_lm_wampler( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ndarray, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Wamplers's Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param end: the particular link or gripper to compute the pose of - :param start: the link considered as the base frame, defaults to the robots's base frame - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== - - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. - - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: - - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. - - :references: - TODO - - :seealso: - TODO - """ - - return self.ets(start, end).ik_lm_wampler( - Tep, q0, ilimit, slimit, tol, reject_jl, we, λ - ) - - def ik_lm_sugihara( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ndarray, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Sugihara's Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param end: the particular link or gripper to compute the pose of - :param start: the link considered as the base frame, defaults to the robots's base frame - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== - - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. - - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: - - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. - - :references: - TODO - - :seealso: - TODO - """ - - return self.ets(start, end).ik_lm_sugihara( - Tep, q0, ilimit, slimit, tol, reject_jl, we, λ - ) - - def ik_nr( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ndarray, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - use_pinv: int = True, - pinv_damping: float = 0.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Newton-Raphson Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param end: the particular link or gripper to compute the pose of - :param start: the link considered as the base frame, defaults to the robots's base frame - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== - - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. - - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: - - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. - - :references: - TODO - - :seealso: - TODO - """ - - return self.ets(start, end).ik_nr( - Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping - ) - - def ik_gn( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ndarray, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - use_pinv: int = True, - pinv_damping: float = 0.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Gauss-NewtonMethod) - - :param Tep: The desired end-effector pose or pose trajectory - :param end: the particular link or gripper to compute the pose of - :param start: the link considered as the base frame, defaults to the robots's base frame - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== - - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. - - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: - - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. - - :references: - TODO - - :seealso: - TODO - """ - - return self.ets(start, end).ik_gn( - Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping - ) - - def ikine_LM( - self, - Tep: Union[ndarray, SE3], - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - q0: Union[ArrayLike, None] = None, - ilimit: int = 30, - slimit: int = 100, - tol: float = 1e-6, - joint_limits: bool = False, - mask: Union[ArrayLike, None] = None, - seed: Union[int, None] = None, - ): - return self.ets(start, end).ikine_LM( - Tep=Tep, - q0=q0, - ilimit=ilimit, - slimit=slimit, - tol=tol, - joint_limits=joint_limits, - mask=mask, - seed=seed, - ) - - -# =========================================================================== # - - -class ERobot2(BaseERobot): - def __init__(self, arg, **kwargs): - - if isinstance(arg, ETS2): - # we're passed an ETS string - links = [] - # chop it up into segments, a link frame after every joint - parent = None - for j, ets_j in enumerate(arg.split()): - elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}") - parent = elink - if ( - elink.qlim is None - and elink.v is not None - and elink.v.qlim is not None - ): - elink.qlim = elink.v.qlim - links.append(elink) - - elif islistof(arg, Link2): - links = arg - else: - raise TypeError("constructor argument must be ETS2 or list of Link2") - - super().__init__(links, **kwargs) - - # should just set it to None - self.base = SE2() # override superclass - - @property - def base(self) -> SE2: - """ - Get/set robot base transform (Robot superclass) - - - ``robot.base`` is the robot base transform - - :return: robot tool transform - :rtype: SE2 instance - - - ``robot.base = ...`` checks and sets the robot base transform - - .. note:: The private attribute ``_base`` will be None in the case of - no base transform, but this property will return ``SE3()`` which - is an identity matrix. - """ - if self._base is None: - self._base = SE2() - - # return a copy, otherwise somebody with - # reference to the base can change it - return self._base.copy() - - @base.setter - def base(self, T): - if T is None: - self._base = T - elif isinstance(self, ERobot2): - # 2D robot - if isinstance(T, SE2): - self._base = T - elif SE2.isvalid(T): - self._tool = SE2(T, check=True) - else: - raise ValueError("base must be set to None (no tool) or SE2") - - def jacob0(self, q, start=None, end=None): - return self.ets(start, end).jacob0(q) - - def jacobe(self, q, start=None, end=None): - return self.ets(start, end).jacobe(q) - - def fkine(self, q, end=None, start=None): - - return self.ets(start, end).fkine(q) - - -# --------------------------------------------------------------------- # - -# def teach( -# self, -# q=None, -# block=True, -# limits=None, -# vellipse=False, -# fellipse=False, -# eeframe=True, -# name=False, -# unit='rad', -# backend='pyplot2'): -# """ -# 2D Graphical teach pendant -# :param block: Block operation of the code and keep the figure open -# :type block: bool -# :param q: The joint configuration of the robot (Optional, -# if not supplied will use the stored q values). -# :type q: float ndarray(n) -# :param limits: Custom view limits for the plot. If not supplied will -# autoscale, [x1, x2, y1, y2] -# :type limits: array_like(4) -# :param vellipse: (Plot Option) Plot the velocity ellipse at the -# end-effector -# :type vellipse: bool -# :param vellipse: (Plot Option) Plot the force ellipse at the -# end-effector -# :type vellipse: bool -# :param eeframe: (Plot Option) Plot the end-effector coordinate frame -# at the location of the end-effector. Uses three arrows, red, -# green and blue to indicate the x, y, and z-axes. -# :type eeframe: bool -# :param name: (Plot Option) Plot the name of the robot near its base -# :type name: bool -# :param unit: angular units: 'rad' [default], or 'deg' -# :type unit: str - -# :return: A reference to the PyPlot object which controls the -# matplotlib figure -# :rtype: PyPlot -# - ``robot.teach2(q)`` creates a 2D matplotlib plot which allows the -# user to "drive" a graphical robot using a graphical slider panel. -# The robot's inital joint configuration is ``q``. The plot will -# autoscale with an aspect ratio of 1. -# - ``robot.teach2()`` as above except the robot's stored value of ``q`` -# is used. -# .. note:: -# - Program execution is blocked until the teach window is -# dismissed. If ``block=False`` the method is non-blocking but -# you need to poll the window manager to ensure that the window -# remains responsive. -# - The slider limits are derived from the joint limit properties. -# If not set then: -# - For revolute joints they are assumed to be [-pi, +pi] -# - For prismatic joint they are assumed unknown and an error -# occurs. -# If not set then -# - For revolute joints they are assumed to be [-pi, +pi] -# - For prismatic joint they are assumed unknown and an error -# occurs. -# """ - -# if q is None: -# q = zeros((self.n,)) -# else: -# q = getvector(q, self.n) - -# if unit == 'deg': -# q = self.toradians(q) - -# # Make an empty 3D figure -# env = self._get_graphical_backend(backend) - -# # Add the robot to the figure in readonly mode -# env.launch('Teach ' + self.name, limits=limits) -# env.add( -# self, readonly=True, -# eeframe=eeframe, name=name) - -# env._add_teach_panel(self, q) - -# if limits is None: -# limits = r_[-1, 1, -1, 1] * self.reach * 1.5 -# env.ax.set_xlim([limits[0], limits[1]]) -# env.ax.set_ylim([limits[2], limits[3]]) - -# if vellipse: -# vell = self.vellipse(centre='ee', scale=0.5) -# env.add(vell) - -# if fellipse: -# fell = self.fellipse(centre='ee') -# env.add(fell) - -# # Keep the plot open -# if block: # pragma: no cover -# env.hold() - -# return env - - -if __name__ == "__main__": # pragma nocover +class ERobot2(Robot2): + def __init__(self, *args, **kwargs): - e1 = Link(ETS(ET.Rz()), jindex=0) - e2 = Link(ETS(ET.Rz()), jindex=1, parent=e1) - e3 = Link(ETS(ET.Rz()), jindex=2, parent=e2) - e4 = Link(ETS(ET.Rz()), jindex=5, parent=e3) + # warn("ERobot2 is deprecated, use iscollided instead", FutureWarning) - ERobot([e1, e2, e3, e4]) + super().__init__(*args, **kwargs) diff --git a/roboticstoolbox/robot/ET.py b/roboticstoolbox/robot/ET.py index 37df93320..60968a98a 100644 --- a/roboticstoolbox/robot/ET.py +++ b/roboticstoolbox/robot/ET.py @@ -23,13 +23,14 @@ from spatialmath import SE3, SE2 from typing import Optional, Callable, Union, TYPE_CHECKING -ArrayLike = Union[list, ndarray, tuple, set] +# from spatialmath.base.types import ArrayLike +from roboticstoolbox.tools.types import ArrayLike, NDArray -if TYPE_CHECKING: +if TYPE_CHECKING: # pragma: nocover import sympy Sym = sympy.core.symbol.Symbol -else: +else: # pragma: nocover Sym = None @@ -47,6 +48,10 @@ def __init__( ): self._axis = axis + # A flag to check if the ET is a static joint with a symbolic value + # Defaults to False as is set to True if eta is a symbol below + self._isstaticsym = False + if eta is None: self._eta = None else: @@ -61,9 +66,9 @@ def __init__( self._jindex = jindex if qlim is not None: - self._qlim = array(getvector(qlim, 2, out="array")) + self._qlim: Union[NDArray, None] = getvector(qlim, 2, out="array") else: - self._qlim = None + self._qlim: Union[NDArray, None] = None if self.eta is None: if T is None: @@ -75,6 +80,10 @@ def __init__( self._joint = False self._T = T.copy(order="F") else: + # This is a static joint + if issymbol(eta): + self._isstaticsym = True + self._joint = False if axis_func is not None: self._T = axis_func(self.eta).copy(order="F") @@ -98,11 +107,15 @@ def __init_c(self): jindex = self.jindex if self.qlim is None: - qlim = array([0, 0]) + if self.axis[0] == "R": + qlim = array([-pi, pi]) + else: + qlim = array([0, 1]) else: qlim = self.qlim return ET_init( + self._isstaticsym, self.isjoint, self.isflip, jindex, @@ -121,12 +134,16 @@ def __update_c(self): jindex = self.jindex if self.qlim is None: - qlim = array([0, 0]) + if self.axis[0] == "R": + qlim = array([-pi, pi]) + else: + qlim = array([0, 1]) else: qlim = self.qlim ET_update( self.fknm, + self._isstaticsym, self.isjoint, self.isflip, jindex, @@ -136,7 +153,6 @@ def __update_c(self): ) def __str__(self): - eta_str = "" if self.isjoint: @@ -154,7 +170,10 @@ def __str__(self): T = self.A() rpy = tr2rpy(T) * 180.0 / pi if T[:3, -1].any() and rpy.any(): - eta_str = f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g}; {rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" + eta_str = ( + f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g};" + f" {rpy[0]:.4g}°, {rpy[1]:.4g}°, {rpy[2]:.4g}°" + ) elif T[:3, -1].any(): eta_str = f"{T[0, -1]:.4g}, {T[1, -1]:.4g}, {T[2, -1]:.4g}" elif rpy.any(): @@ -173,7 +192,6 @@ def __str__(self): return f"{self.axis}({eta_str})" def __repr__(self): - s_eta = "" if self.eta is None else f"eta={self.eta}" s_T = ( f"T={repr(self._T)}" @@ -206,7 +224,7 @@ def _repr_pretty_(self, p, cycle): [In [1]: e Out [1]: tx(1) """ - p.text(str(self)) + p.text(str(self)) # pragma: nocover def __deepcopy__(self, memo): cls = self.__class__ @@ -256,21 +274,25 @@ def eta(self) -> Union[float, Sym, None]: """ Get the transform constant - :return: The constant η if set - - Example: + Returns + ------- + ets + The constant η if set + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.eta - >>> e = ET.Rx(90, 'deg') - >>> e.eta - >>> e = ET.ty() - >>> e.eta - - .. note:: If the value was given in degrees it will be converted and + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.eta + >>> e = ET.Rx(90, 'deg') + >>> e.eta + >>> e = ET.ty() + >>> e.eta + + Notes + ----- + - If the value was given in degrees it will be converted and stored internally in radians """ return self._eta @@ -280,9 +302,14 @@ def eta(self, value: Union[float, Sym]) -> None: """ Set the transform constant - :param value: The transform constant η + Parameters + ---------- + value + The transform constant η - .. note:: No unit conversions are applied, it is assumed to be in + Notes + ----- + - No unit conversions are applied, it is assumed to be in radians. """ self._eta = value if issymbol(value) else float(value) @@ -298,17 +325,19 @@ def axis(self) -> str: """ The transform type and axis - :return: The transform type and axis - - Example: + Returns + ------- + axis + The transform type and axis + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.axis - >>> e = ET.Rx(90, 'deg') - >>> e.axis + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.axis + >>> e = ET.Rx(90, 'deg') + >>> e.axis """ return self._axis @@ -318,17 +347,20 @@ def isjoint(self) -> bool: """ Test if ET is a joint - :return: True if a joint - - Example: + Returns + ------- + isjoint + True if a joint + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.isjoint + >>> e = ET.tx() + >>> e.isjoint - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.isjoint - >>> e = ET.tx() - >>> e.isjoint """ return self._joint @@ -337,21 +369,25 @@ def isflip(self) -> bool: """ Test if ET joint is flipped - :return: True if joint is flipped - A flipped joint uses the negative of the joint variable, ie. it rotates or moves in the opposite direction. - Example: + Returns + ------- + isflip + True if joint is flipped + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tx() + >>> e.T(1) + >>> eflip = ET.tx(flip=True) + >>> eflip.T(1) - >>> from roboticstoolbox import ET - >>> e = ET.tx() - >>> e.T(1) - >>> eflip = ET.tx(flip=True) - >>> eflip.T(1) """ + return self._flip @property @@ -359,18 +395,22 @@ def isrotation(self) -> bool: """ Test if ET is a rotation - :return: True if a rotation - - Example: + Returns + ------- + isrotation + True if a rotation + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.isrotation + >>> e = ET.rx() + >>> e.isrotation - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.isrotation - >>> e = ET.rx() - >>> e.isrotation """ + return self.axis[0] == "R" @property @@ -378,18 +418,22 @@ def istranslation(self) -> bool: """ Test if ET is a translation - :return: True if a translation - - Example: + Returns + ------- + istranslation + True if a translation + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.istranslation + >>> e = ET.rx() + >>> e.istranslation - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.istranslation - >>> e = ET.rx() - >>> e.istranslation """ + return self.axis[0] == "t" @property @@ -399,7 +443,7 @@ def qlim(self) -> Union[ndarray, None]: @qlim.setter def qlim(self, qlim_new: Union[ArrayLike, None]) -> None: if qlim_new is not None: - qlim_new = array(getvector(qlim_new, 2, out="array")) + qlim_new = getvector(qlim_new, 2, out="array") self._qlim = qlim_new self.__update_c() @@ -408,21 +452,25 @@ def jindex(self) -> Union[int, None]: """ Get ET joint index - :return: The assigmed joint index + Returns + ------- + jindex + The assigmed joint index Allows an ET to be associated with a numbered joint in a robot. - Example: - + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tx() + >>> print(e) + >>> e = ET.tx(j=3) + >>> print(e) + >>> print(e.jindex) - >>> from roboticstoolbox import ET - >>> e = ET.tx() - >>> print(e) - >>> e = ET.tx(j=3) - >>> print(e) - >>> print(e.jindex) """ + return self._jindex @jindex.setter @@ -437,31 +485,42 @@ def iselementary(self) -> bool: """ Test if ET is an elementary transform - :return: True if an elementary transform + Returns + ------- + iselementary + True if an elementary transform - .. note:: ET's may not actually be "elementary", it can be a complex + Notes + ----- + - ET's may not actually be "elementary", it can be a complex mix of rotations and translations. - :seealso: :func:`compile` + See Also + -------- + :func:`compile` + """ + return self.axis[0] != "S" def inv(self): r""" Inverse of ET - :return: Inverse of the ET - The inverse of a given ET. - Example: + Returns + ------- + inv + Inverse of the ET + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(2.5) - >>> print(e) - >>> print(e.inv()) + >>> from roboticstoolbox import ET + >>> e = ET.Rz(2.5) + >>> print(e) + >>> print(e.inv()) """ # noqa @@ -483,19 +542,24 @@ def A(self, q: Union[float, Sym] = 0.0) -> ndarray: """ Evaluate an elementary transformation - :param q: Is used if this ET is variable (a joint) + Parameters + ---------- + q + Is used if this ET is variable (a joint) - :return: The SE(3) or SE(2) matrix value of the ET - - Example: + Returns + ------- + T + The SE(3) or SE(2) matrix value of the ET + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.tx(1) - >>> e.A() - >>> e = ET.tx() - >>> e.A(0.7) + >>> from roboticstoolbox import ET + >>> e = ET.tx(1) + >>> e.A() + >>> e = ET.tx() + >>> e.A(0.7) """ try: @@ -505,7 +569,7 @@ def A(self, q: Union[float, Sym] = 0.0) -> ndarray: # We can't use the fast version, lets use Python instead if self.isjoint: if self.isflip: - q = -1.0 * q + q = -q # type: ignore if self.axis_func is not None: return self.axis_func(q) @@ -550,20 +614,33 @@ def Rx( """ Pure rotation about the x-axis - :param η: rotation about the x-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.Rx(η)`` is an elementary rotation about the x-axis by a constant angle η - ``ET.Rx()`` is an elementary rotation about the x-axis by a variable angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`isrotation` + Parameters + ---------- + η + rotation about the x-axis + unit + angular unit, "rad" [default] or "deg" + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + Rx + An elementary transform + + See Also + -------- + :func:`ET` + :func:`isrotation` + :SymPy: supported """ @@ -576,20 +653,33 @@ def Ry( """ Pure rotation about the y-axis - :param η: rotation about the y-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.Ry(η)`` is an elementary rotation about the y-axis by a constant angle η - ``ET.Ry()`` is an elementary rotation about the y-axis by a variable angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`isrotation` + Parameters + ---------- + η + rotation about the y-axis + unit + angular unit, "rad" [default] or "deg" + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + Ry + An elementary transform + + See Also + -------- + :func:`ET` + :func:`isrotation` + :SymPy: supported """ return cls(axis="Ry", eta=eta, axis_func=troty, unit=unit, **kwargs) @@ -601,20 +691,33 @@ def Rz( """ Pure rotation about the z-axis - :param η: rotation about the z-axis - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.Rz(η)`` is an elementary rotation about the z-axis by a constant angle η - ``ET.Rz()`` is an elementary rotation about the z-axis by a variable angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`isrotation` + Parameters + ---------- + η + rotation about the z-axis + unit + angular unit, "rad" [default] or "deg" + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + Rz + An elementary transform + + See Also + -------- + :func:`ET` + :func:`isrotation` + :SymPy: supported """ return cls(axis="Rz", eta=eta, axis_func=trotz, unit=unit, **kwargs) @@ -624,19 +727,31 @@ def tx(cls, eta: Union[float, Sym, None] = None, **kwargs) -> "ET": """ Pure translation along the x-axis - :param η: translation distance along the z-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.tx(η)`` is an elementary translation along the x-axis by a distance constant η - ``ET.tx()`` is an elementary translation along the x-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`istranslation` + Parameters + ---------- + η + translation distance along the z-axis + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + tx + An elementary transform + + See Also + -------- + :func:`ET` + :func:`istranslation` + :SymPy: supported """ @@ -658,19 +773,31 @@ def ty(cls, eta: Union[float, Sym, None] = None, **kwargs) -> "ET": """ Pure translation along the y-axis - :param η: translation distance along the y-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.ty(η)`` is an elementary translation along the y-axis by a distance constant η - ``ET.ty()`` is an elementary translation along the y-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`istranslation` + Parameters + ---------- + η + translation distance along the y-axis + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + ty + An elementary transform + + See Also + -------- + :func:`ET` + :func:`istranslation` + :SymPy: supported """ @@ -691,19 +818,31 @@ def tz(cls, eta: Union[float, Sym, None] = None, **kwargs) -> "ET": """ Pure translation along the z-axis - :param η: translation distance along the z-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET.tz(η)`` is an elementary translation along the z-axis by a distance constant η - ``ET.tz()`` is an elementary translation along the z-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`istranslation` + Parameters + ---------- + η + translation distance along the z-axis + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + tz + An elementary transform + + See Also + -------- + :func:`ET` + func:`istranslation` + :SymPy: supported """ @@ -724,17 +863,27 @@ def SE3(cls, T: Union[ndarray, SE3], **kwargs) -> "ET": """ A static SE3 - :param T: The SE3 trnasformation matrix - - :return: An elementary transform - - ``ET.T(η)`` is an elementary translation along the z-axis by a distance constant η - ``ET.tz()`` is an elementary translation along the z-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET`, :func:`istranslation` + Parameters + ---------- + T + The SE3 trnasformation matrix + + Returns + ------- + SE3 + An elementary transform + + See Also + -------- + :func:`ET` + :func:`istranslation` + :SymPy: supported """ @@ -771,23 +920,38 @@ def R( """ Pure rotation - :param η: rotation angle - :param unit: angular unit, "rad" [default] or "deg" - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET2.R(η)`` is an elementary rotation by a constant angle η - ``ET2.R()`` is an elementary rotation by a variable angle, i.e. a revolute robot joint. ``j`` or ``flip`` can be set in this case. - .. note:: In the 2D case this is rotation around the normal to the + Parameters + ---------- + η + rotation angle + unit + angular unit, "rad" [default] or "deg" + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + R + An elementary transform + + Notes + ----- + - In the 2D case this is rotation around the normal to the xy-plane. - :seealso: :func:`ET2`, :func:`isrotation` + See Also + -------- + :func:`ET2`, :func:`isrotation` + """ + return cls( axis="R", eta=eta, axis_func=lambda theta: trot2(theta), unit=unit, **kwargs ) @@ -799,20 +963,33 @@ def tx( """ Pure translation along the x-axis - :param η: translation distance along the z-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET2.tx(η)`` is an elementary translation along the x-axis by a distance constant η - ``ET2.tx()`` is an elementary translation along the x-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET2`, :func:`istranslation` + Parameters + ---------- + η + translation distance along the x-axis + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + tx + An elementary transform + + See Also + -------- + :func:`ET2` + :func:`istranslation` + """ + return cls(axis="tx", eta=eta, axis_func=lambda x: transl2(x, 0), **kwargs) @classmethod @@ -822,20 +999,32 @@ def ty( """ Pure translation along the y-axis - :param η: translation distance along the y-axis - :param j: Explicit joint number within the robot - :param flip: Joint moves in opposite direction - - :return: An elementary transform - - ``ET2.tx(η)`` is an elementary translation along the y-axis by a distance constant η - ``ET2.tx()`` is an elementary translation along the y-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET2` + Parameters + ---------- + η + translation distance along the y-axis + j + Explicit joint number within the robot + flip + Joint moves in opposite direction + + Returns + ------- + ty + An elementary transform + + See Also + -------- + :func:`ET2` + """ + return cls(axis="ty", eta=eta, axis_func=lambda y: transl2(0, y), **kwargs) @classmethod @@ -843,17 +1032,27 @@ def SE2(cls, T: Union[ndarray, SE2], **kwargs) -> "ET2": """ A static SE2 - :param T: The SE2 trnasformation matrix - - :return: An elementary transform - - ``ET2.T(η)`` is an elementary translation along the z-axis by a distance constant η - ``ET2.tz()`` is an elementary translation along the z-axis by a variable distance, i.e. a prismatic robot joint. ``j`` or ``flip`` can be set in this case. - :seealso: :func:`ET2`, :func:`istranslation` + Parameters + ---------- + T + The SE2 trnasformation matrix + + Returns + ------- + SE2 + An elementary transform + + See Also + -------- + :func:`ET2` + :func:`istranslation` + :SymPy: supported """ @@ -865,21 +1064,27 @@ def A(self, q: Union[float, Sym] = 0.0) -> ndarray: """ Evaluate an elementary transformation - :param q: Is used if this ET2 is variable (a joint) - - :return: The SE(2) matrix value of the ET2 + Parameters + ---------- + q + Is used if this ET2 is variable (a joint) - Example: + Returns + ------- + T + The SE(2) matrix value of the ET2 + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET2 - >>> e = ET2.tx(1) - >>> e.A() - >>> e = ET2.tx() - >>> e.A(0.7) + >>> from roboticstoolbox import ET2 + >>> e = ET2.tx(1) + >>> e.A() + >>> e = ET2.tx() + >>> e.A(0.7) """ + if self.isjoint: if self.isflip: q = -1.0 * q diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py index a62d6b926..ee339d6d2 100644 --- a/roboticstoolbox/robot/ETS.py +++ b/roboticstoolbox/robot/ETS.py @@ -5,27 +5,10 @@ @author: Peter Corke """ -from abc import ABC, abstractmethod from collections import UserList -from numpy import ( - pi, - where, - all, - ndarray, - zeros, - array, - eye, - array_equal, - sqrt, - min, - max, - where, - cross, - flip, - concatenate, -) +import numpy as np from numpy.random import uniform -from numpy.linalg import inv, det, cond, pinv, matrix_rank, svd, eig +from numpy.linalg import inv, det, cond, svd from spatialmath import SE3, SE2 from spatialmath.base import ( getvector, @@ -36,12 +19,11 @@ t2r, rotvelxform, simplify, + getmatrix, ) from roboticstoolbox import rtb_get_param -from roboticstoolbox.robot.IK import IK_LM, IK_NR +from roboticstoolbox.robot.IK import IK_GN, IK_LM, IK_NR, IK_QP -from collections import UserList -from spatialmath.base import issymbol, getmatrix from roboticstoolbox.fknm import ( ETS_init, ETS_fkine, @@ -51,19 +33,14 @@ ETS_hessiane, IK_NR_c, IK_GN_c, - IK_LM_Chan_c, - IK_LM_Wampler_c, - IK_LM_Sugihara_c, + IK_LM_c, ) from copy import deepcopy -from roboticstoolbox import rtb_get_param from roboticstoolbox.robot.ET import ET, ET2 -from spatialmath.base import getvector -from spatialmath import SE3 from typing import Union, overload, List, Set, Tuple +from typing_extensions import Literal as L from sys import version_info - -ArrayLike = Union[list, ndarray, tuple, set] +from roboticstoolbox.tools.types import ArrayLike, NDArray py_ver = version_info @@ -71,16 +48,10 @@ from functools import cached_property c_property = cached_property -else: +else: # pragma: nocover c_property = property -# class AbstractETS(ABC): -# @abstractmethod -# def jacobm(self) -> ndarray: -# pass - - class BaseETS(UserList): def __init__(self, *args): super().__init__(*args) @@ -99,10 +70,6 @@ def __str__(self, q: Union[str, None] = None): """ Pretty prints the ETS - :param q: control how joint variables are displayed - :type q: ArrayLike - :return: Pretty printed ETS - ``q`` controls how the joint variables are displayed: - None, format depends on number of joint variables @@ -116,43 +83,53 @@ def __str__(self, q: Union[str, None] = None): display joint variables as θ1, θ2, ... ``j`` is either the joint index, if provided, otherwise a sequential value. - Example: - - .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() - >>> print(e[:2]) - >>> print(e) - >>> print(e.__str__("")) - >>> print(e.__str__("θ{0}")) # numbering from 0 - >>> print(e.__str__("θ{1}")) # numbering from 1 - >>> # explicit joint indices - >>> e = ET.Rz(jindex=3) * ET.tx(1) * ET.Rz(jindex=4) - >>> print(e) - >>> print(e.__str__("θ{0}")) - - .. note:: Angular parameters are converted to degrees, except if they - are symbolic. + Parameters + ---------- + q + control how joint variables are displayed - Example: + Returns + ------- + str + Pretty printed ETS + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() + >>> print(e[:2]) + >>> print(e) + >>> print(e.__str__("")) + >>> print(e.__str__("θ{0}")) # numbering from 0 + >>> print(e.__str__("θ{1}")) # numbering from 1 + >>> # explicit joint indices + >>> e = ET.Rz(jindex=3) * ET.tx(1) * ET.Rz(jindex=4) + >>> print(e) + >>> print(e.__str__("θ{0}")) + + Angular parameters are converted to degrees, except if they + are symbolic. - >>> from roboticstoolbox import ET - >>> from spatialmath.base import symbol - >>> theta, d = symbol('theta, d') - >>> e = ET.Rx(theta) * ET.tx(2) * ET.Rx(45, 'deg') * ET.Ry(0.2) * ET.ty(d) - >>> str(e) + .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> from spatialmath.base import symbol + >>> theta, d = symbol('theta, d') + >>> e = ET.Rx(theta) * ET.tx(2) * ET.Rx(45, 'deg') * ET.Ry(0.2) * ET.ty(d) + >>> str(e) - :SymPy: supported """ + es = [] j = 0 c = 0 s = None unicode = rtb_get_param("unicode") + # An empty SE3 + if len(self.data) == 0: + return "SE3()" + if q is None: if len(self.joints()) > 1: q = "q{0}" @@ -162,10 +139,9 @@ def __str__(self, q: Union[str, None] = None): # For et in the object, display it, data comes from properties # which come from the named tuple for et in self.data: - if et.isjoint: if q is not None: - if et.jindex is None: + if et.jindex is None: # pragma: nocover this is no longer possible _j = j else: _j = et.jindex @@ -185,12 +161,12 @@ def __str__(self, q: Union[str, None] = None): if issymbol(et.eta): s = f"{et.axis}({et.eta})" else: - s = f"{et.axis}({et.eta * 180 / pi:.4g}°)" + s = f"{et.axis}({et.eta * 180 / np.pi:.4g}°)" elif et.istranslation: try: s = f"{et.axis}({et.eta:.4g})" - except TypeError: + except TypeError: # pragma: nocover s = f"{et.axis}({et.eta})" elif not et.iselementary: @@ -208,115 +184,147 @@ def _repr_pretty_(self, p, cycle): """ Pretty string for IPython - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) - Print stringified version when variable is displayed in IPython, ie. on a line by itself. - Example:: + Parameters + ---------- + p + pretty printer handle (ignored) + cycle + pretty printer flag (ignored) + + Examples + -------- + In [1]: e + Out [1]: R(q0) ⊕ tx(1) ⊕ R(q1) ⊕ tx(1) - [In [1]: e - Out [1]: R(q0) ⊕ tx(1) ⊕ R(q1) ⊕ tx(1) """ - print(self.__str__()) + + print(self.__str__()) # pragma: nocover def joint_idx(self) -> List[int]: """ Get index of joint transforms - :return: indices of transforms that are joints - - Example: + Returns + ------- + joint_idx + indices of transforms that are joints + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.joint_idx() + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.joint_idx() """ - return where([e.isjoint for e in self])[0] + + return np.where([e.isjoint for e in self])[0] # type: ignore def joints(self) -> List[ET]: """ Get a list of the variable ETs with this ETS - :return: list of ETs that are joints - - Example: + Returns + ------- + joints + list of ETs that are joints + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.joints() + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.joints() """ + return [e for e in self if e.isjoint] def jindex_set(self) -> Set[int]: # """ Get set of joint indices - :return: set of unique joint indices - - Example: + Returns + ------- + jindex_set + set of unique joint indices + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) + >>> e.jointset() - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) - >>> e.jointset() """ + return set([self[j].jindex for j in self.joint_idx()]) # type: ignore @c_property - def jindices(self) -> ndarray: + def jindices(self) -> NDArray: """ Get an array of joint indices - :return: array of unique joint indices - - Example: + Returns + ------- + jindices + array of unique joint indices + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) + >>> e.jointset() - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=1) * ET.tx(jindex=2) * ET.Rz(jindex=1) * ET.tx(1) - >>> e.jointset() """ - return array([j.jindex for j in self.joints()]) # type: ignore - @c_property + return np.array([j.jindex for j in self.joints()]) # type: ignore + + @property def qlim(self): r""" - Joint limits - - :return: Array of joint limit values - :rtype: ndarray(2,n) - :exception ValueError: unset limits for a prismatic joint + Get/Set Joint limits Limits are extracted from the link objects. If joints limits are not set for: - - a revolute joint [-𝜋. 𝜋] is returned - - a prismatic joint an exception is raised + - a revolute joint [-𝜋. 𝜋] is returned + - a prismatic joint an exception is raised - Example: + Parameters + ---------- + new_qlim + An ndarray(2, n) of the new joint limits to set + Returns + ------- + :return: Array of joint limit values + :rtype: ndarray(2,n) + + Raises + ------ + ValueError + unset limits for a prismatic joint + + Examples + -------- .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.qlim - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.qlim """ - limits = zeros((2, self.n)) + + limits = np.zeros((2, self.n)) for i, et in enumerate(self.joints()): if et.isrotation: if et.qlim is None: - v = [-pi, pi] + v = [-np.pi, np.pi] else: v = et.qlim elif et.istranslation: @@ -325,30 +333,53 @@ def qlim(self): else: v = et.qlim else: - raise ValueError("Undefined Joint Type") + raise ValueError("Undefined Joint Type") # pragma: nocover limits[:, i] = v return limits + @qlim.setter + def qlim(self, new_qlim: ArrayLike): + new_qlim = np.array(new_qlim) + + if new_qlim.shape == (2,) and self.n == 1: + new_qlim = new_qlim.reshape(2, 1) + + if new_qlim.shape != (2, self.n): + raise ValueError("new_qlim must be of shape (2, n)") + + for j, i in enumerate(self.joint_idx()): + et = self[i] + et.qlim = new_qlim[:, j] + + self[i] = et + + self._update_internals() + @property def structure(self) -> str: """ Joint structure string - :return: A string indicating the joint types - A string comprising the characters 'R' or 'P' which indicate the types of joints in order from left to right. - Example: + Returns + ------- + structure + A string indicating the joint types + - .. runblock:: pycon - >>> from roboticstoolbox import ET - >>> e = ET.tz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e.structure + Examples + -------- + .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.tz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e.structure """ + return "".join( ["R" if self.data[i].isrotation else "P" for i in self.joint_idx()] ) @@ -358,19 +389,24 @@ def n(self) -> int: """ Number of joints - :return: the number of joints in the ETS - Counts the number of joints in the ETS. - Example: + Returns + ------- + n + the number of joints in the ETS + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rx() * ET.tx(1) * ET.tz() + >>> e.n - >>> from roboticstoolbox import ET - >>> e = ET.Rx() * ET.tx(1) * ET.tz() - >>> e.n + See Also + -------- + :func:`joints` - :seealso: :func:`joints` """ return self._n @@ -380,29 +416,31 @@ def m(self) -> int: """ Number of transforms - :return: the number of transforms in the ETS - Counts the number of transforms in the ETS. - Example: + Returns + ------- + m + the number of transforms in the ETS + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rx() * ET.tx(1) * ET.tz() - >>> e.m + >>> from roboticstoolbox import ET + >>> e = ET.Rx() * ET.tx(1) * ET.tz() + >>> e.m """ return self._m @overload - def data(self: "ETS") -> List[ET]: # pragma: nocover - ... + def data(self: "ETS") -> List[ET]: + ... # pragma: nocover @overload - def data(self: "ETS2") -> List[ET2]: # pragma: nocover - ... + def data(self: "ETS2") -> List[ET2]: + ... # pragma: nocover @property def data(self): @@ -410,66 +448,83 @@ def data(self): @data.setter @overload - def data(self: "ETS", new_data: List[ET]): # pragma: nocover - ... + def data(self: "ETS", new_data: List[ET]): + ... # pragma: nocover @data.setter @overload - def data(self: "ETS", new_data: List[ET2]): # pragma: nocover - ... + def data(self: "ETS", new_data: List[ET2]): + ... # pragma: nocover @data.setter def data(self, new_data): self._data = new_data @overload - def pop(self: "ETS", i: int = -1) -> ET: # pragma: nocover - ... + def pop(self: "ETS", i: int = -1) -> ET: + ... # pragma: nocover @overload - def pop(self: "ETS2", i: int = -1) -> ET2: # pragma: nocover - ... + def pop(self: "ETS2", i: int = -1) -> ET2: + ... # pragma: nocover def pop(self, i=-1): """ Pop value - :param i: item in the list to pop, default is last - :return: the popped value - :raises IndexError: if there are no values to pop - Removes a value from the value list and returns it. The original instance is modified. - Example: + Parameters + ---------- + i + item in the list to pop, default is last + + Returns + ------- + pop + the popped value + Raises + ------ + IndexError + if there are no values to pop + + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> tail = e.pop() + >>> tail + >>> e - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> tail = e.pop() - >>> tail - >>> e """ + item = super().pop(i) self._update_internals() return item @overload - def split(self: "ETS") -> List["ETS"]: # pragma: nocover - ... + def split(self: "ETS") -> List["ETS"]: + ... # pragma: nocover @overload - def split(self: "ETS2") -> List["ETS2"]: # pragma: nocover - ... + def split(self: "ETS2") -> List["ETS2"]: + ... # pragma: nocover def split(self): """ Split ETS into link segments - Returns a list of ETS, each one, apart from the last, - ends with a variable ET. + Returns + ------- + split + a list of ETS, each one, apart from the last, + ends with a variable ET. + """ + segments = [] start = 0 @@ -493,19 +548,17 @@ def split(self): return segments @overload - def inv(self: "ETS") -> "ETS": # pragma: nocover - ... + def inv(self: "ETS") -> "ETS": + ... # pragma: nocover @overload - def inv(self: "ETS2") -> "ETS2": # pragma: nocover - ... + def inv(self: "ETS2") -> "ETS2": + ... # pragma: nocover def inv(self): r""" Inverse of ETS - :return: Inverse of the ETS - The inverse of a given ETS. It is computed as the inverse of the individual ETs in the reverse order. @@ -513,59 +566,71 @@ def inv(self): (\mathbf{E}_0, \mathbf{E}_1 \cdots \mathbf{E}_{n-1} )^{-1} = (\mathbf{E}_{n-1}^{-1}, \mathbf{E}_{n-2}^{-1} \cdots \mathbf{E}_0^{-1}{n-1} ) - Example: + Returns + ------- + inv + Inverse of the ETS + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz(jindex=2) * ET.tx(1) * ET.Rx(jindex=3,flip=True) * ET.tx(1) - >>> print(e) - >>> print(e.inv()) - - .. note:: It is essential to use explicit joint indices to account for + >>> from roboticstoolbox import ET + >>> e = ET.Rz(jindex=2) * ET.tx(1) * ET.Rx(jindex=3,flip=True) * ET.tx(1) + >>> print(e) + >>> print(e.inv()) + + Notes + ----- + - It is essential to use explicit joint indices to account for the reversed order of the transforms. + """ # noqa return self.__class__([et.inv() for et in reversed(self.data)]) @overload - def __getitem__(self: "ETS", i: int) -> ET: # pragma: nocover - ... + def __getitem__(self: "ETS", i: int) -> ET: + ... # pragma: nocover @overload - def __getitem__(self: "ETS", i: slice) -> List[ET]: # pragma: nocover - ... + def __getitem__(self: "ETS", i: slice) -> List[ET]: + ... # pragma: nocover @overload - def __getitem__(self: "ETS2", i: int) -> ET2: # pragma: nocover - ... + def __getitem__(self: "ETS2", i: int) -> ET2: + ... # pragma: nocover @overload - def __getitem__(self: "ETS2", i: slice) -> List[ET2]: # pragma: nocover - ... + def __getitem__(self: "ETS2", i: slice) -> List[ET2]: + ... # pragma: nocover def __getitem__(self, i): """ Index or slice an ETS - :param i: the index or slince - :return: Elementary transform + Parameters + ---------- + i + the index or slince - Example: + Returns + ------- + et + Elementary transform + Examples + -------- .. runblock:: pycon - - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> e[0] - >>> e[1] - >>> e[1:3] + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> e[0] + >>> e[1] + >>> e[1:3] """ return self.data[i] # can be [2] or slice, eg. [3:5] def __deepcopy__(self, memo): - new_data = [] for data in self: @@ -577,54 +642,56 @@ def __deepcopy__(self, memo): return result def plot(self, *args, **kwargs): - from roboticstoolbox.robot.ERobot import ERobot, ERobot2 + from roboticstoolbox.robot.Robot import Robot, Robot2 if isinstance(self, ETS): - robot = ERobot(self) + robot = Robot(self) else: - robot = ERobot2(self) + robot = Robot2(self) robot.plot(*args, **kwargs) def teach(self, *args, **kwargs): - from roboticstoolbox.robot.ERobot import ERobot, ERobot2 + from roboticstoolbox.robot.Robot import Robot, Robot2 if isinstance(self, ETS): - robot = ERobot(self) + robot = Robot(self) else: - robot = ERobot2(self) + robot = Robot2(self) robot.teach(*args, **kwargs) - def random_q(self, i: int = 1) -> ndarray: + def random_q(self, i: int = 1) -> NDArray: """ Generate a random valid joint configuration - :param i: number of configurations to generate - Generates a random q vector within the joint limits defined by `self.qlim`. - Example: + Parameters + ---------- + i + number of configurations to generate + Examples + -------- .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.Panda() - >>> ets = robot.ets() - >>> q = ets.random_q() - >>> q + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.Panda() + >>> ets = robot.ets() + >>> q = ets.random_q() + >>> q """ if i == 1: - q = zeros(self.n) + q = np.zeros(self.n) for i in range(self.n): q[i] = uniform(self.qlim[0, i], self.qlim[1, i]) else: - q = zeros((i, self.n)) + q = np.zeros((i, self.n)) for j in range(i): for i in range(self.n): @@ -637,8 +704,6 @@ class ETS(BaseETS): """ This class implements an elementary transform sequence (ETS) for 3D - :param arg: Function to compute ET value - An instance can contain an elementary transform (ET) or an elementary transform sequence (ETS). It has list-like properties by subclassing UserList, which means we can perform indexing, slicing pop, insert, as well @@ -648,24 +713,39 @@ class ETS(BaseETS): - ``ETS(et)`` an ETS containing a single ET - ``ETS([et0, et1, et2])`` an ETS consisting of three ET's - Example: - - .. runblock:: pycon - - >>> from roboticstoolbox import ETS, ET - >>> e = ET.Rz(0.3) # a single ET, rotation about z - >>> ets1 = ETS(e) - >>> len(ets1) - >>> ets2 = ET.Rz(0.3) * ET.tx(2) # an ETS - >>> len(ets2) # of length 2 - >>> ets2[1] # an ET sliced from the ETS + Parameters + ---------- + arg + Function to compute ET value + + Examples + -------- + .. runblock:: pycon + >>> from roboticstoolbox import ETS, ET + >>> e = ET.Rz(0.3) # a single ET, rotation about z + >>> ets1 = ETS(e) + >>> len(ets1) + >>> ets2 = ET.Rz(0.3) * ET.tx(2) # an ETS + >>> len(ets2) # of length 2 + >>> ets2[1] # an ET sliced from the ETS + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + + See Also + -------- + :func:`rx` + :func:`ry` + :func:`rz` + :func:`tx` + :func:`ty` + :func:`tz` - :references: - - Kinematic Derivatives using the Elementary Transform Sequence, - J. Haviland and P. Corke - - :seealso: :func:`rx`, :func:`ry`, :func:`rz`, :func:`tx`, - :func:`ty`, :func:`tz` """ def __init__( @@ -725,8 +805,9 @@ def __init__( elif jindices > 0 and not jindices == self.n: raise ValueError( - "You can not have some jindices set for the ET's in arg. It must be all or none" - ) + "You can not have some jindices set for the ET's in arg. It must be all" + " or none" + ) # pragma: nocover elif jindices == 0 and self.n > 0: # Set them ourself for j, joint in enumerate(joints): @@ -753,34 +834,37 @@ def compile(self) -> "ETS": """ Compile an ETS - :return: optimised ETS - Perform constant folding for faster evaluation. Consecutive constant ETs are compounded, leading to a constant ET which is denoted by ``SE3`` when displayed. - Example: + Returns + ------- + compile + optimised ETS + Examples + -------- .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.ETS.Panda() - >>> ets = robot.ets() - >>> ets - >>> ets.compile() - - :seealso: :func:`isconstant` + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.ETS.Panda() + >>> ets = robot.ets() + >>> ets + >>> ets.compile() + + See Also + -------- + :func:`isconstant` """ const = None ets = ETS() for et in self: - if et.isjoint: # a joint if const is not None: # flush the constant - if not array_equal(const, eye(4)): + if not np.array_equal(const, np.eye(4)): ets *= ET.SE3(const) const = None ets *= et # emit the joint ET @@ -793,7 +877,7 @@ def compile(self) -> "ETS": if const is not None: # flush the constant, tool transform - if not array_equal(const, eye(4)): + if not np.array_equal(const, np.eye(4)): ets *= ET.SE3(const) return ets @@ -805,21 +889,25 @@ def insert( """ Insert value - :param i: insert an ET or ETS into the ETS, default is at the end - :param arg: the elementary transform or sequence to insert - Inserts an ET or ETS into the ET sequence. The inserted value is at position ``i``. - Example: + Parameters + ---------- + i + insert an ET or ETS into the ETS, default is at the end + arg + the elementary transform or sequence to insert + Examples + -------- .. runblock:: pycon + >>> from roboticstoolbox import ET + >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) + >>> f = ET.Ry() + >>> e.insert(f, 2) + >>> e - >>> from roboticstoolbox import ET - >>> e = ET.Rz() * ET.tx(1) * ET.Rz() * ET.tx(1) - >>> f = ET.Ry() - >>> e.insert(f, 2) - >>> e """ if isinstance(arg, ET): @@ -839,42 +927,64 @@ def insert( def fkine( self, q: ArrayLike, - base: Union[ndarray, SE3, None] = None, - tool: Union[ndarray, SE3, None] = None, + base: Union[NDArray, SE3, None] = None, + tool: Union[NDArray, SE3, None] = None, include_base: bool = True, ) -> SE3: """ Forward kinematics - :param q: Joint coordinates - :type q: ArrayLike - :param base: base transform, optional - :param tool: tool transform, optional + ``T = ets.fkine(q)`` evaluates forward kinematics for the ets at + joint configuration ``q``. - :return: The transformation matrix representing the pose of the - end-effector - - - ``T = ets.fkine(q)`` evaluates forward kinematics for the robot at - joint configuration ``q``. **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a trajectory and the result is an ``SE3`` instance with ``m`` values. - .. note:: - - The robot's base tool transform, if set, is incorporated - into the result. - - A tool transform, if provided, is incorporated into the result. - - Works from the end-effector link to the base - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + + Attributes + ---------- + q + Joint coordinates + base + A base transform applied before the ETS + tool + tool transform, optional + include_base + set to True if the base transform should be considered + + Returns + ------- + The transformation matrix representing the pose of the + end-effector + + Examples + -------- + The following example makes a ``panda`` robot object, gets the ets, and + solves for the forward kinematics at the listed configuration. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + Notes + ----- + - A tool transform, if provided, is incorporated into the result. + - Works from the end-effector link to the base + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + + """ # noqa ret = SE3.Empty() fk = self.eval(q, base, tool, include_base) if fk.dtype == "O": # symbolic - fk = array(simplify(fk)) + fk = np.array(simplify(fk)) if fk.ndim == 3: for T in fk: @@ -887,35 +997,56 @@ def fkine( def eval( self, q: ArrayLike, - base: Union[ndarray, SE3, None] = None, - tool: Union[ndarray, SE3, None] = None, + base: Union[NDArray, SE3, None] = None, + tool: Union[NDArray, SE3, None] = None, include_base: bool = True, - ) -> ndarray: + ) -> NDArray: """ Forward kinematics - :param q: Joint coordinates - :type q: ArrayLike - :param base: base transform, optional - :param tool: tool transform, optional - - :return: The transformation matrix representing the pose of the - end-effector + ``T = ets.fkine(q)`` evaluates forward kinematics for the ets at + joint configuration ``q``. - - ``T = ets.fkine(q)`` evaluates forward kinematics for the robot at - joint configuration ``q``. **Trajectory operation**: If ``q`` has multiple rows (mxn), it is considered a trajectory and the result is an ``SE3`` instance with ``m`` values. - .. note:: - - The robot's base tool transform, if set, is incorporated - into the result. - - A tool transform, if provided, is incorporated into the result. - - Works from the end-effector link to the base - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + + Attributes + ---------- + q + Joint coordinates + base + A base transform applied before the ETS + tool + tool transform, optional + include_base + set to True if the base transform should be considered + Returns + ------- + The transformation matrix representing the pose of the + end-effector + + Examples + -------- + The following example makes a ``panda`` robot object, gets the ets, and + solves for the forward kinematics at the listed configuration. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + Notes + ----- + - A tool transform, if provided, is incorporated into the result. + - Works from the end-effector link to the base + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + + """ # noqa try: return ETS_fkine(self._fknm, q, base, tool, include_base) @@ -927,29 +1058,29 @@ def eval( end = self.data[-1] if isinstance(tool, SE3): - tool = array(tool.A) + tool = np.array(tool.A) if isinstance(base, SE3): - base = array(base.A) + base = np.array(base.A) if base is None: bases = None - elif array_equal(base, eye(3)): # pragma: nocover + elif np.array_equal(base, np.eye(3)): # pragma: nocover bases = None else: # pragma: nocover bases = base if tool is None: tools = None - elif array_equal(tool, eye(3)): # pragma: nocover + elif np.array_equal(tool, np.eye(3)): # pragma: nocover tools = None else: # pragma: nocover tools = tool if l > 1: - T = zeros((l, 4, 4), dtype=object) + T = np.zeros((l, 4, 4), dtype=object) else: - T = zeros((4, 4), dtype=object) + T = np.zeros((4, 4), dtype=object) # Tk = None @@ -974,7 +1105,7 @@ def eval( Tk = A @ Tk # add base transform if it is set - if include_base == True and bases is not None: + if include_base is True and bases is not None: Tk = bases @ Tk # append @@ -988,38 +1119,53 @@ def eval( def jacob0( self, q: ArrayLike, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: r""" - Jacobian in base frame - :param q: Joint coordinate vector - :type q: ArrayLike - :param tool: a static tool transformation matrix to apply to the + Manipulator geometric Jacobian in the base frame + + ``robot.jacobo(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + base frame. + + End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` + is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. + + Parameters + ---------- + q + Joint coordinate vector + tool + a static tool transformation matrix to apply to the end of end, defaults to None - :return J: Manipulator Jacobian in the base frame - ``jacob0(q)`` is the ETS Jacobian matrix which maps joint - velocity to spatial velocity in the {0} frame. - End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, - \omega_y, \omega_z)^T` is related to joint velocity by - :math:`{}^{e}\nu = {}^{e}\mathbf{J}_0(q) \dot{q}`. - If ``ets.eval(q)`` is already computed it can be passed in as ``T`` to - reduce computation time. - An ETS represents the relative pose from the {0} frame to the end frame - {e}. This is the composition of mAny relative poses, some constant and - some functions of the joint variables, which we can write as - :math:`\mathbf{E}(q)`. - .. math:: - {}^0 T_e = \mathbf{E}(q) \in \mbox{SE}(3) - The temporal derivative of this is the spatial - velocity :math:`\nu` which is a 6-vector is related to the rate of - change of joint coordinates by the Jacobian matrix. - .. math:: - {}^0 \nu = {}^0 \mathbf{J}(q) \dot{q} \in \mathbb{R}^6 - This velocity can be expressed relative to the {0} frame or the {e} - frame. - :references: - - `Kinematic Derivatives using the Elementary Transform Sequence, J. Haviland and P. Corke `_ - :seealso: :func:`jacobe`, :func:`hessian0` + + Returns + ------- + J0 + Manipulator Jacobian in the base frame + + Examples + -------- + The following example makes a ``Puma560`` robot object, and solves for the + base-frame Jacobian at the zero joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.Puma560().ets() + >>> puma.jacob0([0, 0, 0, 0, 0, 0]) + + Notes + ----- + - This is the geometric Jacobian as described in texts by + Corke, Spong etal., Siciliano etal. The end-effector velocity is + described in terms of translational and angular velocity, not a + velocity twist as per the text by Lynch & Park. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + """ # noqa # Use c extension @@ -1030,20 +1176,20 @@ def jacob0( # Otherwise use Python if tool is None: - tools = eye(4) + tools = np.eye(4) elif isinstance(tool, SE3): - tools = array(tool.A) + tools = np.array(tool.A) else: # pragma: nocover - tools = eye(4) + tools = np.eye(4) q = getvector(q, None) T = self.eval(q, include_base=False) @ tools - U = eye(4) + U = np.eye(4) j = 0 - J = zeros((6, self.n), dtype="object") - zero = array([0, 0, 0]) + J = np.zeros((6, self.n), dtype="object") + zero = np.array([0, 0, 0]) end = self.data[-1] for link in self.data: @@ -1098,28 +1244,58 @@ def jacob0( def jacobe( self, q: ArrayLike, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: r""" Manipulator geometric Jacobian in the end-effector frame - :param q: Joint coordinate vector - :type q: ArrayLike - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return J: Manipulator Jacobian in the end-effector frame + ``robot.jacobe(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + ``end`` frame. - - ``ets.jacobe(q)`` is the manipulator Jacobian matrix which maps - joint velocity to end-effector spatial velocity expressed in the - end-effector frame. End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. - .. warning:: This is the **geometric Jacobian** as described in texts by + Parameters + ---------- + q + Joint coordinate vector + end + the particular link or gripper whose velocity the Jacobian + describes, defaults to the end-effector if only one is present + start + the link considered as the base frame, defaults to the robots's base frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + Je + Manipulator Jacobian in the ``end`` frame + + Examples + -------- + The following example makes a ``Puma560`` robot object, and solves for the + end-effector frame Jacobian at the zero joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.Puma560().ets() + >>> puma.jacobe([0, 0, 0, 0, 0, 0]) + + Notes + ----- + - This is the geometric Jacobian as described in texts by Corke, Spong etal., Siciliano etal. The end-effector velocity is described in terms of translational and angular velocity, not a velocity twist as per the text by Lynch & Park. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + """ # noqa # Use c extension @@ -1134,27 +1310,35 @@ def jacobe( def hessian0( self, q: Union[ArrayLike, None] = None, - J0: Union[ndarray, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: + J0: Union[NDArray, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: r""" Manipulator Hessian The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the world-coordinate frame. This - function calulcates this based on the ETS of the robot. - - One of J0 or q + spatial acceleration, expressed in the base frame. This + function calulcates this based on the ETS of the robot. One of J0 or q is required. Supply J0 if already calculated to save computation time - :param q: The joint angles/configuration of the robot. - :type q: ArrayLike - :param J0: The manipulator Jacobian in the 0 frame - :param tool: a static tool transformation matrix to apply to the - end frame, defaults to None + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + J0 + The manipulator Jacobian in the base frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None - :return: The manipulator Hessian in 0 frame + Returns + ------- + h0 + The manipulator Hessian in the base frame + Synopsis + -------- This method computes the manipulator Hessian in the base frame. If we take the time derivative of the differential kinematic relationship .. math:: @@ -1165,18 +1349,35 @@ def hessian0( \dmat{J} = \mat{H} \dvec{q} and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the Hessian tensor. + The elements of the Hessian are .. math:: \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements of the spatial velocity vector. + Similarly, we can write .. math:: \mat{J}_{i,j} = \frac{d u_i}{d q_j} - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + base frame Hessian at the given joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> panda.hessian0([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa # Use c extension try: @@ -1188,7 +1389,7 @@ def cross(a, b): x = a[1] * b[2] - a[2] * b[1] y = a[2] * b[0] - a[0] * b[2] z = a[0] * b[1] - a[1] * b[0] - return array([x, y, z]) + return np.array([x, y, z]) n = self.n @@ -1198,11 +1399,10 @@ def cross(a, b): else: verifymatrix(J0, (6, self.n)) - H = zeros((n, 6, n)) + H = np.zeros((n, 6, n)) for j in range(n): for i in range(j, n): - H[j, :3, i] = cross(J0[3:, j], J0[:3, i]) H[j, 3:, i] = cross(J0[3:, j], J0[3:, i]) @@ -1214,28 +1414,36 @@ def cross(a, b): def hessiane( self, q: Union[ArrayLike, None] = None, - Je: Union[ndarray, None] = None, - tool: Union[ndarray, SE3, None] = None, - ) -> ndarray: + Je: Union[NDArray, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: r""" Manipulator Hessian The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the world-coordinate frame. This - function calulcates this based on the ETS of the robot. - - One of Je or q - is required. Supply Je if already calculated to save computation time + spatial acceleration, expressed in the end-effector coordinate frame. This + function calulcates this based on the ETS of the robot. One of J0 or q + is required. Supply J0 if already calculated to save computation time - :param q: The joint angles/configuration of the robot. - :type q: ArrayLike - :param Je: The manipulator Jacobian in the ee frame - :param tool: a static tool transformation matrix to apply to the - end frame, defaults to None + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + J0 + The manipulator Jacobian in the end-effector frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None - :return: The manipulator Hessian in ee frame + Returns + ------- + he + The manipulator Hessian in end-effector frame - This method computes the manipulator Hessian in the ee frame. If + Synopsis + -------- + This method computes the manipulator Hessian in the end-effector frame. If we take the time derivative of the differential kinematic relationship .. math:: \nu &= \mat{J}(\vec{q}) \dvec{q} \\ @@ -1245,18 +1453,35 @@ def hessiane( \dmat{J} = \mat{H} \dvec{q} and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the Hessian tensor. + The elements of the Hessian are .. math:: \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements of the spatial velocity vector. + Similarly, we can write .. math:: \mat{J}_{i,j} = \frac{d u_i}{d q_j} - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + end-effector frame Hessian at the given joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> panda.hessiane([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa # Use c extension try: @@ -1268,7 +1493,7 @@ def cross(a, b): x = a[1] * b[2] - a[2] * b[1] y = a[2] * b[0] - a[0] * b[2] z = a[0] * b[1] - a[1] * b[0] - return array([x, y, z]) + return np.array([x, y, z]) n = self.n @@ -1278,11 +1503,10 @@ def cross(a, b): else: verifymatrix(Je, (6, self.n)) - H = zeros((n, 6, n)) + H = np.zeros((n, 6, n)) for j in range(n): for i in range(j, n): - H[j, :3, i] = cross(Je[3:, j], Je[:3, i]) H[j, 3:, i] = cross(Je[3:, j], Je[3:, i]) @@ -1295,36 +1519,51 @@ def jacob0_analytical( self, q: ArrayLike, representation: str = "rpy/xyz", - tool: Union[ndarray, SE3, None] = None, + tool: Union[NDArray, SE3, None] = None, ): r""" Manipulator analytical Jacobian in the base frame - :param q: Joint coordinate vector - :type q: ArrayLike - :param tool: a static tool transformation matrix to apply to the + ``robot.jacob0_analytical(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + base frame. + + Parameters + ---------- + q + Joint coordinate vector + representation + angular representation + tool + a static tool transformation matrix to apply to the end of end, defaults to None - :param representation: describes the rotational representation - :return J: Manipulator Jacobian in the base frame + Returns + ------- + jacob0 + Manipulator Jacobian in the base frame + Synopsis + -------- End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. - ================== ================================== - ``representation`` Rotational representation - ================== ================================== - ``'rpy/xyz'`` RPY angular rates in XYZ order - ``'rpy/zyx'`` RPY angular rates in XYZ order - ``'eul'`` Euler angular rates in ZYZ order - ``'exp'`` exponential coordinate rates - ================== ================================== + |``representation`` | Rotational representation | + |---------------------|-------------------------------------| + |``'rpy/xyz'`` | RPY angular rates in XYZ order | + |``'rpy/zyx'`` | RPY angular rates in XYZ order | + |``'eul'`` | Euler angular rates in ZYZ order | + |``'exp'`` | exponential coordinate rates | + + Examples + -------- + Makes a robot object and computes the analytic Jacobian for the given + joint configuration - Example: .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.ETS.Puma560() - >>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0]) + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.ETS.Puma560().ets() + >>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0]) """ # noqa @@ -1333,17 +1572,27 @@ def jacob0_analytical( A = rotvelxform(t2r(T), full=True, inverse=True, representation=representation) return A @ J - def jacobm(self, q: ArrayLike) -> ndarray: + def jacobm(self, q: ArrayLike) -> NDArray: r""" - Calculates the manipulability Jacobian. This measure relates the rate - of change of the manipulability to the joint velocities of the robot. + The manipulability Jacobian + + This measure relates the rate of change of the manipulability to the + joint velocities of the robot. One of J or q is required. Supply J + and H if already calculated to save computation time - :param q: The joint angles/configuration of the robot (Optional, + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, if not supplied will use the stored q values). - :return: The manipulability Jacobian - :rtype: float ndarray(n) + Returns + ------- + jacobm + The manipulability Jacobian + Synopsis + -------- Yoshikawa's manipulability measure .. math:: @@ -1356,10 +1605,14 @@ def jacobm(self, q: ArrayLike) -> ndarray: \frac{\partial m(\vec{q})}{\partial \vec{q}} - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa J = self.jacob0(q) H = self.hessian0(q) @@ -1370,7 +1623,7 @@ def jacobm(self, q: ArrayLike) -> ndarray: # H = H[:, axes, :] b = inv(J @ J.T) - Jm = zeros((self.n, 1)) + Jm = np.zeros((self.n, 1)) for i in range(self.n): c = J @ H[i, :, :].T @@ -1378,50 +1631,50 @@ def jacobm(self, q: ArrayLike) -> ndarray: return Jm - def manipulability(self, q, method="yoshikawa"): + def manipulability( + self, + q, + method: L["yoshikawa", "minsingular", "invcondition"] = "yoshikawa", + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + ): """ Manipulability measure - :param q: Joint coordinates, one of J or q required - :type q: ndarray(n), or ndarray(m,n) - :param J: Jacobian in world frame if already computed, one of J or - q required - :type J: ndarray(6,n) - :param method: method to use, "yoshikawa" (default), "condition", - "minsingular" or "asada" - :type method: str - :param axes: Task space axes to consider: "all" [default], - "trans", "rot" or "both" - :type axes: str - :param kwargs: extra arguments to pass to ``jacob0`` - :return: manipulability - :rtype: float or ndarray(m) - - - ``manipulability(q)`` is the scalar manipulability index - for the robot at the joint configuration ``q``. It indicates - dexterity, that is, how well conditioned the robot is for motion - with respect to the 6 degrees of Cartesian motion. The values is - zero if the robot is at a singularity. + ``manipulability(q)`` is the scalar manipulability index + for the ets at the joint configuration ``q``. It indicates + dexterity, that is, how well conditioned the ets is for motion + with respect to the 6 degrees of Cartesian motion. The values is + zero if the ets is at a singularity. + + Parameters + ---------- + q + Joint coordinates, one of J or q required + method + method to use, "yoshikawa" (default), "invcondition", + "minsingular" + axes + Task space axes to consider: "all" [default], + "trans", or "rot" + + Returns + ------- + manipulability + the manipulability metric + + Synopsis + -------- Various measures are supported: - +-------------------+-------------------------------------------------+ | Measure | Description | - +-------------------+-------------------------------------------------+ + |-------------------|-------------------------------------------------| | ``"yoshikawa"`` | Volume of the velocity ellipsoid, *distance* | | | from singularity [Yoshikawa85]_ | - +-------------------+-------------------------------------------------+ | ``"invcondition"``| Inverse condition number of Jacobian, isotropy | | | of the velocity ellipsoid [Klein87]_ | - +-------------------+-------------------------------------------------+ | ``"minsingular"`` | Minimum singular value of the Jacobian, | | | *distance* from singularity [Klein87]_ | - +-------------------+-------------------------------------------------+ - | ``"asada"`` | Isotropy of the task-space acceleration | - | | ellipsoid which is a function of the Cartesian | - | | inertia matrix which depends on the inertial | - | | parameters [Asada83]_ | - +-------------------+-------------------------------------------------+ **Trajectory operation**: @@ -1429,36 +1682,48 @@ def manipulability(self, q, method="yoshikawa"): manipulability indices for each joint configuration specified by a row of ``q``. - .. note:: - - - Invokes the ``jacob0`` method of the robot if ``J`` is not passed - - The "all" option includes rotational and translational - dexterity, but this involves adding different units. It can be - more useful to look at the translational and rotational - manipulability separately. - - Examples in the RVC book (1st edition) can be replicated by - using the "all" option - - Asada's measure requires inertial a robot model with inertial - parameters. - - :references: - + Notes + ----- + - Invokes the ``jacob0`` method of the robot if ``J`` is not passed + - The "all" option includes rotational and translational + dexterity, but this involves adding different units. It can be + more useful to look at the translational and rotational + manipulability separately. + - Examples in the RVC book (1st edition) can be replicated by + using the "all" option + - Asada's measure requires inertial a robot model with inertial + parameters. + + References + ---------- .. [Yoshikawa85] Manipulability of Robotic Mechanisms. Yoshikawa T., The International Journal of Robotics Research. 1985;4(2):3-9. doi:10.1177/027836498500400201 - .. [Asada83] A geometrical representation of manipulator dynamics and - its application to arm design, H. Asada, - Journal of Dynamic Systems, Measurement, and Control, - vol. 105, p. 131, 1983. .. [Klein87] Dexterity Measures for the Design and Control of Kinematically Redundant Manipulators. Klein CA, Blaho BE. The International Journal of Robotics Research. 1987;6(2):72-83. doi:10.1177/027836498700600206 + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 7. + - - Robotics, Vision & Control, Chap 8, P. Corke, Springer 2011. + .. versionchanged:: 1.0.4 + Removed 'both' option for axes, added a custom list option. """ + axes_list: List[bool] = [] + + if isinstance(axes, list): + axes_list = axes + elif axes == "all": + axes_list = [True, True, True, True, True, True] + elif axes.startswith("trans"): + axes_list = [True, True, True, False, False, False] + elif axes.startswith("rot"): + axes_list = [False, False, False, True, True, True] + else: + raise ValueError("axes must be all, trans, rot or both") + def yoshikawa(robot, J, q, axes, **kwargs): J = J[axes, :] if J.shape[0] == J.shape[1]: @@ -1466,7 +1731,7 @@ def yoshikawa(robot, J, q, axes, **kwargs): return abs(det(J)) else: m2 = det(J @ J.T) - return sqrt(abs(m2)) + return np.sqrt(abs(m2)) def condition(robot, J, q, axes, **kwargs): J = J[axes, :] @@ -1477,18 +1742,6 @@ def minsingular(robot, J, q, axes, **kwargs): s = svd(J, compute_uv=False) return s[-1] # return last/smallest singular value of J - def asada(robot, J, q, axes, **kwargs): - # dof = np.sum(axes) - if matrix_rank(J) < 6: - return 0 - Ji = pinv(J) - Mx = Ji.T @ robot.inertia(q) @ Ji - d = where(axes)[0] - Mx = Mx[d] - Mx = Mx[:, d.tolist()] - e, _ = eig(Mx) - return min(e) / max(e) - # choose the handler function if method == "yoshikawa": mfunc = yoshikawa @@ -1496,47 +1749,68 @@ def asada(robot, J, q, axes, **kwargs): mfunc = condition elif method == "minsingular": mfunc = minsingular - elif method == "asada": - mfunc = asada else: raise ValueError("Invalid method chosen") - # q = getmatrix(q, (None, self.n)) - # w = zeros(q.shape[0]) - axes = [True, True, True, True, True, True] + # Otherwise use the q vector/matrix + q = np.array(getmatrix(q, (None, self.n))) + w = np.zeros(q.shape[0]) - # for k, qk in enumerate(q): - J = self.jacob0(q) - w = mfunc(self, J, q, axes) + for k, qk in enumerate(q): + Jk = self.jacob0(qk) + w[k] = mfunc(self, Jk, qk, axes_list) - # if len(w) == 1: - # return w[0] - # else: - return w + if len(w) == 1: + return w[0] + else: + return w - def partial_fkine0(self, q: ArrayLike, n: int) -> ndarray: + def partial_fkine0(self, q: ArrayLike, n: int) -> NDArray: r""" Manipulator Forward Kinematics nth Partial Derivative - The manipulator Hessian tensor maps joint acceleration to end-effector - spatial acceleration, expressed in the ee frame. This - function calulcates this based on the ETS of the robot. One of Je or q - is required. Supply Je if already calculated to save computation time + This method computes the nth derivative of the forward kinematics where ``n`` is + greater than or equal to 3. This is an extension of the differential kinematics + where the Jacobian is the first partial derivative and the Hessian is the + second. - :param q: The joint angles/configuration of the robot (Optional, + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, if not supplied will use the stored q values). - :type q: ArrayLike - :param end: the final link/Gripper which the Hessian represents - :param start: the first link which the Hessian represents - :param tool: a static tool transformation matrix to apply to the + end + the final link/Gripper which the Hessian represents + start + the first link which the Hessian represents + tool + a static tool transformation matrix to apply to the end of end, defaults to None - :return: The nth Partial Derivative of the forward kinematics + Returns + ------- + A + The nth Partial Derivative of the forward kinematics - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + base-effector frame 4th defivative of the forward kinematics at the given + joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> panda.partial_fkine0([0, -0.3, 0, -2.2, 0, 2, 0.7854], n=4) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa # Calculate the Jacobian and Hessian J = self.jacob0(q) @@ -1553,7 +1827,7 @@ def partial_fkine0(self, q: ArrayLike, n: int) -> ndarray: # we are calculating # It stores the indices in the order: "j, k, l. m, n, o, ..." # where count is extended to match oder of the partial derivative - count = array([0, 0]) + count = np.array([0, 0]) # The order of derivative for which we are calculating # The Hessian is the 2nd-order so we start with c = 2 @@ -1606,19 +1880,22 @@ def add_pdi(pdi): # The length of dT correspods to the number of derivatives we have calculated while len(dT) != n: - # Add to the start of the tensor size list size.insert(0, self.n) # Add an axis to the count array - count = concatenate(([0], count)) + count = np.concatenate(([0], count)) - # This variables corresponds to indices within the previous partial derivatives + # This variables corresponds to indices within the previous + # partial derivatives # to be cross prodded # The order is: "[j, k, l, m, n, o, ...]" - # Although, our partial derivatives have the order: pd[..., o, n, m, l, k, cartesian DoF, j] - # For example, consider the Hessian Tensor H[n, 6, n], the index H[k, :, j]. This corrsponds - # to the second partial derivative of the kinematics of joint j with respect to joint k. + # Although, our partial derivatives have the order: + # pd[..., o, n, m, l, k, cartesian DoF, j] + # For example, consider the Hessian Tensor H[n, 6, n], + # the index H[k, :, j]. This corrsponds + # to the second partial derivative of the kinematics of joint j with + # respect to joint k. indices = add_indices(indices, c) # This variable corresponds to the indices in Td which corresponds to the @@ -1628,17 +1905,17 @@ def add_pdi(pdi): c += 1 # Allocate our new partial derivative tensor - pd = zeros(size) + pd = np.zeros(size) # We need to loop n^c times # There are n^c columns to calculate for _ in range(self.n**c): - # Allocate the rotation and translation components - rot = zeros(3) - trn = zeros(3) + rot = np.zeros(3) + trn = np.zeros(3) - # This loop calculates a single column ([trn, rot]) of the tensor for dT(x) + # This loop calculates a single column ([trn, rot]) + # of the tensor for dT(x) for j in range(len(indices)): pdr0 = dT[pdi[j][0]] pdr1 = dT[pdi[j][1]] @@ -1646,11 +1923,13 @@ def add_pdi(pdi): idx0 = count[indices[j][0]] idx1 = count[indices[j][1]] - # This is a list of indices selecting the slices of the previous tensor - idx0_slices = flip(idx0[1:]) - idx1_slices = flip(idx1[1:]) + # This is a list of indices selecting the slices of the + # previous tensor + idx0_slices = np.flip(idx0[1:]) + idx1_slices = np.flip(idx1[1:]) - # This index selecting the column within the 2d slice of the previous tensor + # This index selecting the column within the 2d slice of the + # previous tensor idx0_n = idx0[0] idx1_n = idx1[0] @@ -1662,11 +1941,11 @@ def add_pdi(pdi): col1_trn = pdr1[(*idx1_slices, slice(0, 3), idx1_n)] # Perform the cross product as described in the maths above - rot += cross(col0_rot, col1_rot) - trn += cross(col0_rot, col1_trn) + rot += np.cross(col0_rot, col1_rot) + trn += np.cross(col0_rot, col1_trn) - pd[(*flip(count[1:]), slice(0, 3), count[0])] = trn - pd[(*flip(count[1:]), slice(3, 6), count[0])] = rot + pd[(*np.flip(count[1:]), slice(0, 3), count[0])] = trn + pd[(*np.flip(count[1:]), slice(3, 6), count[0])] = rot count[0] += 1 for j in range(len(count)): @@ -1679,145 +1958,222 @@ def add_pdi(pdi): return dT[-1] - def ik_lm_chan( + def ik_LM( self, - Tep: Union[ndarray, SE3], - q0: Union[ndarray, None] = None, + Tep: Union[NDArray, SE3], + q0: Union[NDArray, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + k: float = 1.0, + method: L["chan", "wampler", "sugihara"] = "chan", + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast levenberg-Marquadt Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Levemberg-Marquadt method. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + k + Sets the gain value for the damping matrix Wn in the next iteration. See + synopsis + method + One of "chan", "sugihara" or "wampler". Defines which method is used + to calculate the damping matrix Wn in the ``step`` method + + Synopsis + -------- + The operation is defined by the choice of the ``method`` kwarg. + + The step is deined as - :param Tep: The desired end-effector pose or pose trajectory - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn + .. math:: - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) + \vec{q}_{k+1} + &= + \vec{q}_k + + \left( + \mat{A}_k + \right)^{-1} + \bf{g}_k \\ + % + \mat{A}_k + &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + + + \mat{W}_n + + where :math:`\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})` is a + diagonal damping matrix. The damping matrix ensures that :math:`\mat{A}_k` is + non-singular and positive definite. The performance of the LM method largely depends + on the choice of :math:`\mat{W}_n`. + + *Chan's Method* + + Chan proposed - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: + .. math:: - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== + \mat{W}_n + = + λ E_k \mat{1}_n - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. + where λ is a constant which reportedly does not have much influence on performance. + Use the kwarg `k` to adjust the weighting term λ. - **Joint Limits**: + *Sugihara's Method* - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. + Sugihara proposed - **Global search**: + .. math:: - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. + \mat{W}_n + = + E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n) - **Underactuated robots:** + where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`, + and :math:`l` is the length of a typical link within the manipulator. We provide the + variable `k` as a kwarg to adjust the value of :math:`w_n`. - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. + *Wampler's Method* - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. + Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg. - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_LM` method. + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_LM(Tep) + Notes + ----- + The value for the ``k`` kwarg will depend on the ``method`` chosen and the arm you are + using. Use the following as a rough guide ``chan, k = 1.0 - 0.01``, + ``wampler, k = 0.01 - 0.0001``, and ``sugihara, k = 0.1 - 0.0001`` - .. note:: + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. - :references: - TODO + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). - :seealso: - TODO - """ + See Also + -------- + ik_NR + A fast numerical inverse kinematics solver using Newton-Raphson optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation + + + .. versionchanged:: 1.0.4 + Merged the Levemberg-Marquadt IK solvers into the ik_LM method + + """ # noqa - return IK_LM_Chan_c(self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ) + return IK_LM_c( + self._fknm, Tep, q0, ilimit, slimit, tol, joint_limits, mask, k, method + ) - def ik_lm_wampler( + def ik_NR( self, - Tep: Union[ndarray, SE3], - q0: Union[ndarray, None] = None, + Tep: Union[NDArray, SE3], + q0: Union[NDArray, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + pinv: int = True, + pinv_damping: float = 0.0, + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast numerical inverse kinematics using Newton-Raphson optimization - :param Tep: The desired end-effector pose or pose trajectory - :param q0: initial joint configuration (default to random valid joint + ``sol = ets.ik_NR(Tep)`` are the joint coordinates (n) corresponding + to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. + This method can be used for robots with any number of degrees of freedom. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose or pose trajectory + q0 + initial joint configuration (default to random valid joint configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. + ilimit + maximum number of iterations per search + slimit + maximum number of search attempts + tol + final error tolerance + mask + a mask vector which weights the end-effector error priority. Corresponds to translation in X, Y and Z and rotation about X, Y and Z respectively - :param λ: value of lambda for the damping matrix Wn + joint_limits + constrain the solution to being within the joint limits of + the robot (reject solution with invalid joint configurations and perfrom + another search up to the slimit) + pinv + Use the psuedo-inverse instad of the normal matrix inverse + pinv_damping + Damping factor for the psuedo-inverse - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) + Returns + ------- + sol + tuple (q, success, iterations, searches, residual) - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. The return value ``sol`` is a tuple with elements: ============ ========== =============================================== @@ -1834,97 +2190,118 @@ def ik_lm_wampler( solution will be in error. The amount of error is indicated by the ``residual``. - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: - - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - + Synopsis + -------- + Each iteration uses the Newton-Raphson optimisation method + .. math:: - .. note:: + \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. - :references: - TODO + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ik_NR(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + ik_LM + A fast numerical inverse kinematics solver using Levenberg-Marquadt optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation - :seealso: - TODO - """ + """ # noqa - return IK_LM_Wampler_c( - self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ + return IK_NR_c( + self._fknm, + Tep, + q0, + ilimit, + slimit, + tol, + joint_limits, + mask, + pinv, + pinv_damping, ) - def ik_lm_sugihara( + def ik_GN( self, - Tep: Union[ndarray, SE3], - q0: Union[ndarray, None] = None, + Tep: Union[NDArray, SE3], + q0: Union[NDArray, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - λ: float = 1.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + pinv: int = True, + pinv_damping: float = 0.0, + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast numerical inverse kinematics by Gauss-Newton optimization - :param Tep: The desired end-effector pose or pose trajectory - :param q0: initial joint configuration (default to random valid joint + ``sol = ets.ik_GN(Tep)`` are the joint coordinates (n) corresponding + to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. + This method can be used for robots with any number of degrees of freedom. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose or pose trajectory + q0 + initial joint configuration (default to random valid joint configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. + ilimit + maximum number of iterations per search + slimit + maximum number of search attempts + tol + final error tolerance + mask + a mask vector which weights the end-effector error priority. Corresponds to translation in X, Y and Z and rotation about X, Y and Z respectively - :param λ: value of lambda for the damping matrix Wn + joint_limits + constrain the solution to being within the joint limits of + the robot (reject solution with invalid joint configurations and perfrom + another search up to the slimit) + pinv + Use the psuedo-inverse instad of the normal matrix inverse + pinv_damping + Damping factor for the psuedo-inverse - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) + Returns + ------- + sol + tuple (q, success, iterations, searches, residual) - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. The return value ``sol`` is a tuple with elements: ============ ========== =============================================== @@ -1941,319 +2318,758 @@ def ik_lm_sugihara( solution will be in error. The amount of error is indicated by the ``residual``. - **Joint Limits**: - - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. - - **Global search**: + Synopsis + -------- + Each iteration uses the Gauss-Newton optimisation method - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. - - **Underactuated robots:** - - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. - - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. - - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. - - - - .. note:: + .. math:: - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. + \vec{q}_{k+1} &= \vec{q}_k + + \left( + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + \right)^{-1} + \bf{g}_k \\ + \bf{g}_k &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e + \vec{e}_k + + where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If + :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then + the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)` + is singular, the above can not be computed and the GN solution is infeasible. + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. - :references: - TODO + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ik_GN(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + ik_NR + A fast numerical inverse kinematics solver using Newton-Raphson optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation - :seealso: - TODO - """ + """ # noqa - return IK_LM_Sugihara_c( - self._fknm, Tep, q0, ilimit, slimit, tol, reject_jl, we, λ + return IK_GN_c( + self._fknm, + Tep, + q0, + ilimit, + slimit, + tol, + joint_limits, + mask, + pinv, + pinv_damping, ) - def ik_nr( + def ikine_LM( self, - Tep: Union[ndarray, SE3], - q0: Union[ndarray, None] = None, + Tep: Union[NDArray, SE3], + q0: Union[ArrayLike, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - use_pinv: int = True, - pinv_damping: float = 0.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + k: float = 1.0, + method: L["chan", "wampler", "sugihara"] = "chan", + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Levemberg-Marquadt Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Levemberg-Marquadt method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + k + Sets the gain value for the damping matrix Wn in the next iteration. See + synopsis + method + One of "chan", "sugihara" or "wampler". Defines which method is used + to calculate the damping matrix Wn in the ``step`` method + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + The operation is defined by the choice of the ``method`` kwarg. + + The step is deined as - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: + .. math:: - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== + \vec{q}_{k+1} + &= + \vec{q}_k + + \left( + \mat{A}_k + \right)^{-1} + \bf{g}_k \\ + % + \mat{A}_k + &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + + + \mat{W}_n + + where :math:`\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})` is a + diagonal damping matrix. The damping matrix ensures that :math:`\mat{A}_k` is + non-singular and positive definite. The performance of the LM method largely depends + on the choice of :math:`\mat{W}_n`. + + *Chan's Method* + + Chan proposed - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. + .. math:: - **Joint Limits**: + \mat{W}_n + = + λ E_k \mat{1}_n - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. + where λ is a constant which reportedly does not have much influence on performance. + Use the kwarg `k` to adjust the weighting term λ. - **Global search**: + *Sugihara's Method* - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. + Sugihara proposed - **Underactuated robots:** + .. math:: - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. + \mat{W}_n + = + E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n) - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. + where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`, + and :math:`l` is the length of a typical link within the manipulator. We provide the + variable `k` as a kwarg to adjust the value of :math:`w_n`. - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. + *Wampler's Method* + Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg. + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_LM` method. - .. note:: + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_LM(Tep) + + Notes + ----- + The value for the ``k`` kwarg will depend on the ``method`` chosen and the arm you are + using. Use the following as a rough guide ``chan, k = 1.0 - 0.01``, + ``wampler, k = 0.01 - 0.0001``, and ``sugihara, k = 0.1 - 0.0001`` + + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_LM` + An IK Solver class which implements the Levemberg Marquadt optimisation technique + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`ETS` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`ETS` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Levemberg-Marquadt IK solver method on the `ETS` class - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. + """ # noqa - :references: - TODO + solver = IK_LM( + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + k=k, + method=method, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) - :seealso: - TODO - """ + # if isinstance(Tep, SE3): + # Tep = Tep.A - return IK_NR_c( - self._fknm, - Tep, - q0, - ilimit, - slimit, - tol, - reject_jl, - we, - use_pinv, - pinv_damping, - ) + return solver.solve(ets=self, Tep=Tep, q0=q0) - def ik_gn( + def ikine_NR( self, - Tep: Union[ndarray, SE3], - q0: Union[ndarray, None] = None, + Tep: Union[NDArray, SE3], + q0: Union[ArrayLike, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - reject_jl: bool = True, - we: Union[ndarray, None] = None, - use_pinv: int = True, - pinv_damping: float = 0.0, - ) -> Tuple[ndarray, int, int, int, float]: - """ - Numerical inverse kinematics by Levenberg-Marquadt optimization (Chan's Method) - - :param Tep: The desired end-effector pose or pose trajectory - :param q0: initial joint configuration (default to random valid joint - configuration contrained by the joint limits of the robot) - :param ilimit: maximum number of iterations per search - :param slimit: maximum number of search attempts - :param tol: final error tolerance - :param reject_jl: constrain the solution to being within the joint limits of - the robot (reject solution with invalid joint configurations and perfrom - another search up to the slimit) - :param we: a mask vector which weights the end-effector error priority. - Corresponds to translation in X, Y and Z and rotation about X, Y and Z - respectively - :param λ: value of lambda for the damping matrix Wn - - :return: inverse kinematic solution - :rtype: tuple (q, success, iterations, searches, residual) - - ``sol = ets.ik_lm_chan(Tep)`` are the joint coordinates (n) corresponding - to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. - This method can be used for robots with any number of degrees of freedom. - The return value ``sol`` is a tuple with elements: - - ============ ========== =============================================== - Element Type Description - ============ ========== =============================================== - ``q`` ndarray(n) joint coordinates in units of radians or metres - ``success`` int whether a solution was found - ``iterations`` int total number of iterations - ``searches`` int total number of searches - ``residual`` float final value of cost function - ============ ========== =============================================== + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Newton-Raphson Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Newton-Raphson method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + Each iteration uses the Newton-Raphson optimisation method - If ``success == 0`` the ``q`` values will be valid numbers, but the - solution will be in error. The amount of error is indicated by - the ``residual``. + .. math:: - **Joint Limits**: + \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k - ``sol = robot.ikine_LM(T, slimit=100)`` which is the deafualt for this method. - The solver will initialise a solution attempt with a random valid q0 and - perform a maximum of ilimit steps within this attempt. If a solution is not - found, this process is repeated up to slimit times. + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_NR` method. - **Global search**: + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_NR(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`ETS` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Newton-Raphson IK solver method on the `ETS` class - ``sol = robot.ikine_LM(T, reject_jl=True)`` is the deafualt for this method. - By setting reject_jl to True, the solver will discard any solution which - violates the defined joint limits of the robot. The solver will then - re-initialise with a new random q0 and repeat the process up to slimit times. - Note that finding a solution with valid joint coordinates takes longer than - without. + """ # noqa - **Underactuated robots:** + solver = IK_NR( + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + pinv=pinv, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) - For the case where the manipulator has fewer than 6 DOF the - solution space has more dimensions than can be spanned by the - manipulator joint coordinates. + # if isinstance(Tep, SE3): + # Tep = Tep.A - In this case we specify the ``we`` option where the ``we`` vector - (6) specifies the Cartesian DOF (in the wrist coordinate frame) that - will be ignored in reaching a solution. The we vector has six - elements that correspond to translation in X, Y and Z, and rotation - about X, Y and Z respectively. The value can be 0 (for ignore) - or above to assign a priority relative to other Cartesian DoF. The number - of non-zero elements must equal the number of manipulator DOF. + return solver.solve(ets=self, Tep=Tep, q0=q0) - For example when using a 3 DOF manipulator tool orientation might - be unimportant, in which case use the option ``we=[1, 1, 1, 0, 0, 0]``. + def ikine_GN( + self, + Tep: Union[NDArray, SE3], + q0: Union[ArrayLike, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Gauss-Newton Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Gauss-Newton method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + Each iteration uses the Gauss-Newton optimisation method + .. math:: + \vec{q}_{k+1} &= \vec{q}_k + + \left( + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + \right)^{-1} + \bf{g}_k \\ + \bf{g}_k &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e + \vec{e}_k + + where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If + :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then + the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)` + is singular, the above can not be computed and the GN solution is infeasible. + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. - .. note:: + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_GN(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`ETS` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Gauss-Newton IK solver method on the `ETS` class - - See `Toolbox kinematics wiki page - `_ - - Implements a Levenberg-Marquadt variable-damping solver. - - The tolerance is computed on the norm of the error between - current and desired tool pose. This norm is computed from - distances and angles without any kind of weighting. - - The inverse kinematic solution is generally not unique, and - depends on the initial guess ``q0``. + """ # noqa - :references: - TODO + solver = IK_GN( + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + pinv=pinv, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) - :seealso: - TODO - """ + # if isinstance(Tep, SE3): + # Tep = Tep.A - return IK_GN_c( - self._fknm, - Tep, - q0, - ilimit, - slimit, - tol, - reject_jl, - we, - use_pinv, - pinv_damping, - ) + return solver.solve(ets=self, Tep=Tep, q0=q0) - def ikine_LM( + def ikine_QP( self, - Tep: Union[ndarray, SE3], + Tep: Union[NDArray, SE3], q0: Union[ArrayLike, None] = None, ilimit: int = 30, slimit: int = 100, tol: float = 1e-6, - joint_limits: bool = False, mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, seed: Union[int, None] = None, + kj=1.0, + ks=1.0, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, ): - solver = IK_LM( + r""" + Quadratic Programming Numerical Inverse Kinematics Solver + + A method that provides functionality to perform numerical inverse kinematics + (IK) using a quadratic progamming approach. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + kj + A gain for joint velocity norm minimisation + ks + A gain which adjusts the cost of slack (intentional error) + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Raises + ------ + ImportError + If the package ``qpsolvers`` is not installed + + Synopsis + -------- + Each iteration uses the following approach + + .. math:: + + \vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}. + + where the QP is defined as + + .. math:: + + \min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\ + \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu}, \\ + \mathcal{A} \vec{x} &\leq \mathcal{B}, \\ + \vec{x}^- &\leq \vec{x} \leq \vec{x}^+ + + with + + .. math:: + + \vec{x} &= + \begin{pmatrix} + \dvec{q} \\ \vec{\delta} + \end{pmatrix} \in \mathbb{R}^{(n+6)} \\ + \mathcal{Q} &= + \begin{pmatrix} + \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\ + \mathcal{J} &= + \begin{pmatrix} + \mat{J}(\vec{q}) & \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\ + \mathcal{C} &= + \begin{pmatrix} + \mat{J}_m \\ \bf{0}_{6 \times 1} + \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\ + \mathcal{A} &= + \begin{pmatrix} + \mat{1}_{n \times n + 6} \\ + \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\ + \mathcal{B} &= + \eta + \begin{pmatrix} + \frac{\rho_0 - \rho_s} + {\rho_i - \rho_s} \\ + \vdots \\ + \frac{\rho_n - \rho_s} + {\rho_i - \rho_s} + \end{pmatrix} \in \mathbb{R}^{n} \\ + \vec{x}^{-, +} &= + \begin{pmatrix} + \dvec{q}^{-, +} \\ + \vec{\delta}^{-, +} + \end{pmatrix} \in \mathbb{R}^{(n+6)}, + + where :math:`\vec{\delta} \in \mathbb{R}^6` is the slack vector, + :math:`\lambda_\delta \in \mathbb{R}^+` is a gain term which adjusts the + cost of the norm of the slack vector in the optimiser, + :math:`\dvec{q}^{-,+}` are the minimum and maximum joint velocities, and + :math:`\dvec{\delta}^{-,+}` are the minimum and maximum slack velocities. + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_QP` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_QP(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`ETS` class + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Quadratic Programming IK solver method on the `ETS` class + + """ # noqa: E501 + + solver = IK_QP( ilimit=ilimit, slimit=slimit, tol=tol, joint_limits=joint_limits, mask=mask, seed=seed, + kj=kj, + ks=ks, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, ) - if isinstance(Tep, SE3): - Tep = Tep.A + # if isinstance(Tep, SE3): + # Tep = Tep.A return solver.solve(ets=self, Tep=Tep, q0=q0) @@ -2354,8 +3170,9 @@ def __init__( self._auto_jindex = True elif jindices > 0 and not jindices == self.n: raise ValueError( - "You can not have some jindices set for the ET's in arg. It must be all or none" - ) + "You can not have some jindices set for the ET's in arg. It must be all" + " or none" + ) # pragma: nocover elif jindices == 0 and self.n > 0: # Set them ourself for j, joint in enumerate(joints): @@ -2393,12 +3210,11 @@ def compile(self) -> "ETS2": ets = ETS2() for et in self: - if et.isjoint: # a joint if const is not None: # flush the constant - if not array_equal(const, eye(3)): + if not np.array_equal(const, np.eye(3)): ets *= ET2.SE2(const) const = None ets *= et # emit the joint ET @@ -2411,7 +3227,7 @@ def compile(self) -> "ETS2": if const is not None: # flush the constant, tool transform - if not array_equal(const, eye(3)): + if not np.array_equal(const, np.eye(3)): ets *= ET2.SE2(const) return ets @@ -2457,8 +3273,8 @@ def insert( def fkine( self, q: ArrayLike, - base: Union[ndarray, SE2, None] = None, - tool: Union[ndarray, SE2, None] = None, + base: Union[NDArray, SE2, None] = None, + tool: Union[NDArray, SE2, None] = None, include_base: bool = True, ) -> SE2: """ @@ -2491,7 +3307,7 @@ def fkine( if fk.dtype == "O": # symbolic - fk = array(simplify(fk)) + fk = np.array(simplify(fk)) if fk.ndim == 3: for T in fk: @@ -2504,10 +3320,10 @@ def fkine( def eval( self, q: ArrayLike, - base: Union[ndarray, SE2, None] = None, - tool: Union[ndarray, SE2, None] = None, + base: Union[NDArray, SE2, None] = None, + tool: Union[NDArray, SE2, None] = None, include_base: bool = True, - ) -> ndarray: + ) -> NDArray: """ Forward kinematics :param q: Joint coordinates @@ -2540,8 +3356,8 @@ def eval( if base is None: bases = None elif isinstance(base, SE2): - bases = array(base.A) - elif array_equal(base, eye(3)): # pragma: nocover + bases = np.array(base.A) + elif np.array_equal(base, np.eye(3)): # pragma: nocover bases = None else: # pragma: nocover bases = base @@ -2549,16 +3365,16 @@ def eval( if tool is None: tools = None elif isinstance(tool, SE2): - tools = array(tool.A) - elif array_equal(tool, eye(3)): # pragma: nocover + tools = np.array(tool.A) + elif np.array_equal(tool, np.eye(3)): # pragma: nocover tools = None else: # pragma: nocover tools = tool if l > 1: - T = zeros((l, 3, 3), dtype=object) + T = np.zeros((l, 3, 3), dtype=object) else: - T = zeros((3, 3), dtype=object) + T = np.zeros((3, 3), dtype=object) for k, qk in enumerate(q): # type: ignore link = end # start with last link @@ -2580,7 +3396,7 @@ def eval( Tk = A @ Tk # add base transform if it is set - if include_base == True and bases is not None: + if include_base is True and bases is not None: Tk = bases @ Tk # append @@ -2596,17 +3412,16 @@ def eval( def jacob0( self, q: ArrayLike, - ): - + ) -> NDArray: # very inefficient implementation, just put a 1 in last row # if its a rotation joint q = getvector(q) j = 0 - J = zeros((3, self.n)) + J = np.zeros((3, self.n)) etjoints = self.joint_idx() - if not all(array([self[i].jindex for i in etjoints])): + if not np.all(np.array([self[i].jindex for i in etjoints])): # not all joints have a jindex it is required, set them for j in range(self.n): i = etjoints[j] @@ -2624,13 +3439,13 @@ def jacob0( axis = self[i].axis if axis == "R": - dTdq = array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A( + dTdq = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 0]]) @ self[i].A( q[jindex] # type: ignore ) elif axis == "tx": - dTdq = array([[0, 0, 1], [0, 0, 0], [0, 0, 0]]) + dTdq = np.array([[0, 0, 1], [0, 0, 0], [0, 0, 0]]) elif axis == "ty": - dTdq = array([[0, 0, 0], [0, 0, 1], [0, 0, 0]]) + dTdq = np.array([[0, 0, 0], [0, 0, 1], [0, 0, 0]]) else: # pragma: nocover raise TypeError("Invalid axes") @@ -2672,14 +3487,3 @@ def jacobe( T = self.fkine(q, include_base=False).A return tr2jac2(T.T) @ self.jacob0(q) - - -# if __name__ == "__main__": - -# from roboticstoolbox import models - -# ur5 = models.URDF.UR5() - -# ur5.fkine(ur5.qz) -# ur5.jacob0(ur5.qz) -# ur5.jacob0_analytic(ur5.qz) diff --git a/roboticstoolbox/robot/ETSRobot.py b/roboticstoolbox/robot/ETSRobot.py deleted file mode 100644 index 71298135e..000000000 --- a/roboticstoolbox/robot/ETSRobot.py +++ /dev/null @@ -1,13 +0,0 @@ -""" -ETSRobot - -This is a very light-weight wrapper for an ETS to give it robot-like -characteristics. -""" - -from roboticstoolbox.robot import Robot - - -class ETSRobot(Robot): - - pass diff --git a/roboticstoolbox/robot/Gripper.py b/roboticstoolbox/robot/Gripper.py index 781cca3f9..1d35d5ddb 100644 --- a/roboticstoolbox/robot/Gripper.py +++ b/roboticstoolbox/robot/Gripper.py @@ -4,19 +4,27 @@ """ import numpy as np +from spatialmath import SE3 import spatialmath as sm from spatialmath.base.argcheck import getvector from roboticstoolbox.robot.Link import Link -from typing import List from functools import lru_cache -from typing import Union +from typing import Union, TypeVar, Generic, List, Callable from roboticstoolbox.fknm import Robot_link_T +from roboticstoolbox.tools.types import ArrayLike, NDArray +from roboticstoolbox.robot.Link import BaseLink -ArrayLike = Union[list, np.ndarray, tuple, set] +# A generic type variable representing any subclass of BaseLink +LinkType = TypeVar("LinkType", bound=BaseLink) -class Gripper: - def __init__(self, elinks, name="", tool=None): +class Gripper(Generic[LinkType]): + def __init__( + self, + links: List[LinkType], + name: str = "", + tool: Union[NDArray, SE3, None] = None, + ): self._n = 0 @@ -27,18 +35,18 @@ def __init__(self, elinks, name="", tool=None): else: self.tool = tool - for link in elinks: + for link in links: if link.isjoint: self._n += 1 self.q = np.zeros(self.n) - self._links = elinks + self._links = links # assign the joint indices if all( [ link.jindex is None or link.ets._auto_jindex - for link in elinks + for link in links if link.isjoint ] ): @@ -52,12 +60,12 @@ def visit_link(link, jindex): jindex[0] += 1 # visit all links in DFS order - self.dfs_links(elinks[0], lambda link: visit_link(link, jindex)) + self.dfs_links(links[0], lambda link: visit_link(link, jindex)) - elif all([link.jindex is not None for link in elinks if link.isjoint]): + elif all([link.jindex is not None for link in links if link.isjoint]): # jindex set on all, check they are unique and sequential jset = set(range(self.n)) - for link in elinks: + for link in links: if link.isjoint: if link.jindex not in jset: raise ValueError( @@ -67,7 +75,7 @@ def visit_link(link, jindex): jset -= set([link.jindex]) if len(jset) > 0: # pragma nocover # is impossible raise ValueError("gripper joints {jset} were not assigned") - else: + else: # pragma nocover # must be a mixture of Links with/without jindex raise ValueError( "all gripper links must have a jindex, or none have a jindex" @@ -77,25 +85,42 @@ def __str__(self): s = "Gripper(" if self.name is not None: s += f'"{self.name}"' - s += f", connected to {self.links[0].parent_name}, {self.n} joints, {len(self.links)} links" + s += ( + f", connected to {self.links[0].parent_name}, {self.n} joints," + f" {len(self.links)} links" + ) s += ")" return s def __repr__(self): - return self.__str__() + links = [link.__repr__() for link in self.links] + + tool = None if np.all(self.tool.A == np.eye(4)) else self.tool.A.__repr__() + s = f'Gripper({links}, name="{self.name}", tool={tool})' + + return s - def dfs_links(self, start, func=None): + def dfs_links( + self, start: LinkType, func: Union[Callable[[Link], None], None] = None + ): """ + Search links using depth first search + Visit all links from start in depth-first order and will apply func to each visited link - :param start: the link to start at - :type start: Link - :param func: An optional function to apply to each link as it is found - :type func: function + Parameters + ---------- + start + the link to start at + func + An optional function to apply to each link as it is found + + Returns + ------- + links + A list of links - :returns: A list of links - :rtype: list of Link """ visited = [] @@ -113,61 +138,128 @@ def vis_children(link): return visited @property - def n(self): + def tool(self) -> SE3: + """ + Get/set gripper tool transform + + - ``gripper.tool`` is the gripper tool transform as an SE3 object + - ``gripper._tool`` is the gripper tool transform as a numpy array + - ``gripper.tool = ...`` checks and sets the gripper tool transform + + Parameters + ---------- + tool + the new gripper tool transform (as an SE(3)) + + Returns + ------- + tool + gripper tool transform + + + + """ + return SE3(self._tool, check=False) + + @tool.setter + def tool(self, T: Union[SE3, NDArray]): + if isinstance(T, SE3): + self._tool = T.A + else: + self._tool = T + + @property + def n(self) -> int: + """ + Number of joints + + Returns + ------- + n + Number of joints + + Examples + -------- + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot.n + + See Also + -------- + :func:`nlinks` + :func:`nbranches` + + """ + return self._n @property - def q(self): + def q(self) -> NDArray: """ Get/set gripper joint configuration - ``gripper.q`` is the gripper joint configuration + - ``gripper.q = ...`` checks and sets the joint configuration - :return: gripper joint configuration - :rtype: ndarray(n,) + Parameters + ---------- + q + the new gripper joint configuration - - ``gripper.q = ...`` checks and sets the joint configuration + Returns + ------- + q + gripper joint configuration - .. note:: ??? """ + return self._q @q.setter - def q(self, q_new): - self._q = getvector(q_new, self.n) + def q(self, q_new: ArrayLike): + self._q = np.array(getvector(q_new, self.n)) # --------------------------------------------------------------------- # @property - def links(self) -> List[Link]: + def links(self) -> List[LinkType]: """ Gripper links - :return: A list of link objects - :rtype: list of Link subclass instances + Returns + ------- + links + A list of link objects - .. note:: It is probably more concise to index the robot object rather - than the list of links, ie. the following are equivalent:: - - robot.links[i] - robot[i] """ return self._links # --------------------------------------------------------------------- # @property - def name(self): + def name(self) -> str: """ - Gripper name + Get/set gripper name + + - ``gripper.name`` is the gripper name + - ``gripper.name = ...`` checks and sets the gripper name + + Parameters + ---------- + name + the new gripper name + + Returns + ------- + name + the current gripper name - :return: The gripper name - :rtype: string """ return self._name @name.setter - def name(self, new_name): + def name(self, new_name: str): self._name = new_name # --------------------------------------------------------------------- # diff --git a/roboticstoolbox/robot/IK.py b/roboticstoolbox/robot/IK.py index 12ca4eaf5..96687cd15 100644 --- a/roboticstoolbox/robot/IK.py +++ b/roboticstoolbox/robot/IK.py @@ -1,41 +1,23 @@ #!/usr/bin/env python + """ @author Jesse Haviland """ import numpy as np -from collections import namedtuple from abc import ABC, abstractmethod -from typing import Optional, TYPE_CHECKING, Tuple, Union +from typing import Tuple, Union import roboticstoolbox as rtb from dataclasses import dataclass from spatialmath import SE3 +from roboticstoolbox.tools.types import ArrayLike -ArrayLike = Union[list, np.ndarray, tuple, set] - -# from roboticstoolbox.tools.null import null -# import roboticstoolbox as rtb -# from spatialmath import base -# from spatialmath import SE3 -# import scipy.optimize as opt -# import math - -# from roboticstoolbox.tools.p_servo import p_servo - -# iksol = namedtuple("IKsolution", "q, success, reason, iterations, residual", -# defaults=(None, False, None, None, None)) # Py >= 3.7 only -# iksol = namedtuple("IKsolution", "q, success, reason, iterations, residual, jl_valid") - - -# try: -# import qpsolvers as qp - -# _qp = True -# except ImportError: # pragma nocover -# _qp = False - +try: + import qpsolvers as qp -# ===================================================================== # + _qp = True +except ImportError: # pragma nocover + _qp = False @dataclass @@ -43,26 +25,56 @@ class IKSolution: """ A dataclass for representing an IK solution - :param q: The joint coordinates of the solution (ndarray). Note that these + Attributes + ---------- + q + The joint coordinates of the solution (ndarray). Note that these will not be valid if failed to find a solution - :param success: True if a valid solution was found - :param iterations: How many iterations were performed - :param searches: How many searches were performed - :param residual: The final error value from the cost function - :param reason: The reason the IK problem failed if applicable + success + True if a valid solution was found + iterations + How many iterations were performed + searches + How many searches were performed + residual + The final error value from the cost function + reason + The reason the IK problem failed if applicable + + + .. versionchanged:: 1.0.3 + Added IKSolution dataclass to replace the IKsolution named tuple + """ - q: Union[np.ndarray, None] + q: np.ndarray success: bool iterations: int = 0 searches: int = 0 residual: float = 0.0 reason: str = "" - def __str__(self): + def __iter__(self): + return iter( + ( + self.q, + self.success, + self.iterations, + self.searches, + self.residual, + self.reason, + ) + ) + def __str__(self): if self.q is not None: - q_str = np.round(self.q, 4) + q_str = np.array2string( + self.q, + separator=", ", + formatter={ + "float": lambda x: "{:.4g}".format(0 if abs(x) < 1e-6 else x) + }, + ) # np.round(self.q, 4) else: q_str = None @@ -75,15 +87,62 @@ def __str__(self): else: # Otherwise it is a numeric solution if self.success: - return f"IKSolution: q={q_str}, success=True, iterations={self.iterations}, searches={self.searches}, residual={self.residual}" + return ( + f"IKSolution: q={q_str}, success=True," + f" iterations={self.iterations}, searches={self.searches}," + f" residual={self.residual:.3g}" + ) else: - return f"IKSolution: q={q_str}, success=False, reason={self.reason}, iterations={self.iterations}, searches={self.searches}, residual={np.round(self.residual, 4)}" + return ( + f"IKSolution: q={q_str}, success=False, reason={self.reason}," + f" iterations={self.iterations}, searches={self.searches}," + f" residual={np.round(self.residual, 4):.3g}" + ) class IKSolver(ABC): """ - An abstract super class which provides basic functionality to perform numerical inverse - kinematics (IK). Superclasses can inherit this class and implement the solve method. + An abstract super class for numerical inverse kinematics (IK) + + This class provides basic functionality to perform numerical IK. Superclasses + can inherit this class and implement the `solve` method and redefine any other + methods necessary. + + Parameters + ---------- + name + The name of the IK algorithm + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + + See Also + -------- + IK_NR + Implements this class using the Newton-Raphson method + IK_GN + Implements this class using the Gauss-Newton method + IK_LM + Implements this class using the Levemberg-Marquadt method + IK_QP + Implements this class using a quadratic programming approach + + + .. versionchanged:: 1.0.3 + Added the abstract super class IKSolver + """ def __init__( @@ -96,16 +155,6 @@ def __init__( joint_limits: bool = True, seed: Union[int, None] = None, ): - """ - name: The name of the IK algorithm - ilimit: How many iterations are allowed within a search before a new search is started - slimit: How many searches are allowed before being deemed unsuccessful - tol: Maximum allowed residual error E - mask: A 6 vector which assigns weights to Cartesian degrees-of-freedom - problems: Total number of IK problems within the experiment - joint_limits: Reject solutions with joint limit violations - """ - # Solver parameters self.name = name self.slimit = slimit @@ -118,37 +167,133 @@ def __init__( if mask is None: mask = np.ones(6) - self.We = np.diag(mask) + self.We = np.diag(mask) # type: ignore self.joint_limits = joint_limits def solve( - self, ets: "rtb.ETS", Tep: np.ndarray, q0: Union[ArrayLike, None] + self, + ets: "rtb.ETS", + Tep: Union[SE3, np.ndarray], + q0: Union[ArrayLike, None] = None, ) -> IKSolution: """ + Solves the IK problem + This method will attempt to solve the IK problem and obtain joint coordinates - which result the the end-effector pose Tep. - - :return: An IKSolution dataclass: - :param q: The joint coordinates of the solution (ndarray). Note that these - will not be valid if failed to find a solution - :param success: True if a valid solution was found - :param iterations: How many iterations were performed - :param searches: How many searches were performed - :param residual: The final error value from the cost function - :param jl_valid: True if q is inbounds of the robots joint limits - :param reason: The reason the IK problem failed if applicable + which result the the end-effector pose `Tep`. + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q0 + The initial joint coordinate vector + + Returns + ------- + q + The joint coordinates of the solution (ndarray). Note that these + will not be valid if failed to find a solution + success + True if a valid solution was found + iterations + How many iterations were performed + searches + How many searches were performed + residual + The final error value from the cost function + jl_valid + True if q is inbounds of the robots joint limits + reason + The reason the IK problem failed if applicable + """ + # Get the largest jindex in the ETS. If this is greater than ETS.n + # then we need to pad the q vector with zeros + max_jindex: int = 0 + + for j in ets.joints(): + if j.jindex > max_jindex: # type: ignore + max_jindex = j.jindex # type: ignore + + q0_method = np.zeros((self.slimit, max_jindex + 1)) if q0 is None: - q0 = self.random_q(ets, self.slimit) + q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) + elif not isinstance(q0, np.ndarray): q0 = np.array(q0) - if q0.ndim == 1: - q0_new = self.random_q(ets, self.slimit) - q0_new[0] = q0 - q0 = q0_new + if q0 is not None and q0.ndim == 1: + q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) + + q0_method[0, ets.jindices] = q0 + + if q0 is not None and q0.ndim == 2: + q0_method[:, ets.jindices] = self._random_q(ets, self.slimit) + + q0_method[: q0.shape[0], ets.jindices] = q0 + + q0 = q0_method + + traj = False + methTep: np.ndarray + + if isinstance(Tep, SE3): + if len(Tep) > 1: + traj = True + methTep = np.empty((len(Tep), 4, 4)) + + for i, T in enumerate(Tep): + methTep[i] = T.A + else: + methTep = Tep.A + elif Tep.ndim == 3: + traj = True + methTep = Tep + elif Tep.shape != (4, 4): + raise ValueError("Tep must be a 4x4 SE3 matrix") + else: + methTep = Tep + + if traj: + q = np.empty((methTep.shape[0], ets.n)) + success = True + interations = 0 + searches = 0 + residual = np.inf + reason = "" + + for i, T in enumerate(methTep): + sol = self._solve(ets, T, q0) + q[i] = sol.q + if not sol.success: + success = False + reason = sol.reason + interations += sol.iterations + searches += sol.searches + + if sol.residual < residual: + residual = sol.residual + + return IKSolution( + q=q, + success=success, + iterations=interations, + searches=searches, + residual=residual, + reason=reason, + ) + + else: + sol = self._solve(ets, methTep, q0) + + return sol + + def _solve(self, ets: "rtb.ETS", Tep: np.ndarray, q0: np.ndarray) -> IKSolution: # Iteration count i = 0 total_i = 0 @@ -170,7 +315,7 @@ def solve( # Attempt a step try: - E, q = self.step(ets, Tep, q) + E, q[ets.jindices] = self.step(ets, Tep, q) except np.linalg.LinAlgError: # Abandon search and try again @@ -179,14 +324,13 @@ def solve( # Check if we have arrived if E < self.tol: - # Wrap q to be within +- 180 deg # If your robot has larger than 180 deg range on a joint # this line should be modified in incorporate the extra range q = (q + np.pi) % (2 * np.pi) - np.pi # Check if we have violated joint limits - jl_valid = self.check_jl(ets, q) + jl_valid = self._check_jl(ets, q) if not jl_valid and self.joint_limits: # Abandon search and try again @@ -194,7 +338,7 @@ def solve( break else: return IKSolution( - q=q, + q=q[ets.jindices], success=True, iterations=total_i + i, searches=search + 1, @@ -207,7 +351,7 @@ def solve( reason = "iteration and search limit reached" if linalg_error: - reason += f", {linalg_error} np.LinAlgError encountered" + reason += f", {linalg_error} numpy.LinAlgError encountered" if found_with_limits: reason += ", solution found but violates joint limits" @@ -222,71 +366,100 @@ def solve( ) def error(self, Te: np.ndarray, Tep: np.ndarray) -> Tuple[np.ndarray, float]: - """ + r""" + Calculates the error between Te and Tep + Calculates the engle axis error between current end-effector pose Te and the desired end-effector pose Tep. Also calulates the quadratic error E which is weighted by the diagonal matrix We. - Returns a tuple: - e: angle-axis error (ndarray in R^6) - E: The quadratic error weighted by We - """ - e = rtb.angle_axis(Te, Tep) - E = 0.5 * e @ self.We @ e - - return e, E + .. math:: - def check_jl(self, ets: "rtb.ETS", q: np.ndarray): - """ - Checks if the joints are within their respective limits + E = \frac{1}{2} \vec{e}^{\top} \mat{W}_e \vec{e} - Returns a True if joints within feasible limits otherwise False - """ + where :math:`\vec{e} \in \mathbb{R}^6` is the angle-axis error. - # Loop through the joints in the ETS - for i in range(ets.n): + Parameters + ---------- + Te + The current end-effector pose + Tep + The desired end-effector pose - # Get the corresponding joint limits - ql0 = ets.qlim[0, i] - ql1 = ets.qlim[1, i] + Returns + ------- + e + angle-axis error (6 vector) + E + The quadratic error weighted by We - # Check if q exceeds the limits - if q[i] < ql0 or q[i] > ql1: - return False + """ + e = rtb.angle_axis(Te, Tep) + E = 0.5 * e @ self.We @ e - # If we make it here, all the joints are fine - return True + return e, E @abstractmethod def step( self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray ) -> Tuple[float, np.ndarray]: """ - Superclasses will implement this method to perform a step of the implemented - IK algorithm - - :return: A tuple containing - :param E: The new error value - :param q: The new joint coordinate vector + Abstract step method + + Superclasses will implement this method to perform a step of the + implemented IK algorithm + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q + The current joint coordinate vector + + Raises + ------ + numpy.LinAlgError + If a step is impossible due to a linear algebra error + + Returns + ------- + E + The new error value + q + The new joint coordinate vector """ - pass + pass # pragma: nocover - def random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray: + def _random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray: """ - Generate a random valid joint configuration - - :param i: number of configurations to generate + Generate a random valid joint configuration using a private RNG Generates a random q vector within the joint limits defined by - `self.qlim`. + `ets.qlim`. + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + i + number of configurations to generate + + Returns + ------- + q + An `i x n` ndarray of random valid joint configurations, where n + is the number of joints in the `ets` + """ if i == 1: - q = np.zeros(ets.n) + q = np.zeros((1, ets.n)) for i in range(ets.n): - q[i] = self._private_random.uniform(ets.qlim[0, i], ets.qlim[1, i]) + q[0, i] = self._private_random.uniform(ets.qlim[0, i], ets.qlim[1, i]) else: q = np.zeros((i, ets.n)) @@ -299,25 +472,54 @@ def random_q(self, ets: "rtb.ETS", i: int = 1) -> np.ndarray: return q + def _check_jl(self, ets: "rtb.ETS", q: np.ndarray) -> bool: + """ + Checks if the joints are within their respective limits + + Parameters + ---------- + ets + the ETS + q + the current joint coordinate vector + + Returns + ------- + True if joints within feasible limits otherwise False + + """ + + # Loop through the joints in the ETS + for i in range(ets.n): + # Get the corresponding joint limits + ql0 = ets.qlim[0, i] + ql1 = ets.qlim[1, i] + + # Check if q exceeds the limits + if q[i] < ql0 or q[i] > ql1: + return False + + # If we make it here, all the joints are fine + return True + -def null_Σ(ets: "rtb.ETS", q: np.ndarray, ps: float, pi: Optional[np.ndarray]): +def _null_Σ(ets: "rtb.ETS", q: np.ndarray, ps: float, pi: Union[np.ndarray, float]): """ Formulates a relationship between joint limits and the joint velocity. When this is projected into the null-space of the differential kinematics to attempt to avoid exceeding joint limits - q: The joint coordinates of the robot - ps: The minimum angle/distance (in radians or metres) in which the joint is + :param q: The joint coordinates of the robot + :param ps: The minimum angle/distance (in radians or metres) in which the joint is allowed to approach to its limit - pi: The influence angle/distance (in radians or metres) in which the velocity + :param pi: The influence angle/distance (in radians or metres) in which the velocity damper becomes active - returns: Σ + :return: Σ """ - # If pi wasn't supplied, set it to some default value - if pi is None: - pi = 0.3 * np.ones(ets.n) + if isinstance(pi, float): + pi = pi * np.ones(ets.n) # Add cost to going in the direction of joint limits, if they are within # the influence distance @@ -336,20 +538,20 @@ def null_Σ(ets: "rtb.ETS", q: np.ndarray, ps: float, pi: Optional[np.ndarray]): return -Σ -def calc_qnull( +def _calc_qnull( ets: "rtb.ETS", q: np.ndarray, J: np.ndarray, λΣ: float, λm: float, ps: float, - pi: Optional[np.ndarray], + pi: Union[np.ndarray, float], ): """ Calculates the desired null-space motion according to the gains λΣ and λm. This is a helper method that is used within the `step` method of an IK solver - Returns qnull: the desired null-space motion + :return: qnull - the desired null-space motion """ qnull_grad = np.zeros(ets.n) @@ -357,7 +559,7 @@ def calc_qnull( # Add the joint limit avoidance if the gain is above 0 if λΣ > 0: - Σ = null_Σ(ets, q, ps, pi) + Σ = _null_Σ(ets, q, ps, pi) qnull_grad += (1.0 / λΣ * Σ).flatten() # Add the manipulability maximisation if the gain is above 0 @@ -373,63 +575,314 @@ def calc_qnull( return qnull.flatten() -# λΣ: The gain for joint limit avoidance. Setting to 0.0 will remove this completely from the solution -# λm: The gain for maximisation. Setting to 0.0 will remove this completely from the solution -# ps: The minimum angle/distance (in radians or metres) in which the joint is allowed to approach to its limit -# pi: The influence angle/distance (in radians or metres) in null space motion becomes active - - class IK_NR(IKSolver): + """ + Newton-Raphson Numerical Inverse Kinematics Solver + + A class which provides functionality to perform numerical inverse kinematics (IK) + using the Newton-Raphson method. See `step` method for mathematical description. + + Note + ---- + When using this class with redundant robots (>6 DoF), `pinv` must be set to `True` + + Parameters + ---------- + name + The name of the IK algorithm + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, instantiates + the IK_NR solver class using default parameters, makes a goal pose ``Tep``, + and then solves for the joint coordinates which result in the pose ``Tep`` + using the ``solve`` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> solver = rtb.IK_NR(pinv=True) + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> solver.solve(panda, Tep) + + Notes + ----- + When using the NR method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the + the problem is solvable, it converges very quickly. However, this method frequently + fails to converge on the goal. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + IKSolver + An abstract super class for numerical IK solvers + IK_GN + Implements the IKSolver class using the Gauss-Newton method + IK_LM + Implements the IKSolver class using the Levemberg-Marquadt method + IK_QP + Implements the IKSolver class using a quadratic programming approach + + + .. versionchanged:: 1.0.3 + Added the Newton-Raphson IK solver class + + """ # noqa + def __init__( self, - pinv=False, - λΣ: float = 0.0, - λm: float = 0.0, - ps: float = 0.1, - pi: Optional[np.ndarray] = None, + name: str = "IK Solver", + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[np.ndarray, float] = 0.3, **kwargs, ): - super().__init__(**kwargs) + super().__init__( + name=name, + ilimit=ilimit, + slimit=slimit, + tol=tol, + mask=mask, + joint_limits=joint_limits, + seed=seed, + **kwargs, + ) self.pinv = pinv - self.λΣ = λΣ - self.λm = λm + self.kq = kq + self.km = km self.ps = ps self.pi = pi self.name = f"NR (pinv={pinv})" - if self.λΣ > 0.0: + if self.kq > 0.0: self.name += " Σ" - if self.λm > 0.0: + if self.km > 0.0: self.name += " Jm" - def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): + def step( + self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray + ) -> Tuple[float, np.ndarray]: + r""" + Performs a single iteration of the Newton-Raphson optimisation method + + .. math:: + + \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q + The current joint coordinate vector + + Raises + ------ + numpy.LinAlgError + If a step is impossible due to a linear algebra error + + Returns + ------- + E + The new error value + q + The new joint coordinate vector + + """ + Te = ets.eval(q) e, E = self.error(Te, Tep) J = ets.jacob0(q) # Null-space motion - qnull = calc_qnull(ets, q, J, self.λΣ, self.λm, self.ps, self.pi) + qnull = _calc_qnull( + ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi + ) if self.pinv: - q += np.linalg.pinv(J) @ e + qnull + q[ets.jindices] += np.linalg.pinv(J) @ e + qnull else: - q += np.linalg.inv(J) @ e + qnull + q[ets.jindices] += np.linalg.inv(J) @ e + qnull - return E, q + return E, q[ets.jindices] class IK_LM(IKSolver): + """ + Levemberg-Marquadt Numerical Inverse Kinematics Solver + + A class which provides functionality to perform numerical inverse kinematics (IK) + using the Levemberg-Marquadt method. See ``step`` method for mathematical description. + + Parameters + ---------- + name + The name of the IK algorithm + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + k + Sets the gain value for the damping matrix Wn in the ``step`` method. See + notes + method + One of "chan", "sugihara" or "wampler". Defines which method is used + to calculate the damping matrix Wn in the ``step`` method + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, instantiates + the IK_LM solver class using default parameters, makes a goal pose ``Tep``, + and then solves for the joint coordinates which result in the pose ``Tep`` + using the `solve` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> solver = rtb.IK_LM() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> solver.solve(panda, Tep) + + Notes + ----- + The value for the ``k`` kwarg will depend on the ``method`` chosen and the arm you are + using. Use the following as a rough guide ``chan, k = 1.0 - 0.01``, + ``wampler, k = 0.01 - 0.0001``, and ``sugihara, k = 0.1 - 0.0001`` + + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + IKSolver + An abstract super class for numerical IK solvers + IK_NR + Implements the IKSolver class using the Newton-Raphson method + IK_GN + Implements the IKSolver class using the Gauss-Newton method + IK_QP + Implements the IKSolver class using a quadratic programming approach + + + .. versionchanged:: 1.0.3 + Added the Levemberg-Marquadt IK solver class + + """ # noqa + def __init__( self, - k=1.0, + name: str = "IK Solver", + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + k: float = 1.0, method="chan", + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[np.ndarray, float] = 0.3, **kwargs, ): - super().__init__(**kwargs) + super().__init__( + name=name, + ilimit=ilimit, + slimit=slimit, + tol=tol, + mask=mask, + joint_limits=joint_limits, + seed=seed, + **kwargs, + ) if method.lower().startswith("sugi"): self.method = 1 @@ -442,10 +895,101 @@ def __init__( method_name = "Chan" self.k = k + self.kq = kq + self.km = km + self.ps = ps + self.pi = pi self.name = f"LM ({method_name} λ={k})" + if self.kq > 0.0: + self.name += " Σ" + + if self.km > 0.0: + self.name += " Jm" + def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): + r""" + Performs a single iteration of the Levenberg-Marquadt optimisation + + The operation is defined by the choice of `method` when instantiating the class. + + The next step is deined as + + .. math:: + \vec{q}_{k+1} + &= + \vec{q}_k + + \left( + \mat{A}_k + \right)^{-1} + \bf{g}_k \\ + % + \mat{A}_k + &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + + + \mat{W}_n + + where :math:`\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})` is a + diagonal damping matrix. The damping matrix ensures that :math:`\mat{A}_k` is + non-singular and positive definite. The performance of the LM method largely depends + on the choice of :math:`\mat{W}_n`. + + **Chan's Method** + + Chan proposed + + .. math:: + \mat{W}_n + = + λ E_k \mat{1}_n + + where λ is a constant which reportedly does not have much influence on performance. + Use the kwarg `k` to adjust the weighting term λ. + + **Sugihara's Method** + + Sugihara proposed + + .. math:: + \mat{W}_n + = + E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n) + + where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`, + and :math:`l` is the length of a typical link within the manipulator. We provide the + variable `k` as a kwarg to adjust the value of :math:`w_n`. + + **Wampler's Method** + + Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg. + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q + The current joint coordinate vector + + Raises + ------ + numpy.LinAlgError + If a step is impossible due to a linear algebra error + + Returns + ------- + E + The new error value + q + The new joint coordinate vector + + """ # noqa + Te = ets.eval(q) e, E = self.error(Te, Tep) @@ -462,990 +1006,516 @@ def step(self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray): J = ets.jacob0(q) g = J.T @ self.We @ e - q += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + # Null-space motion + qnull = _calc_qnull( + ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi + ) + + q[ets.jindices] += np.linalg.inv(J.T @ self.We @ J + Wn) @ g + qnull + + return E, q[ets.jindices] + + +class IK_GN(IKSolver): + """ + Gauss-Newton Numerical Inverse Kinematics Solver + + A class which provides functionality to perform numerical inverse kinematics (IK) + using the Gauss-Newton method. See `step` method for mathematical description. + + Note + ---- + When using this class with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + name + The name of the IK algorithm + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, instantiates + the `IK_GN` solver class using default parameters, makes a goal pose ``Tep``, + and then solves for the joint coordinates which result in the pose ``Tep`` + using the `solve` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> solver = rtb.IK_GN(pinv=True) + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> solver.solve(panda, Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the + the problem is solvable, it converges very quickly. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + IKSolver + An abstract super class for numerical IK solvers + IK_NR + Implements IKSolver using the Newton-Raphson method + IK_LM + Implements IKSolver using the Levemberg-Marquadt method + IK_QP + Implements IKSolver using a quadratic programming approach + + + .. versionchanged:: 1.0.3 + Added the Gauss-Newton IK solver class + + """ # noqa + + def __init__( + self, + name: str = "IK Solver", + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[np.ndarray, float] = 0.3, + **kwargs, + ): + super().__init__( + name=name, + ilimit=ilimit, + slimit=slimit, + tol=tol, + mask=mask, + joint_limits=joint_limits, + seed=seed, + **kwargs, + ) + + self.pinv = pinv + self.kq = kq + self.km = km + self.ps = ps + self.pi = pi + + self.name = f"GN (pinv={pinv})" + + if self.kq > 0.0: + self.name += " Σ" + + if self.km > 0.0: + self.name += " Jm" + + def step( + self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray + ) -> Tuple[float, np.ndarray]: + r""" + Performs a single iteration of the Gauss-Newton optimisation method + + The next step is defined as + + .. math:: + + \vec{q}_{k+1} &= \vec{q}_k + + \left( + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + \right)^{-1} + \bf{g}_k \\ + \bf{g}_k &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e + \vec{e}_k + + where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If + :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then + the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)` + is singular, the above can not be computed and the GN solution is infeasible. + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q + The current joint coordinate vector + + Raises + ------ + numpy.LinAlgError + If a step is impossible due to a linear algebra error + + Returns + ------- + E + The new error value + q + The new joint coordinate vector + + """ # noqa + + Te = ets.eval(q) + e, E = self.error(Te, Tep) + + J = ets.jacob0(q) + + # Null-space motion + qnull = _calc_qnull( + ets=ets, q=q, J=J, λΣ=self.kq, λm=self.km, ps=self.ps, pi=self.pi + ) + + if self.pinv: + q[ets.jindices] += np.linalg.pinv(J) @ e + qnull + else: + q[ets.jindices] += np.linalg.inv(J) @ e + qnull + + return E, q[ets.jindices] + + +class IK_QP(IKSolver): + """ + Quadratic Progamming Numerical Inverse Kinematics Solver + + A class which provides functionality to perform numerical inverse kinematics (IK) + using a quadratic progamming approach. See `step` method for mathematical + description. + + Parameters + ---------- + name + The name of the IK algorithm + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + kj + A gain for joint velocity norm minimisation + ks + A gain which adjusts the cost of slack (intentional error) + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Raises + ------ + ImportError + If the package ``qpsolvers`` is not installed + + Examples + -------- + The following example gets the ``ets`` of a ``panda`` robot object, instantiates + the `IK_QP` solver class using default parameters, makes a goal pose ``Tep``, + and then solves for the joint coordinates which result in the pose ``Tep`` + using the `solve` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda().ets() + >>> solver = rtb.IK_QP() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> solver.solve(panda, Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. When the + the problem is solvable, it converges very quickly. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + IKSolver + An abstract super class for numerical IK solvers + IK_NR + Implements IKSolver class using the Newton-Raphson method + IK_GN + Implements IKSolver class using the Gauss-Newton method + IK_LM + Implements IKSolver class using the Levemberg-Marquadt method + + + .. versionchanged:: 1.0.3 + Added the Quadratic Programming IK solver class + + """ # noqa + + def __init__( + self, + name: str = "IK Solver", + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + kj=0.01, + ks=1.0, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[np.ndarray, float] = 0.3, + **kwargs, + ): + if not _qp: # pragma: nocover + raise ImportError( + "the package qpsolvers is required for this class. \nInstall using 'pip" + " install qpsolvers'" + ) + + super().__init__( + name=name, + ilimit=ilimit, + slimit=slimit, + tol=tol, + mask=mask, + joint_limits=joint_limits, + seed=seed, + **kwargs, + ) + + self.kj = kj + self.ks = ks + self.kq = kq + self.km = km + self.ps = ps + self.pi = pi + + self.name = "QP)" + + if self.kq > 0.0: + self.name += " Σ" + + if self.km > 0.0: + self.name += " Jm" + + def step( + self, ets: "rtb.ETS", Tep: np.ndarray, q: np.ndarray + ) -> Tuple[float, np.ndarray]: + r""" + Performs a single iteration of the Gauss-Newton optimisation method + + The next step is defined as + + .. math:: + + \vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}. + + where the QP is defined as + + .. math:: + + \min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\ + \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu}, \\ + \mathcal{A} \vec{x} &\leq \mathcal{B}, \\ + \vec{x}^- &\leq \vec{x} \leq \vec{x}^+ + + with + + .. math:: + + \vec{x} &= + \begin{pmatrix} + \dvec{q} \\ \vec{\delta} + \end{pmatrix} \in \mathbb{R}^{(n+6)} \\ + \mathcal{Q} &= + \begin{pmatrix} + \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\ + \mathcal{J} &= + \begin{pmatrix} + \mat{J}(\vec{q}) & \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\ + \mathcal{C} &= + \begin{pmatrix} + \mat{J}_m \\ \bf{0}_{6 \times 1} + \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\ + \mathcal{A} &= + \begin{pmatrix} + \mat{1}_{n \times n + 6} \\ + \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\ + \mathcal{B} &= + \eta + \begin{pmatrix} + \frac{\rho_0 - \rho_s} + {\rho_i - \rho_s} \\ + \vdots \\ + \frac{\rho_n - \rho_s} + {\rho_i - \rho_s} + \end{pmatrix} \in \mathbb{R}^{n} \\ + \vec{x}^{-, +} &= + \begin{pmatrix} + \dvec{q}^{-, +} \\ + \vec{\delta}^{-, +} + \end{pmatrix} \in \mathbb{R}^{(n+6)}, + + where :math:`\vec{\delta} \in \mathbb{R}^6` is the slack vector, + :math:`\lambda_\delta \in \mathbb{R}^+` is a gain term which adjusts the + cost of the norm of the slack vector in the optimiser, + :math:`\dvec{q}^{-,+}` are the minimum and maximum joint velocities, and + :math:`\dvec{\delta}^{-,+}` are the minimum and maximum slack velocities. + + Parameters + ---------- + ets + The ETS representing the manipulators kinematics + Tep + The desired end-effector pose + q + The current joint coordinate vector + + Raises + ------ + numpy.LinAlgError + If a step is impossible due to a linear algebra error + + Returns + ------- + E + The new error value + q + The new joint coordinate vector + + """ # noqa + + Te = ets.eval(q) + e, E = self.error(Te, Tep) + J = ets.jacob0(q) + + if isinstance(self.pi, float): + self.pi = self.pi * np.ones(ets.n) + + # Quadratic component of objective function + Q = np.eye(ets.n + 6) + + # Joint velocity component of Q + Q[: ets.n, : ets.n] *= self.kj + + # Slack component of Q + Q[ets.n :, ets.n :] = self.ks * (1 / np.sum(np.abs(e))) * np.eye(6) + + # The equality contraints + Aeq = np.concatenate((J, np.eye(6)), axis=1) + beq = e.reshape((6,)) + + # The inequality constraints for joint limit avoidance + if self.kq > 0.0: + Ain = np.zeros((ets.n + 6, ets.n + 6)) + bin = np.zeros(ets.n + 6) + + # Form the joint limit velocity damper + Ain_l = np.zeros((ets.n, ets.n)) + Bin_l = np.zeros(ets.n) + + for i in range(ets.n): + ql0 = ets.qlim[0, i] + ql1 = ets.qlim[1, i] + + if ql1 - q[i] <= self.pi[i]: + Bin_l[i] = ((ql1 - q[i]) - self.ps) / (self.pi[i] - self.ps) + Ain_l[i, i] = 1 + + if q[i] - ql0 <= self.pi[i]: + Bin_l[i] = -(((ql0 - q[i]) + self.ps) / (self.pi[i] - self.ps)) + Ain_l[i, i] = -1 + + Ain[: ets.n, : ets.n] = Ain_l + bin[: ets.n] = (1.0 / self.kq) * Bin_l + else: + Ain = None + bin = None + + # Manipulability maximisation + if self.km > 0.0: + Jm = ets.jacobm(q).reshape((ets.n,)) + c = np.concatenate(((1.0 / self.km) * -Jm, np.zeros(6))) + else: + c = np.zeros(ets.n + 6) + + xd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq, lb=None, ub=None, solver="quadprog") + + if xd is None: # pragma: nocover + raise np.linalg.LinAlgError("QP Unsolvable") + + q += xd[: ets.n] return E, q -# class IKMixin: -# def ikine_NR( -# self: Union["rtb.ETS", "IKMixin"], -# Tep: Union[np.ndarray, SE3], -# q0: Union[np.ndarray, None] = None, -# ilimit: int = 30, -# slimit: int = 100, -# tol: float = 1e-6, -# joint_limits: bool = True, -# we: Union[np.ndarray, None] = None, -# λΣ: float = 0.0, -# λm: float = 0.0, -# ps: float = 0.1, -# pi: Optional[np.ndarray] = None, -# ): -# solver = NR( -# ilimit=ilimit, -# slimit=slimit, -# tol=tol, -# joint_limits=joint_limits, -# we=we, -# λΣ=λΣ, -# λm=λm, -# ps=ps, -# pi=pi, -# ) - -# if isinstance(Tep, SE3): -# Tep = Tep.A - -# return solver.solve(ets=self, Tep=Tep, q0=q0) - -# def ikine_LM( -# self: Union["rtb.ETS", "IKMixin"], -# Tep: Union[np.ndarray, SE3], -# q0: Union[np.ndarray, None] = None, -# ilimit: int = 30, -# slimit: int = 100, -# tol: float = 1e-6, -# joint_limits: bool = True, -# mask: Union[np.ndarray, None] = None, -# ): -# solver = NR( -# ilimit=ilimit, -# slimit=slimit, -# tol=tol, -# joint_limits=joint_limits, -# mask=mask, -# ) - -# if isinstance(Tep, SE3): -# Tep = Tep.A - -# return solver.solve(ets=self, Tep=Tep, q0=q0) - - -# class IKMixin: -# def ikine_mmc(self, T, q0=None): - -# if not _qp: -# raise ImportError( -# "the package qpsolvers is required for this function. \nInstall using 'pip install qpsolvers'" -# ) - -# arrived = False - -# n = self.n -# q = self.q -# dt = 0.05 - -# e_prev = 100000 -# q_last = q -# gain = 1000.0 - -# while not arrived: - -# Te = self.fkine(q) -# eTep = Te.inv() * T -# e = np.sum(np.abs(np.r_[eTep.t, eTep.rpy() * np.pi / 180])) - -# if e < e_prev: -# # good update -# # gain = gain * 2 -# # dt = dt * 2 -# # print('Up') -# pass -# else: -# # bad update -# # gain = gain / 2 -# dt = dt / 2 -# q = q_last -# # print('Down') - -# e_prev = e -# q_last = q -# # print(gain) -# # print(self.manipulability(q)) -# # print(e) - -# v, arrived = p_servo(Te, T, gain=gain, threshold=0.000001) - -# # Gain term (lambda) for control minimisation -# Y = 0.01 - -# # Quadratic component of objective function -# Q = np.eye(n + 6) - -# # Joint velocity component of Q -# Q[:n, :n] *= Y - -# # Slack component of Q -# Q[n:, n:] = (1 / e) * np.eye(6) - -# # The equality contraints -# Aeq = np.c_[self.jacobe(q), np.eye(6)] -# beq = v.reshape((6,)) - -# # The inequality constraints for joint limit avoidance -# Ain = np.zeros((n + 6, n + 6)) -# bin = np.zeros(n + 6) - -# # The minimum angle (in radians) in which the joint is allowed to -# # approach to its limit -# ps = 0.05 - -# # The influence angle (in radians) in which the velocity damper -# # becomes active -# pi = 0.9 - -# Ain[:n, :n], bin[:n] = self.joint_velocity_damper(ps, pi, n) -# c = np.r_[-self.jacobm(q).reshape((n,)), np.zeros(6)] -# # lb = -np.r_[self.qdlim[:n], 10 * np.ones(6)] -# # ub = np.r_[self.qdlim[:n], 10 * np.ones(6)] - -# # Solve for the joint velocities dq -# qd = qp.solve_qp(Q, c, Ain, bin, Aeq, beq) - -# for i in range(self.n): -# q[i] += qd[i] * (dt) - -# return q - -# # --------------------------------------------------------------------- # - -# def ikine_LM( -# self, -# T, -# q0=None, -# mask=None, -# ilimit=500, -# rlimit=100, -# tol=1e-10, -# L=0.1, -# Lmin=0, -# search=False, -# slimit=100, -# transpose=None, -# end=None, -# ): -# """ -# Numerical inverse kinematics by Levenberg-Marquadt optimization -# (Robot superclass) - -# :param T: The desired end-effector pose or pose trajectory -# :type T: SE3 -# :param q0: initial joint configuration (default all zeros) -# :type q0: ndarray(n) -# :param mask: mask vector that correspond to translation in X, Y and Z -# and rotation about X, Y and Z respectively. -# :type mask: ndarray(6) -# :param ilimit: maximum number of iterations (default 500) -# :type ilimit: int -# :param rlimit: maximum number of consecutive step rejections -# (default 100) -# :type rlimit: int -# :param tol: final error tolerance (default 1e-10) -# :type tol: float -# :param L: initial value of lambda -# :type L: float (default 0.1) -# :param Lmin: minimum allowable value of lambda -# :type Lmin: float (default 0) -# :param search: search over all configurations -# :type search: bool -# :param slimit: maximum number of search attempts -# :type slimit: int (default 100) -# :param transpose: use Jacobian transpose with step size A, rather -# than Levenberg-Marquadt -# :type transpose: float -# :return: inverse kinematic solution -# :rtype: named tuple - -# ``sol = robot.ikine_LM(T)`` are the joint coordinates (n) corresponding -# to the robot end-effector pose ``T`` which is an ``SE3`` object. This -# method can be used for robots with any number of degrees of freedom. -# The return value ``sol`` is a named tuple with elements: - -# ============ ========== =============================================== -# Element Type Description -# ============ ========== =============================================== -# ``q`` ndarray(n) joint coordinates in units of radians or metres -# ``success`` bool whether a solution was found -# ``reason`` str reason for the failure -# ``iterations`` int number of iterations -# ``residual`` float final value of cost function -# ============ ========== =============================================== - -# If ``success=False`` the ``q`` values will be valid numbers, but the -# solution will be in error. The amount of error is indicated by -# the ``residual``. - -# **Trajectory operation**: - -# If ``len(T) = m > 1`` it is considered to be a trajectory, then the -# result is a named tuple whose elements are - -# ============ ============ =============================================== -# Element Type Description -# ============ ============ =============================================== -# ``q`` ndarray(m,n) joint coordinates in units of radians or metres -# ``success`` bool(m) whether a solution was found -# ``reason`` list of str reason for the failure -# ``iterations`` ndarray(m) number of iterations -# ``residual`` ndarray(m) final value of cost function -# ============ ============ =============================================== - -# The initial estimate of q for each time step is taken as the solution -# from the previous time step. - -# **Underactuated robots:** - -# For the case where the manipulator has fewer than 6 DOF the -# solution space has more dimensions than can be spanned by the -# manipulator joint coordinates. - -# In this case we specify the ``mask`` option where the ``mask`` vector -# (6) specifies the Cartesian DOF (in the wrist coordinate frame) that -# will be ignored in reaching a solution. The mask vector has six -# elements that correspond to translation in X, Y and Z, and rotation -# about X, Y and Z respectively. The value should be 0 (for ignore) -# or 1. The number of non-zero elements must equal the number of -# manipulator DOF. - -# For example when using a 3 DOF manipulator tool orientation might -# be unimportant, in which case use the option ``mask=[1 1 1 0 0 0]``. - -# **Global search**: - -# ``sol = robot.ikine_LM(T, search=True)`` as above but peforms a -# brute-force search with initial conditions chosen randomly from the -# entire configuration space. If a numerical solution is found from that -# initial condition, it is returned, otherwise another initial condition -# is chosen. - -# .. note:: - -# - See `Toolbox kinematics wiki page -# `_ -# - Implements a Levenberg-Marquadt variable-damping solver. -# - The tolerance is computed on the norm of the error between -# current and desired tool pose. This norm is computed from -# distances and angles without any kind of weighting. -# - The inverse kinematic solution is generally not unique, and -# depends on the initial guess ``q0``. -# - The default value of ``q0`` is zero which is a poor choice for -# most manipulators since it often corresponds to a -# kinematic singularity. -# - Such a solution is completely general, though much less -# efficient than analytic inverse kinematic solutions derived -# symbolically. -# - This approach allows a solution to be obtained at a singularity, -# but the joint angles within the null space are arbitrarily -# assigned. -# - Joint offsets, if defined, are accounted for in the solution. -# - Joint limits are not considered in this solution. -# - If the search option is used any prismatic joint must have -# joint limits defined. - -# :references: -# - Robotics, Vision & Control, P. Corke, Springer 2011, -# Section 8.4. - -# :seealso: :func:`ikine_LMS`, :func:`ikine_unc`, :func:`ikine_con`, -# :func:`ikine_min` -# """ # noqa E501 - -# if not isinstance(T, SE3): -# raise TypeError("argument must be SE3") - -# if isinstance(self, rtb.DHRobot): -# end = None - -# solutions = [] - -# if search: -# # Randomised search for a starting point -# # quiet = True - -# qlim = self.qlim -# qspan = qlim[1] - qlim[0] # range of joint motion - -# for k in range(slimit): -# # choose a random joint coordinate -# q0_k = np.random.rand(self.n) * qspan + qlim[0, :] -# print("search starts at ", q0_k) - -# # recurse into the solver -# solution = self.ikine_LM( -# T[0], -# q0_k, -# mask, -# ilimit, -# rlimit, -# tol, -# L, -# Lmin, -# False, -# slimit, -# transpose, -# ) - -# if solution.success: -# q0 = solution.q -# if len(T) == 1: -# # we're done -# return solution -# else: -# # more to do on the trajectory -# solutions.append(solution) -# del T[0] -# # no solution found, stop now -# return iksol(None, False, None, None, None) - -# if q0 is None: -# q0 = np.zeros((self.n,)) -# else: -# q0 = base.getvector(q0, self.n) - -# if mask is not None: -# mask = base.getvector(mask, 6) -# if not self.n >= np.sum(mask): -# raise ValueError( -# "Number of robot DOF must be >= the number " -# "of 1s in the mask matrix" -# ) -# else: -# mask = np.ones(6) -# W = np.diag(mask) - -# tcount = 0 # Total iteration count -# rejcount = 0 # Rejected step count -# nm = 0 -# revolutes = self.revolutejoints - -# q = q0 -# for Tk in T: -# iterations = 0 -# Li = L # lambda -# failure = None -# while True: -# # Update the count and test against iteration limit -# iterations += 1 - -# if iterations > ilimit: -# failure = f"iteration limit {ilimit} exceeded" -# break - -# e = base.tr2delta(self.fkine(q, end=end).A, Tk.A) - -# # Are we there yet? -# if base.norm(W @ e) < tol: -# break - -# # Compute the Jacobian -# J = self.jacobe(q, end=end) - -# JtJ = J.T @ W @ J - -# if transpose is not None: -# # Do the simple Jacobian transpose with constant gain -# dq = transpose * J.T @ e # lgtm [py/multiple-definition] -# q += dq -# else: -# # Do the damped inverse Gauss-Newton with -# # Levenberg-Marquadt -# # dq = np.linalg.inv( -# # JtJ + ((Li + Lmin) * np.eye(self.n)) -# # ) @ J.T @ W @ e -# dq = ( -# np.linalg.inv(JtJ + ((Li + Lmin) * np.diag(np.diag(JtJ)))) -# @ J.T -# @ W -# @ e -# ) -# # print(J.T @ W @ e) - -# # Compute possible new value of -# qnew = q + dq - -# # And figure out the new error -# enew = base.tr2delta(self.fkine(qnew, end=end).A, Tk.A) - -# # Was it a good update? -# if np.linalg.norm(W @ enew) < np.linalg.norm(W @ e): -# # Step is accepted -# q = qnew -# e = enew -# Li /= 2 -# rejcount = 0 -# else: -# # Step is rejected, increase the damping and retry -# Li *= 2 -# rejcount += 1 -# if rejcount > rlimit: -# failure = f"rejected-step limit {rlimit} exceeded" -# break - -# # Wrap angles for revolute joints -# k = np.logical_and(q > np.pi, revolutes) -# q[k] -= 2 * np.pi - -# k = np.logical_and(q < -np.pi, revolutes) -# q[k] += +2 * np.pi - -# nm = np.linalg.norm(W @ e) -# # qs = ", ".join(["{:8.3f}".format(qi) for qi in q]) -# # print(f"λ={Li:8.2g}, |e|={nm:8.2g}: q={qs}") - -# # LM process finished, for better or worse -# # failure will be None or an error message -# solution = iksol(q, failure is None, failure, iterations, nm) -# solutions.append(solution) - -# tcount += iterations - -# if len(T) == 1: -# return solutions[0] -# else: -# return iksol( -# np.vstack([sol.q for sol in solutions]), -# np.array([sol.success for sol in solutions]), -# [sol.reason for sol in solutions], -# np.array([sol.iterations for sol in solutions]), -# np.array([sol.residual for sol in solutions]), -# ) - -# # --------------------------------------------------------------------- # - -# def ikine_LMS( -# self, T, q0=None, mask=None, ilimit=500, tol=1e-10, wN=1e-3, Lmin=0, end=None -# ): -# """ -# Numerical inverse kinematics by Levenberg-Marquadt optimization -# (Robot superclass) - -# :param T: The desired end-effector pose or pose trajectory -# :type T: SE3 -# :param q0: initial joint configuration (default all zeros) -# :type q0: ndarray(n) -# :param mask: mask vector that correspond to translation in X, Y and Z -# and rotation about X, Y and Z respectively. -# :type mask: ndarray(6) -# :param ilimit: maximum number of iterations (default 500) -# :type ilimit: int -# :param tol: final error tolerance (default 1e-10) -# :type tol: float -# :param ωN: damping coefficient -# :type ωN: float (default 1e-3) -# :return: inverse kinematic solution -# :rtype: named tuple - -# ``sol = robot.ikine_LM(T)`` are the joint coordinates (n) corresponding -# to the robot end-effector pose ``T`` which is an ``SE3`` object. This -# method can be used for robots with any number of degrees of freedom. -# The return value ``sol`` is a named tuple with elements: - -# ============ ========== =============================================== -# Element Type Description -# ============ ========== =============================================== -# ``q`` ndarray(n) joint coordinates in units of radians or metres -# ``success`` bool whether a solution was found -# ``reason`` str reason for the failure -# ``iterations`` int number of iterations -# ``residual`` float final value of cost function -# ============ ========== =============================================== - -# If ``success=False`` the ``q`` values will be valid numbers, but the -# solution will be in error. The amount of error is indicated by -# the ``residual``. - -# **Trajectory operation**: - -# If ``len(T) = m > 1`` it is considered to be a trajectory, then the -# result is a named tuple whose elements are - -# ============ ============ =============================================== -# Element Type Description -# ============ ============ =============================================== -# ``q`` ndarray(m,n) joint coordinates in units of radians or metres -# ``success`` bool(m) whether a solution was found -# ``reason`` list of str reason for the failure -# ``iterations`` ndarray(m) number of iterations -# ``residual`` ndarray(m) final value of cost function -# ============ ============ =============================================== - -# **Underactuated robots:** - -# For the case where the manipulator has fewer than 6 DOF the -# solution space has more dimensions than can be spanned by the -# manipulator joint coordinates. - -# In this case we specify the ``mask`` option where the ``mask`` vector -# (6) specifies the Cartesian DOF (in the wrist coordinate frame) that -# will be ignored in reaching a solution. The mask vector has six -# elements that correspond to translation in X, Y and Z, and rotation -# about X, Y and Z respectively. The value should be 0 (for ignore) -# or 1. The number of non-zero elements should equal the number of -# manipulator DOF. - -# For example when using a 3 DOF manipulator rotation orientation might -# be unimportant in which case use the option: mask = [1 1 1 0 0 0]. - -# .. note:: - -# - See `Toolbox kinematics wiki page -# `_ -# - Implements a modified Levenberg-Marquadt variable-damping solver -# which is quite robust in practice. -# - Similar to ``ikine_LM`` but uses a different error metric -# - The tolerance is computed on the norm of the error between -# current and desired tool pose. This norm is computed from -# distances and angles without any kind of weighting. -# - The inverse kinematic solution is generally not unique, and -# depends on the initial guess ``q0``. -# - The default value of ``q0`` is zero which is a poor choice for -# most manipulators since it often corresponds to a -# kinematic singularity. -# - Such a solution is completely general, though much less -# efficient than analytic inverse kinematic solutions derived -# symbolically. -# - This approach allows a solution to be obtained at a singularity, -# but the joint angles within the null space are arbitrarily -# assigned. -# - Joint offsets, if defined, are accounted for in the solution. -# - Joint limits are not considered in this solution. - -# :references: -# - "Solvability-Unconcerned Inverse Kinematics by the -# Levenberg–Marquardt Method", T. Sugihara, IEEE T-RO, 27(5), -# October 2011, pp. 984-991. - -# :seealso: :func:`ikine_LM`, :func:`ikine_unc`, :func:`ikine_con`, -# :func:`ikine_min` -# """ # noqa E501 - -# if not isinstance(T, SE3): -# raise TypeError("argument must be SE3") - -# if isinstance(self, rtb.DHRobot): -# end = None - -# solutions = [] - -# if q0 is None: -# q0 = np.zeros((self.n,)) -# else: -# q0 = base.getvector(q0, self.n) - -# if mask is not None: -# mask = base.getvector(mask, 6) -# if not self.n >= np.sum(mask): -# raise ValueError( -# "Number of robot DOF must be >= the number " -# "of 1s in the mask matrix" -# ) -# else: -# mask = np.ones(6) -# W = np.diag(mask) - -# tcount = 0 # Total iteration count -# revolutes = self.revolutejoints - -# q = q0 -# for Tk in T: -# iterations = 0 -# failure = None -# while True: -# # Update the count and test against iteration limit -# iterations += 1 - -# if iterations > ilimit: -# failure = f"iteration limit {ilimit} exceeded" -# break - -# e = _angle_axis(self.fkine(q, end=end).A, Tk.A) - -# # Are we there yet? -# E = 0.5 * e.T @ W @ e -# if E < tol: -# break - -# # Compute the Jacobian and projection matrices -# J = self.jacob0(q, end=end) -# WN = E * np.eye(self.n) + wN * np.eye(self.n) -# H = J.T @ W @ J + WN # n x n -# g = J.T @ W @ e # n x 1 - -# # Compute new value of q -# q += np.linalg.inv(H) @ g # n x 1 -# # print(np.linalg.norm(np.linalg.inv(H) @ g)) -# # print(e) -# # print(g) -# # print(q) -# # print(J) - -# # Wrap angles for revolute joints -# k = np.logical_and(q > np.pi, revolutes) -# q[k] -= 2 * np.pi - -# k = np.logical_and(q < -np.pi, revolutes) -# q[k] += +2 * np.pi - -# # qs = ", ".join(["{:8.3f}".format(qi) for qi in q]) -# # print(f"|e|={E:8.2g}, det(H)={np.linalg.det(H)}: q={qs}") - -# # LM process finished, for better or worse -# # failure will be None or an error message -# solution = iksol(q, failure is None, failure, iterations, E) -# solutions.append(solution) - -# tcount += iterations - -# if len(T) == 1: -# return solutions[0] -# else: -# return iksol( -# np.vstack([sol.q for sol in solutions]), -# np.array([sol.success for sol in solutions]), -# [sol.reason for sol in solutions], -# np.array([sol.iterations for sol in solutions]), -# np.array([sol.residual for sol in solutions]), -# ) - -# # --------------------------------------------------------------------- # - -# def ikine_min( -# self, -# T, -# q0=None, -# qlim=False, -# ilimit=1000, -# tol=1e-16, -# method=None, -# stiffness=0, -# costfun=None, -# options={}, -# end=None, -# ): -# r""" -# Inverse kinematics by optimization with joint limits (Robot superclass) - -# :param T: The desired end-effector pose or pose trajectory -# :type T: SE3 -# :param q0: initial joint configuration (default all zeros) -# :type q0: ndarray(n) -# :param qlim: enforce joint limits -# :type qlim: bool -# :param ilimit: Iteration limit (default 1000) -# :type ilimit: int -# :param tol: Tolerance (default 1e-16) -# :type tol: tol -# :param method: minimization method to use -# :type method: str -# :param stiffness: Stiffness used to impose a smoothness contraint on -# joint angles, useful when n is large (default 0) -# :type stiffness: float -# :param costfun: User supplied cost term, optional -# :type costfun: callable -# :return: inverse kinematic solution -# :rtype: named tuple - -# ``sol = robot.ikine_min(T)`` are the joint coordinates (n) -# corresponding to the robot end-effector pose T which is an SE3 object. -# The return value ``sol`` is a named tuple with elements: - -# ============ ========== ============================================================ -# Element Type Description -# ============ ========== ============================================================ -# ``q`` ndarray(n) joint coordinates in units of radians or metres, or ``None`` -# ``success`` bool whether a solution was found -# ``reason`` str reason for the failure -# ``iterations`` int number of iterations -# ``residual`` float final value of cost function -# ============ ========== ============================================================ - -# **Minimization method**: - -# By default this method uses: - -# - the Scipy ``SLSQP`` (Sequential Least Squares Programming) minimizer -# for the case of no joint limits -# - the Scipy ``trust-constr`` minimizer for the case with joint limits. -# This gives good results but is very slow. An alternative is -# ``L-BFGS-B`` (Broyden–Fletcher–Goldfarb–Shanno) but for redundant -# robots can sometimes give poor results, pushing against the joint -# limits when there is no need to. - -# In both case the function to be minimized is the squared norm of a -# vector :math:`[d,a]` with components respectively the -# translation error and rotation error in Euler vector form, between the -# desired pose and the current estimate obtained by inverse kinematics. - -# **Additional cost terms**: - -# This method supports two additional costs: - -# - ``stiffness`` imposes a penalty on joint variation -# :math:`\sum_{j=1}^N (q_j - q_{j-1})^2` which tends to keep the -# arm straight -# - ``costfun`` add a cost given by a user-specified function -# ``costfun(q)`` - -# **Trajectory operation**: - -# If ``len(T) > 1`` it is considered to be a trajectory, and the result -# is a list of named tuples such that ``sol[k]`` corresponds to -# ``T[k]``. The initial estimate of q for each time step is taken as the -# solution from the previous time step. - -# .. note:: - -# - See `Toolbox kinematics wiki page -# `_ -# - Uses ``SciPy.minimize`` with bounds. -# - Joint limits are considered in this solution. -# - Can be used for robots with arbitrary degrees of freedom. -# - The inverse kinematic solution is generally not unique, and -# depends on the initial guess ``q0``. -# - The default value of ``q0`` is zero which is a poor choice for -# most manipulators since it often corresponds to a -# kinematic singularity. -# - Such a solution is completely general, though much less -# efficient than analytic inverse kinematic solutions derived -# symbolically. -# - The objective function (error) is -# :math:`\sum \left( (\mat{T}^{-1} \cal{K}(\vec{q}) - \mat{1} ) \mat{\Omega} \right)^2` -# where :math:`\mat{\Omega}` is a diagonal matrix. -# - Joint offsets, if defined, are accounted for in the solution. - -# .. warning:: - -# - The objective function is rather uncommon. -# - Order of magnitude slower than ``ikine_LM`` or ``ikine_LMS``, it -# uses a scalar cost-function and does not provide a Jacobian. - -# :author: Bryan Moutrie, for RTB-MATLAB - -# :seealso: :func:`ikine_LM`, :func:`ikine_LMS`, :func:`ikine_unc`, -# :func:`ikine_min` - -# """ # noqa E501 - -# if not isinstance(T, SE3): -# raise TypeError("argument must be SE3") - -# if isinstance(self, rtb.DHRobot): -# end = None - -# if q0 is None: -# q0 = np.zeros((self.n)) -# else: -# q0 = base.getvector(q0, self.n) - -# solutions = [] - -# wr = 1 / self.reach -# weight = np.r_[wr, wr, wr, 1, 1, 1] - -# optdict = {"maxiter": ilimit} -# if options is not None and isinstance(options, dict): -# optdict.update(options) -# else: -# raise ValueError("options must be a dict") - -# if qlim: -# # dealing with joint limits -# bounds = opt.Bounds(self.qlim[0, :], self.qlim[1, :]) - -# if method is None: -# method = "trust-constr" -# else: -# # no joint limits -# if method is None: -# method = "SLSQP" -# bounds = None - -# def cost(q, T, weight, costfun, stiffness): -# # T, weight, costfun, stiffness = args -# e = _angle_axis(self.fkine(q, end=end).A, T) * weight -# E = (e**2).sum() - -# if stiffness > 0: -# # Enforce a continuity constraint on joints, minimum bend -# E += np.sum(np.diff(q) ** 2) * stiffness - -# if costfun is not None: -# E += (e**2).sum() + costfun(q) - -# return E - -# for Tk in T: -# res = opt.minimize( -# cost, -# q0, -# args=(Tk.A, weight, costfun, stiffness), -# bounds=bounds, -# method=method, -# tol=tol, -# options=options, -# ) - -# # trust-constr seems to work better than L-BFGS-B which often -# # runs a joint up against its limit and terminates with position -# # error. -# # but 'truts-constr' is 5x slower - -# solution = iksol(res.x, res.success, res.message, res.nit, res.fun) -# solutions.append(solution) -# q0 = res.x # use this solution as initial estimate for next time - -# if len(T) == 1: -# return solutions[0] -# else: -# return solutions - -# # --------------------------------------------------------------------- # - -# def ikine_global( -# self, T, qlim=False, ilimit=1000, tol=1e-16, method=None, options={}, end=None -# ): -# r""" -# .. warning:: Experimental code for using SciPy global optimizers. - -# Each global optimizer has quite a different call signature, so final -# design will need a bit of thought. - -# """ - -# # basinhopping: -# # brute: ranges, finish=None -# # differential_evolution: bounds, tol -# # shgo: bounds, options:f_tol -# # dual_annealing: bounds - -# if not isinstance(T, SE3): -# raise TypeError("argument must be SE3") - -# if isinstance(self, rtb.DHRobot): -# end = None - -# solutions = [] - -# # wr = 1 / self.reach -# # weight = np.r_[wr, wr, wr, 1, 1, 1] - -# optdict = {} - -# if method is None: -# method = "differential-evolution" - -# if method == "brute": -# # requires a tuple of tuples -# optdict["ranges"] = tuple([tuple(li.qlim) for li in self]) -# else: -# optdict["bounds"] = tuple([tuple(li.qlim) for li in self]) - -# if method not in [ -# "basinhopping", -# "brute", -# "differential_evolution", -# "shgo", -# "dual_annealing", -# ]: -# raise ValueError("unknown global optimizer requested") - -# global_minimizer = opt.__dict__[method] - -# def cost(q, T, weight): -# # T, weight, costfun, stiffness = args -# e = _angle_axis(self.fkine(q, end=end).A, T) * weight -# return (e**2).sum() - -# for _ in T: -# res = global_minimizer(cost, **optdict) - -# solution = iksol(res.x, res.success, res.message, res.nit, res.fun) -# solutions.append(solution) - -# # q0 was not used so I commented it out -# # q0 = res.x # use this solution as initial estimate for next time - -# if len(T) == 1: -# return solutions[0] -# else: -# return solutions - - -# def _angle_axis(T, Td): -# d = base.transl(Td) - base.transl(T) -# R = base.t2r(Td) @ base.t2r(T).T -# li = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]] - -# if base.iszerovec(li): -# # diagonal matrix case -# if np.trace(R) > 0: -# # (1,1,1) case -# a = np.zeros((3,)) -# else: -# a = np.pi / 2 * (np.diag(R) + 1) -# else: -# # non-diagonal matrix case -# ln = base.norm(li) -# a = math.atan2(ln, np.trace(R) - 1) * li / ln - -# return np.r_[d, a] - - -# def _angle_axis_sekiguchi(T, Td): -# d = base.transl(Td) - base.transl(T) -# R = base.t2r(Td) @ base.t2r(T).T -# li = np.r_[R[2, 1] - R[1, 2], R[0, 2] - R[2, 0], R[1, 0] - R[0, 1]] - -# if base.iszerovec(li): -# # diagonal matrix case -# if np.trace(R) > 0: -# # (1,1,1) case -# a = np.zeros((3,)) -# else: -# # (1, -1, -1), (-1, 1, -1) or (-1, -1, 1) case -# a = np.pi / 2 * (np.diag(R) + 1) -# # as per Sekiguchi paper -# if R[1, 0] > 0 and R[2, 1] > 0 and R[0, 2] > 0: -# a = np.pi / np.sqrt(2) * np.sqrt(np.diag(R) + 1) -# elif R[1, 0] > 0: # (2) -# a = np.pi / np.sqrt(2) * np.sqrt(np.diag(R) @ np.r_[1, 1, -1] + 1) -# elif R[0, 2] > 0: # (3) -# a = np.pi / np.sqrt(2) * np.sqrt(np.diag(R) @ np.r_[1, -1, 1] + 1) -# elif R[2, 1] > 0: # (4) -# a = np.pi / np.sqrt(2) * np.sqrt(np.diag(R) @ np.r_[-1, 1, 1] + 1) -# else: -# # non-diagonal matrix case -# ln = base.norm(li) -# a = math.atan2(ln, np.trace(R) - 1) * li / ln - -# return np.r_[d, a] - - -# if __name__ == "__main__": # pragma nocover - -# # import roboticstoolbox as rtb -# # from spatialmath import SE3 - -# # np.set_printoptions( -# # linewidth=120, formatter={'float': lambda x: f"{x:9.5g}" -# # if abs(x) > 1e-10 else f"{0:9.5g}"}) - -# robot = rtb.models.DH.Panda() - -# T = SE3(0.7, 0.2, 0.1) * SE3.OA([0, 1, 0], [0, 0, -1]) -# # sol = robot.ikine_LMS(T) # solve IK -# # print(sol) # display joint angles - -# # print(T) -# # print(robot.fkine(sol.q)) -# # robot.plot(sol.q) - -# # sol = robot.ikine_unc( -# # T, costfun=lambda q: q[1] * 1e-6 if q[1] > 0 else -q[1]) -# # print(sol) -# # print(robot.fkine(sol.q)) -# # robot.plot(sol.q) - -# sol = robot.ikine_global(T, method="brute") +if __name__ == "__main__": # pragma nocover + sol = IKSolution( + np.array([1, 2, 3]), success=True, iterations=10, searches=100, residual=0.1 + ) + + a, b, c, d, e = sol + + print(a, b, c, d, e) diff --git a/roboticstoolbox/robot/KinematicCache.py b/roboticstoolbox/robot/KinematicCache.py deleted file mode 100644 index c6adbbbf9..000000000 --- a/roboticstoolbox/robot/KinematicCache.py +++ /dev/null @@ -1,459 +0,0 @@ -# import numpy as np -# from collections import OrderedDict -# import roboticstoolbox as rtb -# import timeit - -# np.set_printoptions(linewidth=np.inf) - - -# class KinematicCache: -# """ -# Kinematic cache - -# Many robot kinematic (and dynamic operations) have dependencies. For -# example, computing the world-frame Jacobian requires the -# forward kinematics, computing operational space acceleration requires -# the Jacobian. To optimize computation time it becomes difficult to keep -# track of all the dependencies. - -# The ``KinematicCache`` acts as a proxy for a ``Robot`` subclass object -# and implements a subset of its methods, just those that concerned with, or -# using kinematics. - -# For everycall a hash is computed for ``q`` and relevant arguments such as -# ``end`` and the value of kinematic operation is looked up in the cache. If -# it is not in the cache it will be computed and added to the cache. - -# For example:: - -# robot = models.ETS.Panda() -# kc = KinematicCache(robot) - -# q = robot.qr -# T = kc.fkine(q) -# J = kc.jacob0(q) -# Ix = kc.inertia_x(q) -# J = kc.jacob0(q) - -# The ``fkine`` method will be a cache miss and the forward kinematics will be -# computed. The ``jacob0`` method will be a cache miss but the required -# forward kinematics are in the cache and will be used. The ``inertia_x`` -# method will be a cache miss but the required Jacobian is in the cache and -# will be used. The final ``jacob0`` method will be a cache hit and the -# previously computed value will be returned. - -# The cost of computing the hash is small compared to the cost of the -# kinematic operations and not having to keep track of saved values makes code -# cleaner. - -# """ -# def __init__(self, robot, cachesize=16): -# """ -# Create kinematic cache instance - -# :param robot: robot to be cached -# :type robot: Robot subclass instance -# :param cachesize: maximum length of cache, defaults to 16 -# :type cachesize: int, optional - -# The cache is an ordered dictionary indexed by function, joint angles and -# method arguments. If you use N different cached functions at each -# timestep then ``cachesize`` should be at least N. -# """ -# self._robot = robot -# self._cachesize = cachesize -# self._dict = OrderedDict() - -# def __str__(self): -# s = f"KinematicCache({self._robot.name})" -# return s - -# def __repr__(self): -# return str(self) - -# def __len__(self): -# """ -# Length of kinematic cache - -# :return: number of cache entries -# :rtype: int - -# This is the length of the cache dictionary. -# """ -# return len(self._dict) - -# def cache(self): -# """ -# Display kinematic cache - -# :return: cache entries, one per line -# :rtype: str - -# The cache dictionary is displayed. Oldest entries are first. -# For example, the display:: - -# fkine_all : 0x59913cdb1a5be5c0, (None,) -# fkine : 0xb9cd1db3d2a255e0, (None,) -# fkine_all : 0xb9cd1db3d2a255e0, (None,) -# fkine : 0x639cf014e2baaafb, (None,) - -# shows the kinematic function, the joint configuration hash, and any -# additional arguments. -# """ -# s = "" -# for key in self._dict.keys(): -# s += f"{key[0]:12s}: {np.uint64(key[1]):#0x}, {key[2:]}\n" -# return s - -# def _qhash(self, q): -# """ -# Compute joint configuration hash - -# :param q: joint configuration :type q: ndarray(N) :return: hash :rtype: -# int - -# Returns an integer hash of the joint configuration and trims the cache -# to length of ``cachesize`` - -# .. note:: Uses ``hash(q.tobytes('C'))`` as the hash, takes around 250us. - -# .. note:: Hashing and cache trimming could be a separate methods, but -# since hash has to be computed for every cached kinematic function -# it's quicker to merge both functions. -# """ -# # cache is an ordered dict, new entries go on the end to old entries -# # are popped from the front (last=False) -# while len(self._dict) > self._cachesize: -# self._dict.popitem(last=False) - -# # compute and return the hash -# return hash(q.tobytes("C")) - -# # TODO: -# # this needs to accept a common subset of arguments for DHRobot and ERobot classes. -# # that means end, tool and start. Need to rationalise these -# # end can be ELink or str, means we have to make Elink hashable -# # if tool is allowed, then SE3 needs to be hashable, probably not a bad idea -# def fkine(self, q, end=None): -# """ -# Cached forward kinematics - -# :param q: Joint configuration -# :type q: ndarray(n) -# :param end: specific end-effector, defaults to None -# :type end: str or ELink instance -# :return: forward kinematics -# :rtype: SE3 instance - -# :seealso: :func:`DHRobot.fkine`, :func:`ERobot.fkine` -# """ -# # compute the key we use for cache lookup -# qhash = self._qhash(q) -# key = ("fkine", qhash, end) -# if key not in self._dict: -# # cache miss, check if we have fkine_all() result -# key_fkall = ("fkine_all", qhash, end) -# if key_fkall in self._dict: -# # we do, take just the EE pose and cache that -# self._dict[key] = self._dict[key_fkall][-1] -# else: -# # nothing cached, compute fkine() the hard way and cache it -# self._dict[key] = self._robot.fkine(q) -# # return the cached value -# return self._dict[key] - -# def fkine_all(self, q, end=None): -# """ -# Cached forward kinematics for all frames - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :return: all link frames including base -# :rtype: multi-valuedSE3 instance -# """ - -# key = ("fkine_all", self._qhash(q), end) -# if key not in self._dict: -# # cache miss, compute it -# self._dict[key] = self._robot.fkine_all(q) -# return self._dict[key] - -# def jacob0(self, q, end=None, analytical=None, half=None): -# """ -# Cached world-frame Jacobian - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :param half: return half Jacobian: 'trans' or 'rot' -# :type half: str -# :param analytical: return analytical Jacobian instead of geometric Jacobian (default) -# :type analytical: str -# :return: Jacobian in world frame -# :rtype: ndarray(6, n) -# """ -# # NOTE: we could write -# # def jacob0(self, q, **kwargs): -# # key = ('jacob0', self._qhash(q), tuple(kwargs.keys())) -# # which is more elegant/compact but the order of the kw arguments would -# # then become important -# key = ("jacob0", self._qhash(q), end, analytical, half) -# if key not in self._dict: -# # cache miss, compute it - -# # get fkine from the cache if possible -# # TODO: fkine() will have to compute the hash again, maybe pass it -# # down as argument _qhash -# T = self.fkine(q, end=end) -# self._dict[key] = self._robot.jacob0( -# q, T=T, end=end, half=half, analytical=analytical -# ) -# return self._dict[key] - -# def jacob0_inv(self, q, end=None, analytical=None): -# """ -# Cached world-frame Jacobian inverse - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :param analytical: return analytical Jacobian instead of geometric Jacobian (default) -# :type analytical: str -# :return: Inverse Jacobian in world frame -# :rtype: ndarray(6, n) - -# .. note:: Robot objects don't have this method. -# """ -# key = ("jacob0_inv", self._qhash(q), end, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian from cache -# J = self.jacob0(q, end=end, analytical=analytical) -# self._dict[key] = np.linalg.inv(J) -# return self._dict[key] - -# def jacob0_pinv(self, q, end=None, analytical=None): -# """ -# Cached world-frame Jacobian pseudo inverse - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :param analytical: return analytical Jacobian instead of geometric Jacobian (default) -# :type analytical: str -# :return: Pseudo inverse Jacobian in world frame -# :rtype: ndarray(6, n) - -# .. note:: Robot objects don't have this method. -# """ -# key = ("jacob0_pinv", self._qhash(q), end, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian from cache -# J = self.jacob0(q, end=end, analytical=analytical) -# self._dict[key] = np.linalg.pinv(J) -# return self._dict[key] - -# # TODO jacobe doesnt support end for DHRobot, should it have analytical -# def jacobe(self, q, end=None, half=None): -# """ -# Cached world-frame Jacobian - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :param half: return half Jacobian: 'trans' or 'rot' -# :type half: str -# :return: Jacobian in end-effector-frame -# :rtype: ndarray(6, n) -# """ -# # NOTE: we could write -# # def jacob0(self, q, **kwargs): -# # key = ('jacob0', self._qhash(q), tuple(kwargs.keys())) -# # which is more elegant/compact but the order of the kw arguments would -# # then become important -# key = ("jacobe", self._qhash(q), end, half) -# if key not in self._dict: -# # cache miss, compute it - -# # get fkine from the cache if possible -# if isinstance(self._robot, DHRobot): -# T = None # actually not needed for DHRobot case -# else: -# T = self.fkine(q, end=end) -# self._dict[key] = self._robot.jacobe(q, T=T, end=end) -# return self._dict[key] - -# def jacobe_inv(self, q, end=None, analytical=None): -# """ -# Cached end-effector-frame Jacobian inverse - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :return: Inverse Jacobian in end-effector-frame -# :rtype: ndarray(6, n) - -# .. note:: Robot objects don't have this method. -# """ -# key = ("jacobe_inv", self._qhash(q), end) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian from cache -# J = self.jacobe(q, end=end) -# self._dict[key] = np.linalg.inv(J) -# return self._dict[key] - -# def jacobe_pinv(self, q, end=None, analytical=None): -# """ -# Cached end-effector-frame Jacobian pseudo inverse - -# :param q: joint configuration -# :type q: ndarray(n) -# :param end: specific end effector, defaults to None -# :type end: str or ELink instance, optional -# :return: Pseudo inverse Jacobian in end-effector-frame -# :rtype: ndarray(6, n) - -# .. note:: Robot objects don't have this method. -# """ -# key = ("jacob0_pinv", self._qhash(q), end, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian from cache -# J = self.jacobe(q, end=end) -# self._dict[key] = np.linalg.pinv(J) -# return self._dict[key] - -# def inertia(self, q): -# key = ("inertia", self._qhash(q), self._qhash(qd)) -# if key not in self._dict: -# # cache miss, compute it -# C = self.inertia(q) -# self._dict[key] = self._robot.inertia(q) -# return self._dict[key] - -# def coriolis(self, q, qd): -# key = ("coriolis", self._qhash(q), self._qhash(qd)) -# if key not in self._dict: -# # cache miss, compute it -# C = self.coriolis(q, qd) -# self._dict[key] = self._robot.coriolis(q, qd) -# return self._dict[key] - -# def gravload(self, q): -# key = ("gravload", self._qhash(q), self._qhash(qd)) -# if key not in self._dict: -# # cache miss, compute it -# C = self.gravload(q) -# self._dict[key] = self._robot.gravload(q) -# return self._dict[key] - -# def inertia_x(self, q, pinv=False, analytical="rpy-xyz"): -# key = ("inertia_x", self._qhash(q), pinv, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian inv or pinv from cache -# if pinv: -# Ji = self.jacobe0_pinv(analytical=analytical) -# else: -# Ji = self.jacobe0_inv(analytical=analytical) -# self._dict[key] = self._robot.inertia_x(q, Ji=Ji) -# return self._dict[key] - -# def coriolis_x(self, q, qd, pinv=False, analytical="rpy-xyz"): -# key = ("coriolis_x", self._qhash(q), self._qhash(qd), pinv, analytical) -# if key not in self._dict: -# # cache miss, compute it -# Ja = self.jacob0(q, pinv=pinv, analytical=analytical) -# # get Jacobian inv or pinv from cache -# if pinv: -# Ji = self.jacobe0_pinv(analytical=analytical) -# else: -# Ji = self.jacobe0_inv(analytical=analytical) -# # get inertia, Jacobian dot and Coriolis from cache -# Mx = self.inertia_x(q, pinv=pinv, analytical=analytical) -# Jd = self.jacob_dot(q, J0=self.jacob0(q, J0=Ja, analytical=analytical)) -# C = self.coriolis(q, qd) -# self._dict[key] = self._robot.coriolis_x(q, qd, pinv, analytical, -# J=Ja, Ji=Ji, Jd=JD, C=C, Mx=Mx) -# return self._dict[key] - -# def gravload_x(self, q, pinv=False, analytical="rpy-xyz"): -# key = ("gravload_x", self._qhash(q), pinv, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# # get Jacobian inv or pinv from cache -# if pinv: -# Ji = self.jacobe0_pinv(analytical=analytical) -# else: -# Ji = self.jacobe0_inv(analytical=analytical) -# self._dict[key] = self._robot.gravload_x(q, Ji=Ji) -# return self._dict[key] - -# def accel_x(self, q, qd, xd, wrench, pinv=False, analytical="rpy-xyz"): -# key = ("accel_x", self._qhash(q), pinv, analytical) -# if key not in self._dict: -# # cache miss, compute it - -# Ja = self.jacob0(q, pinv=pinv, analytical=analytical) -# # get Jacobian inv or pinv from cache -# if pinv: -# Ji = self.jacobe0_pinv(analytical=analytical) -# else: -# Ji = self.jacobe0_inv(analytical=analytical) -# C = self.coriolis(q, qd) -# Jd = self.jacob_dot(q, J0=Ja) - -# Mx = self.inertia_x(q, Ji=Ji, pinv=pinv, analytical=analytical) -# Cx = self.coriolis_x(q, qd, pinv=pinv, analytical=analytical, -# J=Ja, Ji=Ji, Jd=Jd, C=C, Mx=Mx) -# Gx = self.gravload_x(q, Ji=Ji) -# self._dict[key] = np.linalg.inv(Mx) @ (wrench - Cx @ xd - Gx) -# return self._dict[key] - - -# if __name__ == "__main__": -# q = np.r_[1, 2, 3, 4, 5, 6, 7] - -# panda = rtb.models.DH.Panda() -# kc = KinematicCache(panda) -# print(kc) -# print(panda.fkine(panda.qr)) -# print(panda.fkine(panda.qr)) -# print("--") -# print(panda.jacob0(panda.qr)) -# print(panda.jacob0(panda.qr)) -# print("--") - -# def timething(statement, N=1_000): -# t = timeit.timeit(stmt=statement, globals=globals(), number=N) -# print(f"{statement:>25s}: {t / N * 1e6:.1f}μs") - -# timething("kc._qhash(q)", 1_000_000) -# timething("panda.fkine(panda.qr)", 1_000) -# timething("kc.fkine(panda.qr)", 1_000_000) - -# kc.fkine_all(panda.qr) -# print(len(kc)) -# print(kc.cache()) - -# for i in range(50): -# q = np.random.rand(7) -# kc.fkine(q) -# kc.fkine_all(q) -# print(len(kc)) -# print(kc.cache()) diff --git a/roboticstoolbox/robot/Link.py b/roboticstoolbox/robot/Link.py index fbdd158eb..c8b6dc2b8 100644 --- a/roboticstoolbox/robot/Link.py +++ b/roboticstoolbox/robot/Link.py @@ -1,5 +1,7 @@ -from copy import copy as ccopy, deepcopy +from copy import deepcopy from abc import ABC +from typing_extensions import Self + # from multiprocessing.sharedctypes import Value import numpy as np from functools import wraps @@ -8,13 +10,17 @@ from ansitable import ANSITable, Column from spatialgeometry import Shape, SceneNode, SceneGroup from typing import List, Union, Tuple, overload + import roboticstoolbox as rtb from roboticstoolbox.robot.ETS import ETS, ETS2 from roboticstoolbox.robot.ET import ET, ET2 -from numpy import eye, ndarray, array, diag from warnings import warn -ArrayLike = Union[list, ndarray, tuple, set] +from roboticstoolbox.tools.types import ArrayLike, NDArray + +# A generic type variable representing any subclass of BaseETS +# ETSType = TypeVar("ETSType", bound=BaseETS) +# ETType = TypeVar("ETType", bound=BaseET) def _listen_dyn(func): @@ -55,40 +61,51 @@ def wrapper_listen_dyn(*args): class BaseLink(SceneNode, ABC): """ - Link superclass - - :param name: name of the link - :type name: str - - :param qlim: joint variable limits [min, max] - :type qlim: float ndarray(1,2) - :param flip: joint moves in opposite direction - :type flip: bool - - :param m: dynamic - link mass - :type m: float - :param r: dynamic - position of COM with respect to link frame - :type r: float ndarray(3) - :param I: dynamic - inertia of link with respect to COM - :type I: ndarray - - :param Jm: dynamic - motor inertia - :type Jm: float - :param B: dynamic - motor viscous friction - :type B: float, or ndarray(2,) - :param Tc: dynamic - motor Coulomb friction [Tc⁺, Tc⁻] - :type Tc: ndarray(2,) - :param G: dynamic - gear ratio - :type G: float - An abstract link superclass for all link types. + Parameters + ---------- + ets + kinematic - The elementary transforms which make up the link + name + name of the link + parent + a reference to the parent link in the kinematic chain + joint_name + the name of the joint variable + m + dynamic - link mass + r + dynamic - position of COM with respect to link frame + I + dynamic - inertia of link with respect to COM + Jm + dynamic - motor inertia + B + dynamic - motor viscous friction + Tc + dynamic - motor Coulomb friction [Tc⁺, Tc⁻] + G + dynamic - gear ratio + qlim + joint variable limits [min, max] + geometry + the visual geometry which represents the link. This is used + to display the link in Swift + collision + the collision geometry which represents the link in collision + checkers + + .. inheritance-diagram:: roboticstoolbox.RevoluteDH roboticstoolbox.PrismaticDH roboticstoolbox.RevoluteMDH roboticstoolbox.PrismaticMDH roboticstoolbox.Link :top-classes: roboticstoolbox.robot.Link :parts: 2 + Synopsis + -------- + It holds metadata related to: - a robot link, such as rigid-body inertial parameters defined in the link @@ -96,10 +113,11 @@ class BaseLink(SceneNode, ABC): - a robot joint, that connects this link to its parent, such as joint limits, direction of motion, motor and transmission parameters. - .. note:: - - For a more sophisticated actuator model use the ``actuator`` - attribute which is not initialized or used by this Toolbox. - - There is no ability to name a joint as supported by URDF + Notes + ----- + - For a more sophisticated actuator model use the ``actuator`` + attribute which is not initialized or used by this Toolbox. + - There is no ability to name a joint as supported by URDF """ @@ -107,7 +125,7 @@ def __init__( self, ets: Union[ETS, ETS2, ET, ET2] = ETS(), name=None, - parent: Union["BaseLink", str, None] = None, + parent: Union[Self, str, None] = None, joint_name: Union[str, None] = None, m: Union[float, None] = None, r: Union[ArrayLike, None] = None, @@ -181,12 +199,13 @@ def dynpar(self, name, value, default): # Check parent argument if parent is not None: - if isinstance(parent, BaseLink): - self._parent = parent - self._parent_name = None - elif isinstance(parent, str): - self._parent = None + if isinstance(parent, str): + self.parent = None self._parent_name = parent + elif isinstance(parent, BaseLink): + self.parent = parent + self._parent_name = None + else: raise TypeError("parent must be BaseLink subclass") else: @@ -196,6 +215,8 @@ def dynpar(self, name, value, default): self._joint_name = joint_name self._children = [] + self.number = 0 + # Set the qlim if provided if qlim is not None and self.v: self.v.qlim = qlim @@ -205,63 +226,89 @@ def dynpar(self, name, value, default): def _init_Ts(self): # Compute the leading, constant, part of the ETS - # Ts can not be equal to None otherwise things seem - # to break everywhere, so initialise Ts np be identity - if isinstance(self, Link2): - T = eye(3) + T = None else: - T = eye(4) + T = None for et in self._ets: # constant transforms only if et.isjoint: break else: - T = T @ et.A() + if T is None: + T = et.A() + else: + T = T @ et.A() self._Ts = T @property - def Ts(self) -> ndarray: + def Ts(self) -> Union[NDArray, None]: """ Constant part of link ETS - :return: constant part of link transform - :rtype: SE3 instance + The ETS for each Link comprises a constant part (possible the identity) followed by an optional joint variable transform. This property returns the constant part. If no constant part is given, this returns an identity matrix. + + Returns + ------- + Ts + constant part of link transform + + Examples + -------- .. runblock:: pycon - >>> from roboticstoolbox import Link, ET - >>> link = Link( ET.tz(0.333) * ET.Rx(90, 'deg') * ET.Rz() ) - >>> link.Ts - >>> link = Link( ET.Rz() ) - >>> link.Ts + >>> from roboticstoolbox import Link, ET + >>> link = Link( ET.tz(0.333) * ET.Rx(90, 'deg') * ET.Rz() ) + >>> link.Ts + >>> link = Link( ET.Rz() ) + >>> link.Ts + """ return self._Ts @overload def ets(self: "Link") -> ETS: - ... + ... # pragma: nocover @overload def ets(self: "Link2") -> ETS2: - ... + ... # pragma: nocover @property - def ets(self): + def ets(self) -> ETS: + """ + Get/set link ets + + - ``link.ets`` is the link ets + - ``link.ets = ...`` checks and sets the link ets + + Parameters + ---------- + ets + the new link ets + + Returns + ------- + ets + the current link ets + + """ + return self._ets @ets.setter @overload def ets(self: "Link", new_ets: ETS): - ... + ... # pragma: nocover @ets.setter @overload def ets(self: "Link2", new_ets: ETS2): - ... + ... # pragma: nocover @ets.setter def ets(self, new_ets): @@ -281,7 +328,7 @@ def ets(self, new_ets): self._v = None self._isjoint = False - def __repr__(self): + def __repr__(self) -> str: s = self.__class__.__name__ + "(" if len(self.ets) > 0: s += repr(self.ets) + ", " @@ -289,12 +336,19 @@ def __repr__(self): s += ")" return s - def __str__(self): + def __str__(self) -> str: """ - Pretty prints the ETS Model of the link. Will output angles in degrees - :return: Pretty print of the robot link - :rtype: str + Pretty prints the ETS Model of the link + + Will output angles in degrees + + Returns + ------- + str + pretty print of the robot link + """ + s = self.__class__.__name__ + "(" if self.name is not None: s += f'"{self.name}"' @@ -313,55 +367,55 @@ def __str__(self): # parent = f" [{self.parent.name}]" params = self._params(name=False) if len(params) > 0: - s += ", " + s += ", " # pragma: nocover s += ", ".join(params) s += ")" return s def _repr_pretty_(self, p, cycle): """ - Pretty string for IPython (superclass method) - - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) + Pretty string for IPython Print colorized output when variable is displayed in IPython, ie. on a line by itself. - Example:: - - In [1]: x + Parameters + ---------- + p + pretty printer handle (ignored) + cycle + pretty printer flag (ignored) """ - # see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html + # see + # https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html - p.text(str(self)) + p.text(str(self)) # pragma: nocover # -------------------------------------------------------------------------- # - def copy(self, parent: Union["BaseLink", None] = None): + @overload + def copy(self: "Link") -> "Link": + ... # pragma: nocover + + @overload + def copy(self: "Link2") -> "Link2": + ... # pragma: nocover + + def copy(self): """ Copy of link object - :return: Shallow copy of link object - :rtype: Link - ``link.copy()`` is a new Link subclass instance with a copy of all the parameters. - """ - # new = ccopy(self) - # for k, v in self.__dict__.items(): - # # print(k) - # if k.startswith("_") and isinstance(v, np.ndarray): - # setattr(new, k, np.copy(v)) - # new._geometry = [shape.copy() for shape in self._geometry] - # new._collision = [shape.copy() for shape in self._collision] + Returns + ------- + link + copy of link object + + """ - # # invalidate references to parent, child - # new._parent = parent - # new._children = [] - # return new return deepcopy(self) def _copy(self): @@ -397,8 +451,8 @@ def __deepcopy__(self, memo): Tc=Tc, G=G, qlim=qlim, - geometry=geometry, - collision=collision, + geometry=geometry, # type: ignore + collision=collision, # type: ignore ) if self._children: @@ -413,25 +467,33 @@ def __deepcopy__(self, memo): @overload def v(self: "Link") -> Union["ET", None]: - ... + ... # pragma: nocover @overload def v(self: "Link2") -> Union["ET2", None]: - ... + ... # pragma: nocover @property def v(self): """ Variable part of link ETS - :return: joint variable transform - :rtype: ET instance + The ETS for each Link comprises a constant part (possible the identity) followed by an optional joint variable transform. This property returns the latter. + + Returns + ------- + v + joint variable transform + + Examples + -------- .. runblock:: pycon - >>> from roboticstoolbox import Link, ETS - >>> link = Link( ET.tz(0.333) * ET.Rx(90, 'deg') * ETS.Rz() ) - >>> print(link.v) + >>> from roboticstoolbox import Link, ETS + >>> link = Link( ET.tz(0.333) * ET.Rx(90, 'deg') * ETS.Rz() ) + >>> print(link.v) + """ return self._v @@ -443,31 +505,40 @@ def name(self) -> str: Get/set link name - ``link.name`` is the link name + - ``link.name = ...`` checks and sets the link name - :return: link name - :rtype: str + Returns + ------- + name + link name - - ``link.name = ...`` checks and sets the link name """ return self._name @name.setter - def name(self, name): + def name(self, name: str): self._name = name # -------------------------------------------------------------------------- # @property - def robot(self) -> Union["rtb.Robot", None]: + def robot(self) -> Union["rtb.BaseRobot", None]: """ Get forward reference to the robot which owns this link - :return: The robot object + - ``link.robot`` is the robot reference + - ``link.robot = ...`` checks and sets the robot reference + + Returns + ------- + robot + The robot object + """ return self._robot @robot.setter - def robot(self, robot_ref: "rtb.Robot"): + def robot(self, robot_ref: "rtb.BaseRobot"): """ Set the forward reference to the robot which owns this link """ @@ -476,23 +547,29 @@ def robot(self, robot_ref: "rtb.Robot"): # -------------------------------------------------------------------------- # @property - def qlim(self) -> Union[ndarray, None]: + def qlim(self) -> Union[NDArray, None]: """ Get/set joint limits - ``link.qlim`` is the joint limits + - ``link.qlim = ...`` checks and sets the joint limits - :return: joint limits - :rtype: ndarray(2,) or Nine + Returns + ------- + qlim + joint limits - - ``link.qlim = ...`` checks and sets the joint limits + Notes + ----- + - The limits are not widely enforced within the toolbox. + - If no joint limits are specified the value is ``None`` - .. note:: - - The limits are not widely enforced within the toolbox. - - If no joint limits are specified the value is ``None`` + See Also + -------- + :func:`~islimit` - :seealso: :func:`~islimit` """ + if self.v: return self.v.qlim else: @@ -501,7 +578,7 @@ def qlim(self) -> Union[ndarray, None]: @qlim.setter def qlim(self, qlim_new: ArrayLike): if self.v: - self.v.qlim = qlim_new + self.ets.qlim = qlim_new else: raise ValueError("Can not set qlim on a static joint") @@ -510,22 +587,24 @@ def hasdynamics(self) -> bool: """ Link has dynamic parameters (Link superclass) - :return: Link has dynamic parameters - :rtype: bool - Link has some assigned (non-default) dynamic parameters. These could have been assigned: - at constructor time, eg. ``m=1.2`` - by invoking a setter method, eg. ``link.m = 1.2`` - Example: + Returns + ------- + hasdynamics + Link has dynamic parameters + Examples + -------- .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> robot[1].hasdynamics - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot[1].hasdynamics """ return self._hasdynamics @@ -537,10 +616,6 @@ def isflip(self) -> bool: Get/set joint flip - ``link.flip`` is the joint flip status - - :return: joint flip - :rtype: bool - - ``link.flip = ...`` checks and sets the joint flip status Joint flip defines the direction of motion of the joint. @@ -555,7 +630,13 @@ def isflip(self) -> bool: - revolute motion is a negative rotation about the z-axis - prismatic motion is a negative translation along the z-axis + Returns + ------- + isflip + joint flip + """ + return self.v.isflip if self.v else False # -------------------------------------------------------------------------- # @@ -566,13 +647,15 @@ def m(self) -> float: Get/set link mass - ``link.m`` is the link mass - - :return: link mass - :rtype: float - - ``link.m = ...`` checks and sets the link mass + Returns + ------- + m + link mass + """ + return self._m @m.setter @@ -583,20 +666,23 @@ def m(self, m_new: float): # -------------------------------------------------------------------------- # @property - def r(self) -> ndarray: + def r(self) -> NDArray: """ Get/set link centre of mass - - ``link.r`` is the link centre of mass - - :return: link centre of mass - :rtype: ndarray(3,) + The link centre of mass is a 3-vector defined with respect to the link + frame. + - ``link.r`` is the link centre of mass - ``link.r = ...`` checks and sets the link centre of mass - The link centre of mass is a 3-vector defined with respect to the link - frame. + Returns + ------- + r + link centre of mass + """ + return self._r # type: ignore @r.setter @@ -607,40 +693,46 @@ def r(self, r_new: ArrayLike): # -------------------------------------------------------------------------- # @property - def I(self) -> ndarray: # noqa + def I(self) -> NDArray: # noqa r""" Get/set link inertia - - ``link.I`` is the link inertia - - :return: link inertia - :rtype: ndarray(3,3) - - - ``link.I = ...`` checks and sets the link inertia - Link inertia is a symmetric 3x3 matrix describing the inertia with respect to a frame with its origin at the centre of mass, and with axes parallel to those of the link frame. + - ``link.I`` is the link inertia + - ``link.I = ...`` checks and sets the link inertia + + Returns + ------- + I + link inertia + + Synopsis + -------- + The inertia matrix is :math:`\begin{bmatrix} I_{xx} & I_{xy} & I_{xz} \\ I_{xy} & I_{yy} & I_{yz} \\I_{xz} & I_{yz} & I_{zz} \end{bmatrix}` - and can be specified as either: - a 3 ⨉ 3 symmetric matrix - a 3-vector :math:`(I_{xx}, I_{yy}, I_{zz})` - a 6-vector :math:`(I_{xx}, I_{yy}, I_{zz}, I_{xy}, I_{yz}, I_{xz})` - .. note:: Referred to the link side of the gearbox. + Notes + ----- + - Referred to the link side of the gearbox. + """ # noqa + return self._I # type: ignore @I.setter @_listen_dyn def I(self, I_new: ArrayLike): # noqa - if ismatrix(I_new, (3, 3)): # 3x3 matrix passed if np.any(np.abs(I_new - I_new.T) > 1e-8): # type: ignore @@ -655,7 +747,7 @@ def I(self, I_new: ArrayLike): # noqa elif isvector(I_new, 6): # 6-vector passed, moments and products of inertia, # [Ixx Iyy Izz Ixy Iyz Ixz] - I_new = array( + I_new = np.array( [ [I_new[0], I_new[3], I_new[5]], # type: ignore [I_new[3], I_new[1], I_new[4]], # type: ignore @@ -665,7 +757,7 @@ def I(self, I_new: ArrayLike): # noqa elif isvector(I_new, 3): # 3-vector passed, moments of inertia [Ixx Iyy Izz] - I_new = diag(I_new) + I_new = np.diag(I_new) # type: ignore else: raise ValueError("invalid shape passed: must be (3,3), (6,), (3,)") @@ -680,14 +772,19 @@ def Jm(self) -> float: Get/set motor inertia - ``link.Jm`` is the motor inertia + - ``link.Jm = ...`` checks and sets the motor inertia - :return: motor inertia - :rtype: float + Returns + ------- + Jm + motor inertia - - ``link.Jm = ...`` checks and sets the motor inertia + Notes + ----- + - Referred to the motor side of the gearbox. - .. note:: Referred to the motor side of the gearbox. """ + return self._Jm @Jm.setter @@ -703,15 +800,18 @@ def B(self) -> float: Get/set motor viscous friction - ``link.B`` is the motor viscous friction + - ``link.B = ...`` checks and sets the motor viscous friction - :return: motor viscous friction - :rtype: float + Returns + ------- + B + motor viscous friction - - ``link.B = ...`` checks and sets the motor viscous friction + Notes + ----- + - Referred to the motor side of the gearbox. + - Viscous friction is the same for positive and negative motion. - .. note:: - - Referred to the motor side of the gearbox. - - Viscous friction is the same for positive and negative motion. """ return self._B @@ -726,15 +826,11 @@ def B(self, B_new: float): # -------------------------------------------------------------------------- # @property - def Tc(self) -> ndarray: + def Tc(self) -> NDArray: r""" Get/set motor Coulomb friction - ``link.Tc`` is the motor Coulomb friction - - :return: motor Coulomb friction - :rtype: ndarray(2) - - ``link.Tc = ...`` checks and sets the motor Coulomb friction. If a scalar is given the value is set to [T, -T], if a 2-vector it is assumed to be in the order [Tc⁺, Tc⁻] @@ -748,11 +844,19 @@ def Tc(self) -> ndarray: \tau_C^+ & \mbox{if $\dot{q} > 0$} \\ \tau_C^- & \mbox{if $\dot{q} < 0$} \end{array} \right. - .. note:: - - Referred to the motor side of the gearbox. - - :math:`\tau_C^+` must be :math:`> 0`, and :math:`\tau_C^-` must - be :math:`< 0`. + Returns + ------- + Tc + motor Coulomb friction + + Notes + ----- + - Referred to the motor side of the gearbox. + - :math:`\tau_C^+` must be :math:`> 0`, and :math:`\tau_C^-` must + be :math:`< 0`. + """ + return self._Tc @Tc.setter @@ -768,7 +872,7 @@ def Tc(self, Tc_new: ArrayLike): # Coulomb friction model. FP>0 and FM<0. FP is applied for a # positive joint velocity and FM for a negative joint # velocity. - Tc_new = array(getvector(Tc_new, 2)) + Tc_new = np.array(getvector(Tc_new, 2)) self._Tc = Tc_new @@ -780,18 +884,24 @@ def G(self) -> float: Get/set gear ratio - ``link.G`` is the transmission gear ratio + - ``link.G = ...`` checks and sets the gear ratio - :return: gear ratio - :rtype: float + Returns + ------- + G + gear ratio - - ``link.G = ...`` checks and sets the gear ratio + Notes + ----- + - The ratio of motor motion : link motion + - The gear ratio can be negative, see also the ``flip`` attribute. - .. note:: - - The ratio of motor motion : link motion - - The gear ratio can be negative, see also the ``flip`` attribute. + See Also + -------- + :func:`flip` - :seealso: :func:`flip` """ + return self._G @G.setter @@ -805,12 +915,14 @@ def G(self, G_new: float): def geometry(self) -> SceneGroup: """ Get/set joint visual geometry + - ``link.geometry`` is the list of the visual geometries which represent the shape of the link :return: the visual geometries :rtype: list of Shape - ``link.geometry = ...`` checks and sets the geometry - ``link.geometry.append(...)`` add geometry + """ return self._geometry @@ -818,19 +930,22 @@ def geometry(self) -> SceneGroup: def collision(self) -> SceneGroup: """ Get/set joint collision geometry + + The collision geometries are what is used to check for collisions. + - ``link.collision`` is the list of the collision geometries which represent the collidable shape of the link. :return: the collision geometries :rtype: list of Shape - ``link.collision = ...`` checks and sets the collision geometry - ``link.collision.append(...)`` add collision geometry - The collision geometries are what is used to check for collisions. + """ + return self._collision @collision.setter def collision(self, coll: Union[SceneGroup, List[Shape], Shape]): - if isinstance(coll, list): self.collision.scene_children = coll # type: ignore elif isinstance(coll, Shape): @@ -840,7 +955,6 @@ def collision(self, coll: Union[SceneGroup, List[Shape], Shape]): @geometry.setter def geometry(self, geom: Union[SceneGroup, List[Shape], Shape]): - if isinstance(geom, list): self.geometry.scene_children = geom # type: ignore elif isinstance(geom, Shape): @@ -854,17 +968,27 @@ def geometry(self, geom: Union[SceneGroup, List[Shape], Shape]): def isjoint(self) -> bool: """ Test if link has joint - :return: test if link has a joint - :rtype: bool + The ETS for each Link comprises a constant part (possible the identity) followed by an optional joint variable transform. - This property returns the whether the + This property returns the whether the Link contains the + variable transform. + + Returns + ------- + isjoint + test if link has a joint + + Examples + -------- .. runblock:: pycon - >>> from roboticstoolbox import models - >>> robot = models.URDF.Panda() - >>> robot[1].isjoint # link with joint - >>> robot[8].isjoint # static link + >>> from roboticstoolbox import models + >>> robot = models.URDF.Panda() + >>> robot[1].isjoint # link with joint + >>> robot[8].isjoint # static link + """ + # return self.v.isjoint if self.v else False return self._isjoint @@ -872,17 +996,26 @@ def isjoint(self) -> bool: def jindex(self) -> Union[None, int]: """ Get/set joint index + - ``link.jindex`` is the joint index - :return: joint index - :rtype: int - ``link.jindex = ...`` checks and sets the joint index + For a serial-link manipulator the joints are numbered starting at zero and increasing sequentially toward the end-effector. For branched mechanisms this is not so straightforward. The link's ``jindex`` property specifies the index of its joint variable within a vector of joint coordinates. - .. note:: ``jindex`` values must be a sequence of integers starting + + Returns + ------- + jindex + joint index + + Notes + ----- + - ``jindex`` values must be a sequence of integers starting at zero. + """ return None if not self.v else self.v._jindex @@ -896,6 +1029,9 @@ def jindex(self, j: int): def isprismatic(self) -> bool: """ Checks if the joint is of prismatic type + + Returns + ------- :return: True if is prismatic :rtype: bool """ @@ -905,43 +1041,59 @@ def isprismatic(self) -> bool: def isrevolute(self) -> bool: """ Checks if the joint is of revolute type - :return: True if is revolute - :rtype: bool - """ - return self.v.isrotation if self.v else False - @overload - def parent(self: "Link") -> Union["Link", None]: - ... + Returns + ------- + isrevolute + True if is revolute - @overload - def parent(self: "Link2") -> Union["Link2", None]: - ... + """ + + return self.v.isrotation if self.v else False @property - def parent(self): + def parent(self) -> Union[Self, None]: """ Parent link - :return: Link's parent - :rtype: Link instance - This is a reference to + + This is a reference to the links parent in the kinematic + chain + + Returns + ------- + parent + Link's parent + + Examples + -------- .. runblock:: pycon - >>> from roboticstoolbox import models - >>> robot = models.URDF.Panda() - >>> robot[0].parent # base link has no parent - >>> robot[1].parent # second link's parent + >>> from roboticstoolbox import models + >>> robot = models.URDF.Panda() + >>> robot[0].parent # base link has no parent + >>> robot[1].parent # second link's parent + """ + return self._parent + @parent.setter + def parent(self, parent: Union[Self, None]): + self._parent = parent + @property def parent_name(self) -> Union[str, None]: """ Parent link name - :return: Link's parent name + Returns + ------- + parent_name + Link's parent name + """ - if self._parent is not None: - return self._parent.name + + if isinstance(self.parent, BaseLink): + return self.parent.name else: return self._parent_name @@ -949,41 +1101,63 @@ def parent_name(self) -> Union[str, None]: def children(self) -> Union[List["Link"], None]: """ List of child links - :return: child links - :rtype: list of ``Link`` instances + The list will be empty for a end-effector link + + Returns + ------- + children + child links + """ + return self._children @property def nchildren(self) -> int: """ Number of child links - :return: number of child links - :rtype: int + Will be zero for an end-effector link + + Returns + ------- + nchildren + number of child links + """ return len(self._children) def closest_point( self, shape: Shape, inf_dist: float = 1.0, skip: bool = False - ) -> Tuple[Union[int, None], Union[np.ndarray, None], Union[np.ndarray, None],]: + ) -> Tuple[Union[int, None], Union[NDArray, None], Union[NDArray, None],]: """ + Finds the closest point to a shape + closest_point(shape, inf_dist) returns the minimum euclidean distance between this link and shape, provided it is less than inf_dist. It will also return the points on self and shape in the world frame which connect the line of length distance between the shapes. If the distance is negative then the shapes are collided. + Parameters + ---------- :param shape: The shape to compare distance to :param inf_dist: The minimum distance within which to consider the shape :param skip: Skip setting all shape transforms - :returns: d, p1, p2 where d is the distance between the shapes, - p1 and p2 are the points in the world frame on the respective - shapes. The points returned are [x, y, z]. - :rtype: float, ndarray(1x3), ndarray(1x3) + Returns + ------- + d + d is the distance between the shapes + p1 + the points in the world frame on the link + shape. The points returned are [x, y, z]. + p2 + the points in the world frame the + shape. The points returned are [x, y, z]. + """ if not skip: @@ -1010,12 +1184,22 @@ def closest_point( def iscollided(self, shape: Shape, skip: bool = False) -> bool: """ - collided(shape) checks if this link and shape have collided + Checks for collision with a shape - :param shape: The shape to compare distance to - :param skip: Skip setting all shape transforms + ``iscollided(shape)`` checks if this link and shape have collided + + Parameters + ---------- + shape + The shape to compare distance to + skip + Skip setting all shape transforms + + Returns + ------- + iscollided + True if shapes have collided - :returns: True if shapes have collided """ if not skip: @@ -1031,13 +1215,24 @@ def iscollided(self, shape: Shape, skip: bool = False) -> bool: def collided(self, shape: Shape, skip: bool = False): """ - collided(shape) checks if this link and shape have collided + Checks for collision with a shape - :param shape: The shape to compare distance to - :param skip: Skip setting all shape transforms + ``iscollided(shape)`` checks if this link and shape have collided + + Parameters + ---------- + shape + The shape to compare distance to + skip + Skip setting all shape transforms + + Returns + ------- + iscollided + True if shapes have collided - :returns: True if shapes have collided """ + warn("base kwarg is deprecated, use pose instead", FutureWarning) return self.iscollided(shape=shape, skip=skip) @@ -1045,25 +1240,30 @@ def dyn(self, indent=0): """ Inertial properties of link as a string - :param indent: indent each line by this many spaces - :type indent: int - :return: The string representation of the link dynamics - :rtype: string - ``link.dyn()`` is a string representation the inertial properties of the link object in a multi-line format. The properties shown are mass, centre of mass, inertia, friction, gear ratio and motor properties. - Example: + Parameters + ---------- + indent + indent each line by this many spaces + :type indent: int + :return: The string representation of the link dynamics + :rtype: string + Examples + -------- .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> robot = rtb.models.DH.Puma560() + >>> print(robot.links[2]) # kinematic parameters + >>> print(robot.links[2].dyn()) # dynamic parameters - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> print(robot.links[2]) # kinematic parameters - >>> print(robot.links[2].dyn()) # dynamic parameters + See Also + -------- + :func:`~dyntable` - :seealso: :func:`~dyntable` """ qlim = [0, 0] if self.qlim is None else self.qlim @@ -1115,17 +1315,25 @@ def _dyn2list(self, fmt="{: .3g}"): """ Inertial properties of link as a string - :param fmt: conversion format for each number - :type fmt: str - :return: The string representation of the link dynamics - :rtype: string - - ``link.)_dyn2list()`` returns a list of pretty-printed inertial + ``link._dyn2list()`` returns a list of pretty-printed inertial properties of the link The properties included are mass, centre of mass, inertia, friction, gear ratio and motor properties. - :seealso: :func:`~dyn` + Parameters + ---------- + :param fmt: conversion format for each number + + Returns + ------- + dyn2list + The string representation of the link dynamics + + See Also + -------- + :func:`~dyn` + """ + ANSITable( Column("Parameter", headalign="^"), Column("Value", headalign="^", colalign="<"), @@ -1136,13 +1344,13 @@ def format(l, fmt, val): # noqa if isinstance(val, np.ndarray): try: s = ", ".join([fmt.format(v) for v in val]) - except TypeError: + except TypeError: # pragma: nocover # handle symbolic case s = ", ".join([str(v) for v in val]) else: try: s = fmt.format(val) - except TypeError: + except TypeError: # pragma: nocover # handle symbolic case s = str(val) l.append(s) @@ -1151,7 +1359,7 @@ def format(l, fmt, val): # noqa format(dyn, fmt, self.m) format(dyn, fmt, self.r) I = self.I.flatten() # noqa - format(dyn, fmt, np.r_[[I[k] for k in [0, 4, 8, 1, 2, 5]]]) + format(dyn, fmt, np.r_[[I[k] for k in [0, 4, 8, 1, 5, 2]]]) format(dyn, fmt, self.Jm) format(dyn, fmt, self.B) format(dyn, fmt, self.Tc) @@ -1159,99 +1367,115 @@ def format(l, fmt, val): # noqa return dyn - def _params(self, name=True): # pragma nocover - def format_param( - self, l, name, symbol=None, ignorevalue=None, indices=None - ): # noqa # pragma nocover - # if value == ignorevalue then don't display it - - v = getattr(self, name) - s = None - if v is None: - return - if isinstance(v, str): - s = f'{name} = "{v}"' - elif isscalar(v) and v != ignorevalue: - if symbol is not None: - s = f"{symbol}={v:.3g}" - else: - try: - s = f"{name}={v:.3g}" - except TypeError: - s = f"{name}={v}" - elif isinstance(v, np.ndarray): - # if np.linalg.norm(v, ord=np.inf) > 0: - # if indices is not None: - # flat = v.flatten() - # v = np.r_[[flat[k] for k in indices]] - # s = f"{name}=[" + ", ".join([f"{x:.3g}" for x in v]) + "]" - if indices is not None: - v = v.ravel()[indices] - s = f"{name}=" + np.array2string( - v, - separator=", ", - suppress_small=True, - formatter={"float": lambda x: f"{x:.3g}"}, - ) - if s is not None: - l.append(s) + def _format_param( + self, l, name, symbol=None, ignorevalue=None, indices=None + ): # noqa # pragma nocover + # if value == ignorevalue then don't display it + + v = getattr(self, name) + s = None + if v is None: + return + if isinstance(v, str): + s = f'{name} = "{v}"' + elif isscalar(v) and v != ignorevalue: + if symbol is not None: + s = f"{symbol}={v:.3g}" + else: # pragma: nocover + try: + s = f"{name}={v:.3g}" + except TypeError: + s = f"{name}={v}" + elif isinstance(v, np.ndarray): + # if np.linalg.norm(v, ord=np.inf) > 0: + # if indices is not None: + # flat = v.flatten() + # v = np.r_[[flat[k] for k in indices]] + # s = f"{name}=[" + ", ".join([f"{x:.3g}" for x in v]) + "]" + if indices is not None: + v = v.ravel()[indices] + s = f"{name}=" + np.array2string( + v, + separator=", ", + suppress_small=True, + formatter={"float": lambda x: f"{x:.3g}"}, + ) + if s is not None: + l.append(s) + + def _params(self, name: bool = True): # pragma nocover l = [] # noqa if name: - format_param(self, l, "name") + self._format_param(l, "name") if self.parent_name is not None: l.append('parent="' + self.parent_name + '"') - elif self.parent is not None: + elif isinstance(self.parent, BaseLink): l.append('parent="' + self.parent.name + '"') - format_param(self, l, "parent") - format_param(self, l, "isflip", ignorevalue=False) - format_param(self, l, "qlim") + self._format_param(l, "parent") + self._format_param(l, "isflip", ignorevalue=False) + self._format_param(l, "qlim") if self._hasdynamics: - format_param(self, l, "m") - format_param(self, l, "r") - format_param(self, l, "I", indices=[0, 4, 8, 1, 2, 5]) - format_param(self, l, "Jm") - format_param(self, l, "B") - format_param(self, l, "Tc") - format_param(self, l, "G") + self._format_param(l, "m") + self._format_param(l, "r") + self._format_param(l, "I", indices=[0, 4, 8, 1, 2, 5]) + self._format_param(l, "Jm") + self._format_param(l, "B") + self._format_param(l, "Tc") + self._format_param(l, "G") return l - def islimit(self, q): + def islimit(self, q: float): """ Checks if joint exceeds limit - :param q: joint coordinate - :type q: float - :return: True if joint is exceeded - :rtype: bool - ``link.islimit(q)`` is True if ``q`` exceeds the joint limits defined by ``link``. - .. note:: If no limits are set always return False. + Parameters + ---------- + q + joint coordinate + + Returns + ------- + islimit + True if joint is exceeded + + Notes + ----- + - If no limits are set always return False. + + See Also + -------- + :func:`qlim` - :seealso: :func:`qlim` """ + if self.qlim is None: return False else: return q < self.qlim[0] or q > self.qlim[1] - def nofriction(self, coulomb=True, viscous=False): + def nofriction(self, coulomb: bool = True, viscous: bool = False): """ Clone link without friction - :param coulomb: if True, will set the Coulomb friction to 0 - :type coulomb: bool - :param viscous: if True, will set the viscous friction to 0 - :type viscous: bool - ``link.nofriction()`` is a copy of the link instance with the same parameters except, the Coulomb and/or viscous friction parameters are set to zero. - .. note:: For simulation it can be useful to remove Couloumb friction + Parameters + ---------- + coulomb + if True, will set the Coulomb friction to 0 + viscous + if True, will set the viscous friction to 0 + + Notes + ----- + - For simulation it can be useful to remove Couloumb friction which can cause problems for numerical integration. """ @@ -1267,17 +1491,10 @@ def nofriction(self, coulomb=True, viscous=False): return link - def friction(self, qd, coulomb=True): + def friction(self, qd: float, coulomb: bool = True): r""" Compute joint friction - :param qd: The joint velocity - :type qd: float - :param coulomb: include Coulomb friction - :type coloumb: bool, default True - :return tau: the friction force/torque - :rtype tau: float - ``friction(qd)`` is the joint friction force/torque for joint velocity ``qd``. The friction model includes: @@ -1290,24 +1507,36 @@ def friction(self, qd, coulomb=True): \tau_C^+ & \mbox{if $\dot{q} > 0$} \\ \tau_C^- & \mbox{if $\dot{q} < 0$} \end{array} \right. - .. note:: - - - The friction value should be added to the motor output torque to - determine the nett torque. It has a negative value when qd > 0. - - The returned friction value is referred to the output of the - gearbox. - - The friction parameters in the Link object are referred to the - motor. - - Motor viscous friction is scaled up by :math:`G^2`. - - Motor Coulomb friction is scaled up by math:`G`. - - The appropriate Coulomb friction value to use in the - non-symmetric case depends on the sign of the joint velocity, - not the motor velocity. - - Coulomb friction is zero for zero joint velocity, stiction is - not modeled. - - The absolute value of the gear ratio is used. Negative gear - ratios are tricky: the Puma560 robot has negative gear ratio for - joints 1 and 3. + Parameters + ---------- + qd + The joint velocity + coulomb + include Coulomb friction + + Returns + ------- + tau + the friction force/torque + + Notes + ----- + - The friction value should be added to the motor output torque to + determine the nett torque. It has a negative value when qd > 0. + - The returned friction value is referred to the output of the + gearbox. + - The friction parameters in the Link object are referred to the + motor. + - Motor viscous friction is scaled up by :math:`G^2`. + - Motor Coulomb friction is scaled up by math:`G`. + - The appropriate Coulomb friction value to use in the + non-symmetric case depends on the sign of the joint velocity, + not the motor velocity. + - Coulomb friction is zero for zero joint velocity, stiction is + not modeled. + - The absolute value of the gear ratio is used. Negative gear + ratios are tricky: the Puma560 robot has negative gear ratio for + joints 1 and 3. """ @@ -1329,24 +1558,6 @@ class Link(BaseLink): """ ETS link class - :param ets: kinematic - The elementary transforms which make up the link - :param qlim: joint variable limits [min max] - :type qlim: float ndarray(2) - :param m: dynamic - link mass - :type m: float - :param r: dynamic - position of COM with respect to link frame - :type r: SE3 - :param I: dynamic - inertia of link with respect to COM - :type I: float ndarray(3,3) - :param Jm: dynamic - motor inertia - :type Jm: float - :param B: dynamic - motor viscous friction - :type B: float - :param Tc: dynamic - motor Coulomb friction (1x2 or 2x1) - :type Tc: float ndarray(2) - :param G: dynamic - gear ratio - :type G: float - The Link object holds all information related to a robot link and can form a serial-connected chain or a rigid-body tree. It inherits from the Link class which provides common functionality such @@ -1354,26 +1565,60 @@ class Link(BaseLink): The transform to the next link is given as an ETS with the joint variable, if present, as the last term. This is preprocessed and the object stores: - * ``Ts`` the constant part as a NumPy array, or None - * ``v`` a pointer to an ETS object representing the joint variable. - or None - - :references: - - Kinematic Derivatives using the Elementary Transform Sequence, - J. Haviland and P. Corke + - ``Ts`` the constant part as a NumPy array, or None + - ``v`` a pointer to an ETS object representing the joint variable. + or None + + Parameters + ---------- + ets + kinematic - The elementary transforms which make up the link + jindex + the joint variable index + name + name of the link + parent + a reference to the parent link in the kinematic chain + joint_name + the name of the joint variable + m + dynamic - link mass + r + dynamic - position of COM with respect to link frame + I + dynamic - inertia of link with respect to COM + Jm + dynamic - motor inertia + B + dynamic - motor viscous friction + Tc + dynamic - motor Coulomb friction [Tc⁺, Tc⁻] + G + dynamic - gear ratio + qlim + joint variable limits [min, max] + geometry + the visual geometry which represents the link. This is used + to display the link in Swift + collision + the collision geometry which represents the link in collision + checkers + + See Also + -------- + :class:`Link2` + :class:`DHLink` - :seealso: :class:`Link`, :class:`DHLink` """ def __init__( self, ets: Union[ETS, ET] = ETS(), jindex: Union[None, int] = None, **kwargs ): - # process common options super().__init__(ets=ets, **kwargs) # check we have an ETS - if not isinstance(self._ets, ETS): + if not isinstance(self._ets, ETS): # pragma: nocover raise TypeError("The ets argument must be of type ETS") # Set the jindex @@ -1382,146 +1627,80 @@ def __init__( self._ets[-1].jindex = jindex self._ets._auto_jindex = False - # @property - # def ets(self: "Link") -> "ETS": - # """ - # Link transform in ETS form - - # :return: elementary transform sequence for link transform - # :rtype: ETS or ETS2 instance - - # The sequence: - # - has at least one element - # - may include zero or more constant transforms - # - no more than one variable transform, which if present will - # be last in the sequence - # """ - # return self._ets # type: ignore - - # @ets.setter - # def ets(self, new_ets: ETS): - # if new_ets.n > 1: - # raise ValueError("An elementary link can only have one joint variable") - - # if new_ets.n == 1 and not new_ets[-1].isjoint: - # raise ValueError("Variable link must be at the end of the ETS") - - # self._ets = new_ets - - # if self._ets.n: - # self._v = self._ets[-1] - # self._isjoint = True - # else: - # self._v = None - # self._isjoint = False - def A(self, q: float = 0.0) -> SE3: """ Link transform matrix - :param q: Joint coordinate (radians or metres). Not required for links + + ``link.A(q)`` is an SE(3) matrix that describes the rigid-body + transformation from the previous to the current link frame to + the next, which depends on the joint coordinate ``q``. + + Parameters + ---------- + q + Joint coordinate (radians or metres). Not required for links with no variable - :return T: link frame transformation matrix + Returns + ------- + T + link frame transformation matrix - ``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body - transformation from the previous to the current link frame to - the next, which depends on the joint coordinate ``q``. """ if self.isjoint: - return SE3(self._Ts @ self._ets[-1].A(q), check=False) - else: + if self._Ts is not None: + return SE3(self._Ts @ self._ets[-1].A(q), check=False) + else: + return SE3(self._ets[-1].A(q), check=False) + + elif self._Ts is not None: return SE3(self._Ts, check=False) + else: + return SE3() class Link2(BaseLink): def __init__(self, ets: ETS2 = ETS2(), jindex: Union[int, None] = None, **kwargs): - # process common options super().__init__(ets=ets, **kwargs) # check we have an ETS - if not isinstance(self._ets, ETS2): + if not isinstance(self._ets, ETS2): # pragma: nocover raise TypeError("The self._ets argument must be of type ETS2") # Set the jindex if len(self._ets) > 0 and self._ets[-1].isjoint: if jindex is not None: - self._ets[-1].jindex = jindex - - # @property - # def ets(self) -> "ETS2": - # """ - # Link transform in ETS form - - # :return: elementary transform sequence for link transform - # :rtype: ETS or ETS2 instance - - # The sequence: - # - has at least one element - # - may include zero or more constant transforms - # - no more than one variable transform, which if present will - # be last in the sequence - # """ - # return self._ets - - # @ets.setter - # def ets(self, new_ets: ETS2): - # if new_ets.n > 1: - # raise ValueError("An elementary link can only have one joint variable") - - # if new_ets.n == 1 and not new_ets[-1].isjoint: - # raise ValueError("Variable link must be at the end of the ETS") - - # self._ets = new_ets - - # if self._ets.n: - # self._v = self._ets[-1] - # self._isjoint = True - # else: - # self._v = None - # self._isjoint = False + self._ets[-1].jindex = jindex # pragma: nocover def A(self, q: float = 0.0) -> SE2: """ Link transform matrix - :param q: Joint coordinate (radians or metres). Not required for links + ``link.A(q)`` is an SE(2) matrix that describes the rigid-body + transformation from the previous to the current link frame to + the next, which depends on the joint coordinate ``q``. + + Parameters + ---------- + q + Joint coordinate (radians or metres). Not required for links with no variable - :return T: link frame transformation matrix + Returns + ------- + T + link frame transformation matrix - ``LINK.A(q)`` is an SE(3) matrix that describes the rigid-body - transformation from the previous to the current link frame to - the next, which depends on the joint coordinate ``q``. """ if self.isjoint: - return SE2(self._Ts @ self._ets[-1].A(q), check=False) - else: - return SE2(self._Ts, check=False) - - -if __name__ == "__main__": # pragma nocover - - import sympy - from roboticstoolbox import * - - a1, a2, r1, r2, m1, m2, g = sympy.symbols("a1 a2 r1 r2 m1 m2 g") - link1 = Link(ET.Ry(flip=True), m=m1, r=[r1, 0, 0], name="link0") - link2 = Link(ET.tx(a1) * ET.Ry(flip=True), m=m2, r=[r2, 0, 0], name="link1") - print(link1) - robot = ERobot([link1, link2]) - print(robot) - robot.dynamics() - - # a = Link(name='bob') - # print(a) - # print(repr(a)) - - # a = Link(name='bob', parent='foo') - # print(a) - # print(repr(a)) + if self._Ts is not None: + return SE2(self._Ts @ self._ets[-1].A(q), check=False) + else: + return SE2(self._ets[-1].A(q), check=False) - # a = rtb.models.URDF.UR5() - # print(a[3]) - # print(repr(a[3])) + elif self._Ts is not None: + return SE2(self._Ts, check=False) + else: + return SE2() diff --git a/roboticstoolbox/robot/PoERobot.py b/roboticstoolbox/robot/PoERobot.py index dd552a196..5c0edaf46 100644 --- a/roboticstoolbox/robot/PoERobot.py +++ b/roboticstoolbox/robot/PoERobot.py @@ -33,20 +33,8 @@ def __str__(self): s += "]" return s - def _repr_pretty_(self, p, cycle): - """ - Pretty string for IPython (superclass method) - - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) - - """ - # see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html - - p.text(f"{i}:\n{str(x)}") class PoERevolute(PoELink): - def __init__(self, axis, point, **kwargs): """ Construct revolute product of exponential link @@ -62,9 +50,9 @@ def __init__(self, axis, point, **kwargs): """ super().__init__(Twist3.UnitRevolute(axis, point), **kwargs) -class PoEPrismatic(PoELink): +class PoEPrismatic(PoELink): def __init__(self, axis, **kwargs): """ Construct prismatic product of exponential link @@ -77,8 +65,9 @@ def __init__(self, axis, **kwargs): :seealso: :class:`Link` :class:`Robot` """ super().__init__(Twist3.UnitPrismatic(axis), **kwargs) -class PoERobot(Robot): + +class PoERobot(Robot): def __init__(self, links, T0, **kwargs): """ Product of exponential robot class @@ -99,7 +88,6 @@ def __init__(self, links, T0, **kwargs): super().__init__(links, **kwargs) self.T0 = T0 - def __str__(self): """ Pretty prints the PoE Model of the robot. @@ -114,27 +102,15 @@ def __str__(self): return s def __repr__(self): - s = "PoERobot([\n " - for j, link in enumerate(self): - s += repr(link) + "," - s += "],\n" + s = "PoERobot([\n" + s += "\n".join([" " + repr(link) + "," for link in self]) + s += "\n ],\n" s += f" T0=SE3({np.array_repr(self.T0.A)}),\n" - s += f" name={self.name},\n" + s += f" name=\"{self.name}\",\n" s += ")" return s - def _repr_pretty_(self, p, cycle): - """ - Pretty string for IPython (superclass method) - :param p: pretty printer handle (ignored) - :param cycle: pretty printer flag (ignored) - - """ - # see https://ipython.org/ipython-doc/stable/api/generated/IPython.lib.pretty.html - - p.text(f"{i}:\n{str(x)}") - def nbranches(self): return 0 @@ -204,12 +180,13 @@ def jacobe(self, q): def ets(self): return NotImplemented + if __name__ == "__main__": # pragma nocover T0 = SE3.Trans(2, 0, 0) # rotate about z-axis, through (0,0,0) - link1 = PoERevolute([0, 0, 1], [0, 0, 0], name='foo') + link1 = PoERevolute([0, 0, 1], [0, 0, 0], name="foo") # rotate about z-axis, through (1,0,0) link2 = PoERevolute([0, 0, 1], [1, 0, 0]) # end-effector pose when q=[0,0] @@ -220,7 +197,7 @@ def ets(self): robot = PoERobot([link1, link2], T0) - q = [0, np.pi/2] + q = [0, np.pi / 2] # q = [0, 0] # robot.fkine(q).printline() diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index 0ec105779..eadab10d4 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -1,826 +1,782 @@ +#!/usr/bin/env python + +""" +@author: Jesse Haviland +@author: Peter Corke +""" + # import sys -from abc import ABC, abstractmethod, abstractproperty +from os.path import splitext from copy import deepcopy +from warnings import warn +from pathlib import PurePosixPath, Path +from typing import ( + List, + TypeVar, + Union, + Dict, + Tuple, + overload, +) +from typing_extensions import Literal as L + + import numpy as np -import roboticstoolbox as rtb -from spatialmath import SE3 + +import spatialmath.base as smb from spatialmath.base.argcheck import ( - isvector, getvector, getmatrix, - getunit, verifymatrix, ) -import spatialmath.base as smb -from ansitable import ANSITable, Column -from roboticstoolbox.backends.PyPlot import PyPlot -from roboticstoolbox.backends.PyPlot.EllipsePlot import EllipsePlot -from roboticstoolbox.robot.Dynamics import DynamicsMixin -from roboticstoolbox.robot.ETS import ETS -from typing import Union, Dict, Tuple -from spatialgeometry import Shape -from roboticstoolbox.fknm import Robot_link_T -from functools import lru_cache -from spatialgeometry import SceneNode -from roboticstoolbox.robot.Link import BaseLink, Link - -# from numpy import all, eye, isin -from roboticstoolbox.robot.Gripper import Gripper -from numpy import ndarray -from warnings import warn -# import scipy as sp +from spatialgeometry import Shape, CollisionShape, Cylinder -try: - from matplotlib import colors - from matplotlib import cm - - _mpl = True -except ImportError: # pragma nocover - pass +from spatialmath import ( + SE3, + SE2, + SpatialAcceleration, + SpatialVelocity, + SpatialInertia, + SpatialForce, +) -_default_backend = None +import roboticstoolbox as rtb +from roboticstoolbox.robot.BaseRobot import BaseRobot +from roboticstoolbox.robot.RobotKinematics import RobotKinematicsMixin +from roboticstoolbox.robot.Gripper import Gripper +from roboticstoolbox.robot.Link import BaseLink, Link, Link2 +from roboticstoolbox.robot.ETS import ETS, ETS2 +from roboticstoolbox.tools import xacro +from roboticstoolbox.tools import URDF +from roboticstoolbox.tools.types import ArrayLike, NDArray +from roboticstoolbox.tools.data import rtb_path_to_datafile -ArrayLike = Union[list, np.ndarray, tuple, set] +# A generic type variable representing any subclass of BaseLink +LinkType = TypeVar("LinkType", bound=BaseLink) -# TODO maybe this needs to be abstract -# ikine functions need: fkine, jacobe, qlim methods from subclass +# ==================================================================================== # +# ================= Robot Class ====================================================== # +# ==================================================================================== # -class Robot(SceneNode, ABC, DynamicsMixin): +class Robot(BaseRobot[Link], RobotKinematicsMixin): _color = True def __init__( self, - links, - name="noname", - manufacturer="", - comment="", - base=SE3(), - tool=SE3(), - gravity=None, - keywords=(), - symbolic=False, - configs=None, + arg: Union[List[Link], ETS, "Robot"], + gripper_links: Union[Link, List[Link], None] = None, + name: str = "", + manufacturer: str = "", + comment: str = "", + base: Union[NDArray, SE3, None] = None, + tool: Union[NDArray, SE3, None] = None, + gravity: ArrayLike = [0, 0, -9.81], + keywords: Union[List[str], Tuple[str]] = [], + symbolic: bool = False, + configs: Union[Dict[str, NDArray], None] = None, + check_jindex: bool = True, + urdf_string: Union[str, None] = None, + urdf_filepath: Union[Path, PurePosixPath, None] = None, ): + # Process links + if isinstance(arg, Robot): + # We're passed a Robot, clone it + # We need to preserve the parent link as we copy - self.name = name - self.manufacturer = manufacturer - self.comment = comment - self.symbolic = symbolic - self._reach = None - - # Initialise the scene node - SceneNode.__init__(self) - - self._T = base.A - self.tool = tool - - if keywords is not None and not isinstance(keywords, (tuple, list)): - raise TypeError("keywords must be a list or tuple") - else: - self.keywords = keywords - - # gravity is in the negative-z direction. This is the negative of the - # MATLAB Toolbox case (which was wrong). - if gravity is None: - gravity = np.array([0, 0, -9.81]) - self.gravity = gravity - - # validate the links, must be a list of Link subclass objects - if not isinstance(links, list): - raise TypeError("The links must be stored in a list.") - - self._hasdynamics = False - self._hasgeometry = False - self._hascollision = False - - for link in links: - if not isinstance(link, BaseLink): - raise TypeError("links should all be Link subclass") - - # add link back to roboto - link._robot = self - - if link.hasdynamics: - self._hasdynamics = True - if link.geometry: - self._hasgeometry = [] - if link.collision: - self._hascollision = True - - if isinstance(link, Link): - if len(link.geometry) > 0: - self._hasgeometry = True - - self._links = links - self._nlinks = len(links) - - # Current joint angles of the robot - self.q = np.zeros(self.n) - self.qd = np.zeros(self.n) - self.qdd = np.zeros(self.n) - - self.control_mode = "v" - - self._dynchanged = False + # Copy each link within the robot + links = [deepcopy(link) for link in arg.links] + gripper_links = [] - # Set up named configuration property - if configs is None: - configs = dict() - self._configs = configs - - def copy(self): - return deepcopy(self) - - def __deepcopy__(self, memo): - - links = [] - - for link in self.links: - links.append(deepcopy(link)) - - name = deepcopy(self.name) - manufacturer = deepcopy(self.manufacturer) - comment = deepcopy(self.comment) - base = deepcopy(self.base) - tool = deepcopy(self.tool) - gravity = deepcopy(self.gravity) - keywords = deepcopy(self.keywords) - symbolic = deepcopy(self.symbolic) - configs = deepcopy(self.configs) - - cls = self.__class__ - result = cls( - links=links, - name=name, - manufacturer=manufacturer, - comment=comment, - base=base, - tool=tool, - gravity=gravity, - keywords=keywords, - symbolic=symbolic, - configs=configs, - ) + for gripper in arg.grippers: + glinks = [] + for link in gripper.links: + glinks.append(deepcopy(link)) - # if a configuration was an attribute of original robot, make it an - # attribute of the deep copy - for config in configs: - if hasattr(self, config): - setattr(result, config, configs[config]) + gripper_links.append(glinks[0]) + links = links + glinks - memo[id(self)] = result - return result + # Sever parent connection, but save the string + # The constructor will piece this together for us + for link in links: + link._children = [] + if link.parent is not None: + link._parent_name = link.parent.name + link._parent = None - def __repr__(self): - return str(self) + super().__init__(links, gripper_links=gripper_links) - def __iter__(self): - self._iter = 0 - return self + for i, gripper in enumerate(self.grippers): + gripper.tool = arg.grippers[i].tool.copy() - def __next__(self): - if self._iter < len(self.links): - link = self[self._iter] - self._iter += 1 - return link + self._urdf_string = arg.urdf_string + self._urdf_filepath = arg.urdf_filepath else: - raise StopIteration + if isinstance(arg, ETS): + # We're passed an ETS string + links = [] + # chop it up into segments, a link frame after every joint + parent = None + for j, ets_j in enumerate(arg.split()): + elink = Link(ETS(ets_j), parent=parent, name=f"link{j:d}") + if ( + elink.qlim is None + and elink.v is not None + and elink.v.qlim is not None + ): + elink.qlim = elink.v.qlim # pragma nocover + parent = elink + links.append(elink) + + elif smb.islistof(arg, Link): + links = arg - def __getitem__(self, i): - """ - Get link (Robot superclass) - - :param i: link number or name - :type i: int or str - :return: i'th link or named link - :rtype: Link subclass - - This also supports iterating over each link in the robot object, - from the base to the tool. - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> print(robot[1]) # print the 2nd link - >>> print([link.a for link in robot]) # print all the a_j values + else: + raise TypeError("arg was invalid, must be List[Link], ETS, or Robot") + + # Initialise Base Robot object + super().__init__( + links=links, + gripper_links=gripper_links, + name=name, + manufacturer=manufacturer, + comment=comment, + base=base, + tool=tool, + gravity=gravity, + keywords=keywords, + symbolic=symbolic, + configs=configs, + check_jindex=check_jindex, + ) - .. note:: ``ERobot`` supports link lookup by name, - eg. ``robot['link1']`` - """ - return self._links[i] - - # URDF Parser Attempt - # @staticmethod - # def _get_stl_file_paths_and_scales(urdf_path): - # data = [[], [], []] # [ [filenames] , [scales] , [origins] ] - # - # name, ext = splitext(urdf_path) - # - # if ext == '.xacro': - # urdf_string = xacro.main(urdf_path) - # urdf = URDF.loadstr(urdf_string, urdf_path) - # - # for link in urdf.links: - # data[0].append(link.visuals[0].geometry.mesh.filename) - # data[1].append(link.visuals[0].geometry.mesh.scale) - # data[2].append(SE3(link.visuals[0].origin)) - # - # return data - - def dynchanged(self, what=None): - """ - Dynamic parameters have changed (Robot superclass) + # --------------------------------------------------------------------- # + # --------- Swift Methods --------------------------------------------- # + # --------------------------------------------------------------------- # - Called from a property setter to inform the robot that the cache of - dynamic parameters is invalid. + def _to_dict(self, robot_alpha=1.0, collision_alpha=0.0): + ob = [] - :seealso: :func:`roboticstoolbox.Link._listen_dyn` - """ - self._dynchanged = True - if what != "gravity": - self._hasdynamics = True + for link in self.links: + if robot_alpha > 0: + for gi in link.geometry: + gi.set_alpha(robot_alpha) + ob.append(gi.to_dict()) + if collision_alpha > 0: + for gi in link.collision: + gi.set_alpha(collision_alpha) + ob.append(gi.to_dict()) + + # Do the grippers now + for gripper in self.grippers: + for link in gripper.links: + if robot_alpha > 0: + for gi in link.geometry: + gi.set_alpha(robot_alpha) + ob.append(gi.to_dict()) + if collision_alpha > 0: + for gi in link.collision: + gi.set_alpha(collision_alpha) + ob.append(gi.to_dict()) + + # for o in ob: + # print(o) + + return ob + + def _fk_dict(self, robot_alpha=1.0, collision_alpha=0.0): + ob = [] + + # Do the robot + for link in self.links: + if robot_alpha > 0: + for gi in link.geometry: + ob.append(gi.fk_dict()) + if collision_alpha > 0: + for gi in link.collision: + ob.append(gi.fk_dict()) + + # Do the grippers now + for gripper in self.grippers: + for link in gripper.links: + if robot_alpha > 0: + for gi in link.geometry: + ob.append(gi.fk_dict()) + if collision_alpha > 0: + for gi in link.collision: + ob.append(gi.fk_dict()) + + return ob - def _getq(self, q=None): - """ - Get joint coordinates (Robot superclass) + # --------------------------------------------------------------------- # + # --------- URDF Methods ---------------------------------------------- # + # --------------------------------------------------------------------- # - :param q: passed value, defaults to None - :type q: array_like, optional - :return: passed or value from robot state - :rtype: ndarray(n,) - """ - if q is None: - return self.q - elif isvector(q, self.n): - return getvector(q, self.n) + @staticmethod + def URDF_read( + file_path, tld=None, xacro_tld=None + ) -> Tuple[List[Link], str, str, Union[Path, PurePosixPath]]: + """ + Read a URDF file as Links + + File should be specified relative to ``RTBDATA/URDF/xacro`` + + Parameters + ---------- + file_path + File path relative to the xacro folder + tld + A custom top-level directory which holds the xacro data, + defaults to None + xacro_tld + A custom top-level within the xacro data, + defaults to None + + Returns + ------- + links + a list of links + name + the name of the robot + urdf + a string representing the URDF + file_path + a path to the original file + + Notes + ----- + If ``tld`` is not supplied, filepath pointing to xacro data should + be directly under ``RTBDATA/URDF/xacro`` OR under ``./xacro`` relative + to the model file calling this method. If ``tld`` is supplied, then + ```file_path``` needs to be relative to ``tld`` + + """ + + # Get the path to the class that defines the robot + if tld is None: + base_path = rtb_path_to_datafile("xacro") else: - return getmatrix(q, (None, self.n)) - - @property - def n(self): - """ - Number of joints (Robot superclass) - - :return: Number of joints - :rtype: int - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.n - - :seealso: :func:`nlinks`, :func:`nbranches` - """ - return self._n - - @property - def nlinks(self): - """ - Number of links (Robot superclass) - - :return: Number of links - :rtype: int - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.nlinks - - :seealso: :func:`n`, :func:`nbranches` - """ - return self._nlinks - - @property - def configs(self) -> Dict[str, np.ndarray]: - return self._configs - - @abstractproperty - def nbranches(self): - """ - Number of branches (Robot superclass) - - :return: Number of branches - :rtype: int - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.nbranches - - :seealso: :func:`n`, :func:`nlinks` - """ - return self._n - - @property - def hasdynamics(self): - """ - Robot has dynamic parameters (Robot superclass) - - :return: Robot has dynamic parameters - :rtype: bool - - At least one link has associated dynamic parameters. - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.hasdynamics: - """ - return self._hasdynamics - - @property - def hasgeometry(self): - """ - Robot has geometry model (Robot superclass) - - :return: Robot has geometry model - :rtype: bool - - At least one link has associated mesh to describe its shape. - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.hasgeometry - - :seealso: :func:`hascollision` - """ - return self._hasgeometry - - @property - def hascollision(self): - """ - Robot has collision model (Robot superclass) - - :return: Robot has collision model - :rtype: bool - - At least one link has associated collision model. - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.hascollision - - :seealso: :func:`hasgeometry` - """ - return self._hascollision - - @property - def qrandom(self): - """ - Return a random joint configuration - - :return: Random joint configuration :rtype: ndarray(n) - - The value for each joint is uniform randomly distributed between the - limits set for the robot. - - .. note:: The joint limit for all joints must be set. - - :seealso: :func:`Robot.qlim`, :func:`Link.qlim` - """ - qlim = self.qlim - if np.any(np.isnan(qlim)): - raise ValueError("some joint limits not defined") - return np.random.uniform(low=qlim[0, :], high=qlim[1, :], size=(self.n,)) - - @property - def default_backend(self): - """ - Get default graphical backend - - :return: backend name - :rtype: str - - Get the default graphical backend, used when no explicit backend is - passed to ``Robot.plot``. - """ - return _default_backend - - @default_backend.setter - def default_backend(self, be): - """ - Set default graphical backend - - :param be: backend name - :type be: str - - Set the default graphical backend, used when no explicit backend is - passed to ``Robot.plot``. The default set here will be overridden if - the particular ``Robot`` subclass cannot support it. - """ - _default_backend = be - - def addconfiguration_attr(self, name: str, q: ArrayLike, unit: str = "rad"): - """ - Add a named joint configuration as an attribute (Robot superclass) - - :param name: Name of the joint configuration - :param q: Joint configuration - :type q: Arraylike - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.addconfiguration_attr("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) - >>> robot.mypos - >>> robot.configs["mypos"] - - .. note:: - - Used in robot model init method to store the ``qr`` configuration - - Dynamically adding attributes to objects can cause issues with - Python type checking. - - Configuration is also added to the robot instance's dictionary of - named configurations. - - :seealso: :meth:`addconfiguration` - """ - v = getvector(q, self.n) - v = getunit(v, unit) - v = np.array(v) - self._configs[name] = v - setattr(self, name, v) - - def addconfiguration(self, name: str, q: np.ndarray): - """ - Add a named joint configuration (Robot superclass) - - :param name: Name of the joint configuration - :type name: str - :param q: Joint configuration - :type q: ndarray(n) or list - - Add a named configuration to the robot instance's dictionary of named - configurations. - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.addconfiguration_attr("mypos", [0.1, 0.2, 0.3, 0.4, 0.5, 0.6]) - >>> robot.configs["mypos"] - - :seealso: :meth:`addconfiguration` - """ - self._configs[name] = q - - def configurations_str(self, border="thin"): - deg = 180 / np.pi - - # TODO: factor this out of DHRobot - def angle(theta, fmt=None): - - if fmt is not None: - try: - return fmt.format(theta * deg) + "\u00b0" - except TypeError: - pass - - # pragma nocover - return str(theta * deg) + "\u00b0" - - # show named configurations - if len(self._configs) > 0: - table = ANSITable( - Column("name", colalign=">"), - *[ - Column(f"q{j:d}", colalign="<", headalign="<") - for j in range(self.n) - ], - border=border, - ) - - for name, q in self._configs.items(): - qlist = [] - for j, c in enumerate(self.structure): - if c == "P": - qlist.append(f"{q[j]: .3g}") - else: - qlist.append(angle(q[j], "{: .3g}")) - table.row(name, *qlist) - - return "\n" + str(table) + base_path = PurePosixPath(tld) + + # Add on relative path to get to the URDF or xacro file + # base_path = PurePath(classpath).parent.parent / 'URDF' / 'xacro' + file_path = base_path / PurePosixPath(file_path) + _, ext = splitext(file_path) + + if ext == ".xacro": + # it's a xacro file, preprocess it + if xacro_tld is not None: + xacro_tld = base_path / PurePosixPath(xacro_tld) + urdf_string = xacro.main(file_path, xacro_tld) + try: + urdf = URDF.loadstr(urdf_string, file_path, base_path) + except BaseException as e: # pragma nocover + print("error parsing URDF file", file_path) + raise e else: # pragma nocover - return "" - - @property - def structure(self): - """ - Return the joint structure string - - :return: joint configuration string - :rtype: str - - A string with one letter per joint: ``R`` for a revolute - joint, and ``P`` for a prismatic joint. - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.structure - >>> stanford = rtb.models.DH.Stanford() - >>> stanford.structure - - .. note:: Fixed joints, that maintain a constant link relative pose, - are not included. ``len(self.structure) == self.n``. - """ - structure = [] - for link in self: - if link.isrevolute: - structure.append("R") - elif link.isprismatic: - structure.append("P") - - return "".join(structure) - - @property - def revolutejoints(self): - """ - Revolute joints as bool array - - :return: array of joint type, True if revolute - :rtype: bool(n) - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.revolutejoints() - >>> stanford = rtb.models.DH.Stanford() - >>> stanford.revolutejoints() - - .. note:: Fixed joints, that maintain a constant link relative pose, - are not included. ``len(self.structure) == self.n``. - - :seealso: :func:`Link.isrevolute`, :func:`prismaticjoints` - """ - return [link.isrevolute for link in self if link.isjoint] - - # TODO not very efficient - # TODO keep a mapping from joint to link - def isrevolute(self, j): - return self.revolutejoints[j] - - def isprismatic(self, j): - return self.prismaticjoints[j] - - @property - def prismaticjoints(self): - """ - Revolute joints as bool array - - :return: array of joint type, True if prismatic - :rtype: bool(n) - - Example: - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.DH.Puma560() - >>> puma.prismaticjoints() - >>> stanford = rtb.models.DH.Stanford() - >>> stanford.prismaticjoints() + urdf_string = open(file_path).read() + urdf = URDF.loadstr(urdf_string, file_path, base_path) - .. note:: Fixed joints, that maintain a constant link relative pose, - are not included. ``len(self.structure) == self.n``. + if not isinstance(urdf_string, str): # pragma nocover + raise ValueError("Parsing failed, did not get valid URDF string back") - :seealso: :func:`Link.isprismatic`, :func:`revolutejoints` - """ - return [link.isprismatic for link in self if link.isjoint] - - def todegrees(self, q): - """ - Convert joint angles to degrees - - :param q: The joint configuration of the robot - :type q: ndarray(n) or ndarray(m,n) - :return: a vector of joint coordinates in degrees and metres - :rtype: ndarray(n) or ndarray(m,n) - - ``robot.todegrees(q)`` converts joint coordinates ``q`` to degrees - taking into account whether elements of ``q`` correspond to revolute - or prismatic joints, ie. prismatic joint values are not converted. - - If ``q`` is a matrix, with one column per joint, the conversion is - performed columnwise. - - Example: - - .. runblock:: pycon + return urdf.elinks, urdf.name, urdf_string, file_path - >>> import roboticstoolbox as rtb - >>> from math import pi - >>> stanford = rtb.models.DH.Stanford() - >>> stanford.todegrees([pi/4, pi/8, 2, -pi/4, pi/6, pi/3]) + @classmethod + def URDF(cls, file_path: str, gripper: Union[int, str, None] = None): """ + Construct a Robot object from URDF file - q = getmatrix(q, (None, self.n)) + Parameters + ---------- + file_path + the path to the URDF + gripper + index or name of the gripper link(s) - for j, revolute in enumerate(self.revolutejoints): - if revolute: - q[:, j] *= 180.0 / np.pi - if q.shape[0] == 1: - return q[0] - else: - return q + Returns + ------- + If ``gripper`` is specified, links from that link outward are removed + from the rigid-body tree and folded into a ``Gripper`` object. - def toradians(self, q): """ - Convert joint angles to radians - :param q: The joint configuration of the robot - :type q: ndarray(n) or ndarray(m,n) - :return: a vector of joint coordinates in radians and metres - :rtype: ndarray(n) or ndarray(m,n) + links, name, urdf_string, urdf_filepath = Robot.URDF_read(file_path) - ``robot.toradians(q)`` converts joint coordinates ``q`` to radians - taking into account whether elements of ``q`` correspond to revolute - or prismatic joints, ie. prismatic joint values are not converted. + gripperLink: Union[Link, None] = None - If ``q`` is a matrix, with one column per joint, the conversion is - performed columnwise. + if gripper is not None: + if isinstance(gripper, int): + gripperLink = links[gripper] + elif isinstance(gripper, str): + for link in links: + if link.name == gripper: + gripperLink = link + break + else: # pragma nocover + raise ValueError(f"no link named {gripper}") + else: # pragma nocover + raise TypeError("bad argument passed as gripper") - Example: + # links, name, urdf_string, urdf_filepath = Robot.URDF_read(file_path) - .. runblock:: pycon + return cls( + links, + name=name, + gripper_links=gripperLink, + urdf_string=urdf_string, + urdf_filepath=urdf_filepath, + ) - >>> import roboticstoolbox as rtb - >>> stanford = rtb.models.DH.Stanford() - >>> stanford.toradians([10, 20, 2, 30, 40, 50]) - """ + # # --------------------------------------------------------------------- # + # # --------- Utility Methods ------------------------------------------- # + # # --------------------------------------------------------------------- # + + # def showgraph(self, display_graph: bool = True, **kwargs) -> Union[None, str]: + # """ + # Display a link transform graph in browser + + # ``robot.showgraph()`` displays a graph of the robot's link frames + # and the ETS between them. It uses GraphViz dot. + + # The nodes are: + # - Base is shown as a grey square. This is the world frame origin, + # but can be changed using the ``base`` attribute of the robot. + # - Link frames are indicated by circles + # - ETS transforms are indicated by rounded boxes + + # The edges are: + # - an arrow if `jtype` is False or the joint is fixed + # - an arrow with a round head if `jtype` is True and the joint is + # revolute + # - an arrow with a box head if `jtype` is True and the joint is + # prismatic + + # Edge labels or nodes in blue have a fixed transformation to the + # preceding link. + + # Parameters + # ---------- + # display_graph + # Open the graph in a browser if True. Otherwise will return the + # file path + # etsbox + # Put the link ETS in a box, otherwise an edge label + # jtype + # Arrowhead to node indicates revolute or prismatic type + # static + # Show static joints in blue and bold + + # Examples + # -------- + # >>> import roboticstoolbox as rtb + # >>> panda = rtb.models.URDF.Panda() + # >>> panda.showgraph() + + # .. image:: ../figs/panda-graph.svg + # :width: 600 + + # See Also + # -------- + # :func:`dotfile` + + # """ + + # # Lazy import + # import tempfile + # import subprocess + # import webbrowser + + # # create the temporary dotfile + # dotfile = tempfile.TemporaryFile(mode="w") + # self.dotfile(dotfile, **kwargs) + + # # rewind the dot file, create PDF file in the filesystem, run dot + # dotfile.seek(0) + # pdffile = tempfile.NamedTemporaryFile(suffix=".pdf", delete=False) + # subprocess.run("dot -Tpdf", shell=True, stdin=dotfile, stdout=pdffile) + + # # open the PDF file in browser (hopefully portable), then cleanup + # if display_graph: # pragma nocover + # webbrowser.open(f"file://{pdffile.name}") + # else: + # return pdffile.name + + # def dotfile( + # self, + # filename: Union[str, IO[str]], + # etsbox: bool = False, + # ets: L["full", "brief"] = "full", + # jtype: bool = False, + # static: bool = True, + # ): + # """ + # Write a link transform graph as a GraphViz dot file + + # The file can be processed using dot: + # % dot -Tpng -o out.png dotfile.dot + + # The nodes are: + # - Base is shown as a grey square. This is the world frame origin, + # but can be changed using the ``base`` attribute of the robot. + # - Link frames are indicated by circles + # - ETS transforms are indicated by rounded boxes + + # The edges are: + # - an arrow if `jtype` is False or the joint is fixed + # - an arrow with a round head if `jtype` is True and the joint is + # revolute + # - an arrow with a box head if `jtype` is True and the joint is + # prismatic + + # Edge labels or nodes in blue have a fixed transformation to the + # preceding link. + + # Note + # ---- + # If ``filename`` is a file object then the file will *not* + # be closed after the GraphViz model is written. + + # Parameters + # ---------- + # file + # Name of file to write to + # etsbox + # Put the link ETS in a box, otherwise an edge label + # ets + # Display the full ets with "full" or a brief version with "brief" + # jtype + # Arrowhead to node indicates revolute or prismatic type + # static + # Show static joints in blue and bold + + # See Also + # -------- + # :func:`showgraph` + + # """ + + # if isinstance(filename, str): + # file = open(filename, "w") + # else: + # file = filename + + # header = r"""digraph G { + # graph [rankdir=LR]; + # """ + + # def draw_edge(link, etsbox, jtype, static): + # # draw the edge + # if jtype: + # if link.isprismatic: + # edge_options = 'arrowhead="box", arrowtail="inv", dir="both"' + # elif link.isrevolute: + # edge_options = 'arrowhead="dot", arrowtail="inv", dir="both"' + # else: + # edge_options = 'arrowhead="normal"' + # else: + # edge_options = 'arrowhead="normal"' + + # if link.parent is None: + # parent = "BASE" + # else: + # parent = link.parent.name + + # if etsbox: + # # put the ets fragment in a box + # if not link.isjoint and static: + # node_options = ', fontcolor="blue"' + # else: + # node_options = "" + + # try: + # file.write( + # ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format( + # link.name, + # link.ets.__str__(q=f"q{link.jindex}"), + # node_options, + # ) + # ) + # except UnicodeEncodeError: # pragma nocover + # file.write( + # ' {}_ets [shape=box, style=rounded, label="{}"{}];\n'.format( + # link.name, + # link.ets.__str__(q=f"q{link.jindex}") + # .encode("ascii", "ignore") + # .decode("ascii"), + # node_options, + # ) + # ) + + # file.write(" {} -> {}_ets;\n".format(parent, link.name)) + # file.write( + # " {}_ets -> {} [{}];\n".format(link.name, link.name, edge_options) + # ) + # else: + # # put the ets fragment as an edge label + # if not link.isjoint and static: + # edge_options += 'fontcolor="blue"' + # if ets == "full": + # estr = link.ets.__str__(q=f"q{link.jindex}") + # elif ets == "brief": + # if link.jindex is None: + # estr = "" + # else: + # estr = f"...q{link.jindex}" + # else: + # return + # try: + # file.write( + # ' {} -> {} [label="{}", {}];\n'.format( + # parent, + # link.name, + # estr, + # edge_options, + # ) + # ) + # except UnicodeEncodeError: # pragma nocover + # file.write( + # ' {} -> {} [label="{}", {}];\n'.format( + # parent, + # link.name, + # estr.encode("ascii", "ignore").decode("ascii"), + # edge_options, + # ) + # ) + + # file.write(header) + + # # add the base link + # file.write(" BASE [shape=square, style=filled, fillcolor=gray]\n") + + # # add the links + # for link in self: + # # draw the link frame node (circle) or ee node (doublecircle) + # if link in self.ee_links: + # # end-effector + # node_options = 'shape="doublecircle", color="blue", fontcolor="blue"' + # else: + # node_options = 'shape="circle"' + + # file.write(" {} [{}];\n".format(link.name, node_options)) + + # draw_edge(link, etsbox, jtype, static) + + # for gripper in self.grippers: + # for link in gripper.links: + # file.write(" {} [shape=cds];\n".format(link.name)) + # draw_edge(link, etsbox, jtype, static) + + # file.write("}\n") + + # if isinstance(filename, str): + # file.close() # noqa - q = getmatrix(q, (None, self.n)) + # --------------------------------------------------------------------- # + # --------- Kinematic Methods ----------------------------------------- # + # --------------------------------------------------------------------- # - for j, revolute in enumerate(self.revolutejoints): - if revolute: - q[:, j] *= np.pi / 180.0 - if q.shape[0] == 1: - return q[0] - else: - return q + @property + def reach(self) -> float: + r""" + Reach of the robot + + A conservative estimate of the reach of the robot. It is computed as + the sum of the translational ETs that define the link transform. + + Note + ---- + Computed on the first access. If kinematic parameters + subsequently change this will not be reflected. + + Returns + ------- + reach + Maximum reach of the robot + + Notes + ----- + - Probably an overestimate of reach + - Used by numerical inverse kinematics to scale translational + error. + - For a prismatic joint, uses ``qlim`` if it is set + + """ + + # TODO + # This should be a start, end method and compute the reach based on the + # given ets. Then use an lru_cache to speed up return + + if self._reach is None: + d_all = [] + for link in self.ee_links: + d = 0 + while True: + for et in link.ets: + if et.istranslation: + if et.isjoint: + # the length of a prismatic joint depends on the + # joint limits. They might be set in the ET + # or in the Link depending on how the robot + # was constructed + if link.qlim is not None: + d += max(link.qlim) + elif et.qlim is not None: # pragma nocover + d += max(et.qlim) + else: + d += abs(et.eta) + link = link.parent + if link is None or isinstance(link, str): + d_all.append(d) + break + + self._reach = max(d_all) + return self._reach + + def fkine_all(self, q: ArrayLike) -> SE3: + """ + Compute the pose of every link frame + + ``T = robot.fkine_all(q)`` is an SE3 instance with ``robot.nlinks + + 1`` values: + + - ``T[0]`` is the base transform + - ``T[i]`` is the pose of link whose ``number`` is ``i`` + + Parameters + ---------- + q + The joint configuration + + Returns + ------- + fkine_all + Pose of all links + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). - def linkcolormap(self, linkcolors="viridis"): - """ - Create a colormap for robot joints - - :param linkcolors: list of colors or colormap, defaults to "viridis" - :type linkcolors: list or str, optional - :return: color map - :rtype: matplotlib.colors.ListedColormap - - - ``cm = robot.linkcolormap()`` is an n-element colormap that gives a - unique color for every link. The RGBA colors for link ``j`` are - ``cm(j)``. - - ``cm = robot.linkcolormap(cmap)`` as above but ``cmap`` is the name - of a valid matplotlib colormap. The default, example above, is the - ``viridis`` colormap. - - ``cm = robot.linkcolormap(list of colors)`` as above but a - colormap is created from a list of n color names given as strings, - tuples or hexstrings. - - .. runblock:: pycon - - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> cm = robot.linkcolormap("inferno") - >>> print(cm(range(6))) # cm(i) is 3rd color in colormap - >>> cm = robot.linkcolormap( - >>> ['red', 'g', (0,0.5,0), '#0f8040', 'yellow', 'cyan']) - >>> print(cm(range(6))) - - .. note:: - - - Colormaps have 4-elements: red, green, blue, alpha (RGBA) - - Names of supported colors and colormaps are defined in the - matplotlib documentation. - - - `Specifying colors - `_ - - `Colormaps - `_ """ # noqa - if isinstance(linkcolors, list) and len(linkcolors) == self.n: - # provided a list of color names - return colors.ListedColormap(linkcolors) - else: - # assume it is a colormap name - return cm.get_cmap(linkcolors, 6) - - def jtraj(self, T1, T2, t, **kwargs): - """ - Joint-space trajectory between SE(3) poses + q = getvector(q) - :param T1: initial end-effector pose - :type T1: SE3 instance - :param T2: final end-effector pose - :type T2: SE3 instance - :param t: time vector or number of steps - :type t: ndarray(m) or int - :param kwargs: arguments passed to the IK solver - :return: trajectory - :rtype: Trajectory instance + Tbase = SE3(self.base) # add base, also sets the type - ``traj = obot.jtraj(T1, T2, t)`` is a trajectory object whose - attribute ``traj.q`` is a row-wise joint-space trajectory. + linkframes = Tbase.__class__.Alloc(self.nlinks + 1) + linkframes[0] = Tbase - The initial and final poses are mapped to joint space using inverse - kinematics: + def recurse(Tall, Tparent, q, link): + # if joint?? + T = Tparent + while True: + T *= SE3(link.A(q[link.jindex])) - - if the object has an analytic solution ``ikine_a`` that will be used, - - otherwise the general numerical algorithm ``ikine_min`` will be used. + Tall[link.number] = T + if link.nchildren == 0: + # no children + return + elif link.nchildren == 1: + # one child + if link in self.ee_links: # pragma nocover + # this link is an end-effector, go no further + return + link = link.children[0] + continue + else: + # multiple children + for child in link.children: + recurse(Tall, T, q, child) + return - """ + recurse(linkframes, Tbase, q, self.links[0]) - if hasattr(self, "ikine_a"): - ik = self.ikine_a - else: - ik = self.ikine_min + return linkframes - q1 = ik(T1, **kwargs) - q2 = ik(T2, **kwargs) + @overload + def manipulability( + self, + q: ArrayLike = ..., + J: None = None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + method: L["yoshikawa", "asada", "minsingular", "invcondition"] = "yoshikawa", + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + **kwargs, + ) -> Union[float, NDArray]: # pragma nocover + ... - return rtb.jtraj(q1.q, q2.q, t) + @overload + def manipulability( + self, + q: None = None, + J: NDArray = ..., + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + method: L["yoshikawa", "asada", "minsingular", "invcondition"] = "yoshikawa", + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + **kwargs, + ) -> Union[float, NDArray]: # pragma nocover + ... - def manipulability(self, q=None, J=None, method="yoshikawa", axes="all", **kwargs): + def manipulability( + self, + q=None, + J=None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + method: L["yoshikawa", "asada", "minsingular", "invcondition"] = "yoshikawa", + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + **kwargs, + ): """ Manipulability measure - :param q: Joint coordinates, one of J or q required - :type q: ndarray(n), or ndarray(m,n) - :param J: Jacobian in world frame if already computed, one of J or + ``manipulability(q)`` is the scalar manipulability index + for the robot at the joint configuration ``q``. It indicates + dexterity, that is, how well conditioned the robot is for motion + with respect to the 6 degrees of Cartesian motion. The values is + zero if the robot is at a singularity. + + Parameters + ---------- + q + Joint coordinates, one of J or q required + J + Jacobian in base frame if already computed, one of J or q required - :type J: ndarray(6,n) - :param method: method to use, "yoshikawa" (default), "condition", + method + method to use, "yoshikawa" (default), "invcondition", "minsingular" or "asada" - :type method: str - :param axes: Task space axes to consider: "all" [default], - "trans", "rot" or "both" - :type axes: str - :param kwargs: extra arguments to pass to ``jacob0`` - :return: manipulability - :rtype: float or ndarray(m) - - - ``manipulability(q)`` is the scalar manipulability index - for the robot at the joint configuration ``q``. It indicates - dexterity, that is, how well conditioned the robot is for motion - with respect to the 6 degrees of Cartesian motion. The values is - zero if the robot is at a singularity. + axes + Task space axes to consider: "all" [default], + "trans", or "rot" + + Returns + ------- + manipulability + + Synopsis + -------- Various measures are supported: - +-------------------+-------------------------------------------------+ | Measure | Description | - +-------------------+-------------------------------------------------+ + |-------------------|-------------------------------------------------| | ``"yoshikawa"`` | Volume of the velocity ellipsoid, *distance* | | | from singularity [Yoshikawa85]_ | - +-------------------+-------------------------------------------------+ | ``"invcondition"``| Inverse condition number of Jacobian, isotropy | | | of the velocity ellipsoid [Klein87]_ | - +-------------------+-------------------------------------------------+ | ``"minsingular"`` | Minimum singular value of the Jacobian, | | | *distance* from singularity [Klein87]_ | - +-------------------+-------------------------------------------------+ | ``"asada"`` | Isotropy of the task-space acceleration | | | ellipsoid which is a function of the Cartesian | | | inertia matrix which depends on the inertial | | | parameters [Asada83]_ | - +-------------------+-------------------------------------------------+ **Trajectory operation**: @@ -828,20 +784,20 @@ def manipulability(self, q=None, J=None, method="yoshikawa", axes="all", **kwarg manipulability indices for each joint configuration specified by a row of ``q``. - .. note:: - - - Invokes the ``jacob0`` method of the robot if ``J`` is not passed - - The "all" option includes rotational and translational - dexterity, but this involves adding different units. It can be - more useful to look at the translational and rotational - manipulability separately. - - Examples in the RVC book (1st edition) can be replicated by - using the "all" option - - Asada's measure requires inertial a robot model with inertial - parameters. - - :references: - + Notes + ----- + - Invokes the ``jacob0`` method of the robot if ``J`` is not passed + - The "all" option includes rotational and translational + dexterity, but this involves adding different units. It can be + more useful to look at the translational and rotational + manipulability separately. + - Examples in the RVC book (1st edition) can be replicated by + using the "all" option + - Asada's measure requires inertial a robot model with inertial + parameters. + + References + ---------- .. [Yoshikawa85] Manipulability of Robotic Mechanisms. Yoshikawa T., The International Journal of Robotics Research. 1985;4(2):3-9. doi:10.1177/027836498500400201 @@ -853,28 +809,40 @@ def manipulability(self, q=None, J=None, method="yoshikawa", axes="all", **kwarg Kinematically Redundant Manipulators. Klein CA, Blaho BE. The International Journal of Robotics Research. 1987;6(2):72-83. doi:10.1177/027836498700600206 - - Robotics, Vision & Control, Chap 8, P. Corke, Springer 2011. + + .. versionchanged:: 1.0.3 + Removed 'both' option for axes, added a custom list option. + """ - if isinstance(axes, list) and len(axes) == 6: - pass + + ets = self.ets(end, start) + + axes_list: List[bool] = [] + + if isinstance(axes, list): + axes_list = axes elif axes == "all": - axes = [True, True, True, True, True, True] + axes_list = [True, True, True, True, True, True] elif axes.startswith("trans"): - axes = [True, True, True, False, False, False] + axes_list = [True, True, True, False, False, False] elif axes.startswith("rot"): - axes = [False, False, False, True, True, True] + axes_list = [False, False, False, True, True, True] elif axes == "both": return ( - self.manipulability(q, J, method, axes="trans", **kwargs), - self.manipulability(q, J, method, axes="rot", **kwargs), + self.manipulability( + q=q, J=J, end=end, start=start, method=method, axes="trans" + ), + self.manipulability( + q=q, J=J, end=end, start=start, method=method, axes="rot" + ), ) else: raise ValueError("axes must be all, trans, rot or both") - def yoshikawa(robot, J, q, axes, **kwargs): - J = J[axes, :] + def yoshikawa(robot, J, q, axes_list): + J = J[axes_list, :] if J.shape[0] == J.shape[1]: # simplified case for square matrix return abs(np.linalg.det(J)) @@ -882,71 +850,136 @@ def yoshikawa(robot, J, q, axes, **kwargs): m2 = np.linalg.det(J @ J.T) return np.sqrt(abs(m2)) - def condition(robot, J, q, axes, **kwargs): - J = J[axes, :] - return 1 / np.linalg.cond(J) # return 1/cond(J) + def condition(robot, J, q, axes_list): + J = J[axes_list, :] + + # return 1/cond(J) + return 1 / np.linalg.cond(J) - def minsingular(robot, J, q, axes, **kwargs): - J = J[axes, :] + def minsingular(robot, J, q, axes_list): + J = J[axes_list, :] s = np.linalg.svd(J, compute_uv=False) - return s[-1] # return last/smallest singular value of J - def asada(robot, J, q, axes, **kwargs): - # dof = np.sum(axes) + # return last/smallest singular value of J + return s[-1] + + def asada(robot, J, q, axes_list): + # dof = np.sum(axes_list) if np.linalg.matrix_rank(J) < 6: return 0 Ji = np.linalg.pinv(J) Mx = Ji.T @ robot.inertia(q) @ Ji - d = np.where(axes)[0] + d = np.where(axes_list)[0] Mx = Mx[d] Mx = Mx[:, d.tolist()] e, _ = np.linalg.eig(Mx) return np.min(e) / np.max(e) # choose the handler function - if method == "yoshikawa": + if method.lower().startswith("yoshi"): mfunc = yoshikawa - elif method == "invcondition": + elif method.lower().startswith("invc"): mfunc = condition - elif method == "minsingular": + elif method.lower().startswith("mins"): mfunc = minsingular - elif method == "asada": + elif method.lower().startswith("asa"): mfunc = asada else: raise ValueError("Invalid method chosen") # Calculate manipulability based on supplied Jacobian if J is not None: - w = [mfunc(self, J, q, axes)] + w = [mfunc(self, J, q, axes_list)] # Otherwise use the q vector/matrix else: - q = getmatrix(q, (None, self.n)) + q = np.array(getmatrix(q, (None, self.n))) w = np.zeros(q.shape[0]) for k, qk in enumerate(q): - Jk = self.jacob0(qk, **kwargs) - w[k] = mfunc(self, Jk, qk, axes) + Jk = ets.jacob0(qk) + w[k] = mfunc(self, Jk, qk, axes_list) if len(w) == 1: return w[0] else: return w - def jacob0_dot(self, q=None, qd=None, J0=None, representation=None): - r""" - Derivative of Jacobian - - :param q: The joint configuration of the robot - :type q: float ndarray(n) - :param qd: The joint velocity of the robot - :type qd: ndarray(n) - :param J0: Jacobian in {0} frame - :type J0: ndarray(6,n) - :param representation: angular representation - :type representation: str - :return: The derivative of the manipulator Jacobian - :rtype: ndarray(6,n) + def jtraj( + self, + T1: Union[NDArray, SE3], + T2: Union[NDArray, SE3], + t: Union[NDArray, int], + **kwargs, + ): + """ + Joint-space trajectory between SE(3) poses + + The initial and final poses are mapped to joint space using inverse + kinematics: + + - if the object has an analytic solution ``ikine_a`` that will be used, + - otherwise the general numerical algorithm ``ikine_lm`` will be used. + + ``traj = obot.jtraj(T1, T2, t)`` is a trajectory object whose + attribute ``traj.q`` is a row-wise joint-space trajectory. + + Parameters + ---------- + T1 + initial end-effector pose + T2 + final end-effector pose + t + time vector or number of steps + kwargs + arguments passed to the IK solver + + Returns + ------- + trajectory + + """ # noqa + + if hasattr(self, "ikine_a"): + ik = self.ikine_a # type: ignore + else: + ik = self.ikine_LM + + q1 = ik(T1, **kwargs) + q2 = ik(T2, **kwargs) + + return rtb.jtraj(q1.q, q2.q, t) + + @overload + def jacob0_dot( + self, + q: ArrayLike, + qd: ArrayLike, + J0: None = None, + representation: Union[L["rpy/xyz", "rpy/zyx", "eul", "exp"], None] = None, + ) -> NDArray: # pragma no cover + ... + + @overload + def jacob0_dot( + self, + q: None, + qd: ArrayLike, + J0: NDArray = ..., + representation: Union[L["rpy/xyz", "rpy/zyx", "eul", "exp"], None] = None, + ) -> NDArray: # pragma no cover + ... + + def jacob0_dot( + self, + q, + qd: ArrayLike, + J0=None, + representation: Union[L["rpy/xyz", "rpy/zyx", "eul", "exp"], None] = None, + ): + r""" + Derivative of Jacobian ``robot.jacob_dot(q, qd)`` computes the rate of change of the Jacobian elements @@ -957,7 +990,26 @@ def jacob0_dot(self, q=None, qd=None, J0=None, representation=None): where the first term is the rank-3 Hessian. - If ``J0`` is already calculated for the joint + Parameters + ---------- + q + The joint configuration of the robot + qd + The joint velocity of the robot + J0 + Jacobian in {0} frame + representation + angular representation + + Returns + ------- + jdot + The derivative of the manipulator Jacobian + + Synopsis + -------- + + If ``J0`` is already calculated for the joint coordinates ``q`` it can be passed in to to save computation time. It is computed as the mode-3 product of the Hessian tensor and the @@ -966,25 +1018,29 @@ def jacob0_dot(self, q=None, qd=None, J0=None, representation=None): The derivative of an analytical Jacobian can be obtained by setting ``representation`` as - ================== ================================== - ``representation`` Rotational representation - ================== ================================== - ``'rpy/xyz'`` RPY angular rates in XYZ order - ``'rpy/zyx'`` RPY angular rates in XYZ order - ``'eul'`` Euler angular rates in ZYZ order - ``'exp'`` exponential coordinate rates - ================== ================================== + |``representation`` | Rotational representation | + |---------------------|-------------------------------------| + |``'rpy/xyz'`` | RPY angular rates in XYZ order | + |``'rpy/zyx'`` | RPY angular rates in XYZ order | + |``'eul'`` | Euler angular rates in ZYZ order | + |``'exp'`` | exponential coordinate rates | - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - :seealso: :func:`jacob0`, :func:`hessian0` - """ # noqa - # n = len(q) + References + ---------- + - Kinematic Derivatives using the Elementary Transform + Sequence, J. Haviland and P. Corke - if representation is None: + See Also + -------- + :func:`jacob0` + :func:`hessian0` + """ + + qd = np.array(qd) + + if representation is None: if J0 is None: J0 = self.jacob0(q) H = self.hessian0(q, J0=J0) @@ -1012,32 +1068,74 @@ def jacob0_dot(self, q=None, qd=None, J0=None, representation=None): H = smb.numhess( lambda q: self.jacob0_analytical(q, representation=representation), q ) + # Jd = Ai @ Jd + # return Jd + return np.tensordot(H, qd, (0, 0)) - def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes="all"): + @overload + def jacobm( + self, + q: ArrayLike = ..., + J: None = None, + H: None = None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + ) -> NDArray: # pragma no cover + ... + + @overload + def jacobm( + self, + q: None = None, + J: NDArray = ..., + H: NDArray = ..., + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + ) -> NDArray: # pragma no cover + ... + + def jacobm( + self, + q=None, + J=None, + H=None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + axes: Union[L["all", "trans", "rot"], List[bool]] = "all", + ) -> NDArray: r""" - Calculates the manipulability Jacobian. This measure relates the rate - of change of the manipulability to the joint velocities of the robot. - One of J or q is required. Supply J and H if already calculated to - save computation time + The manipulability Jacobian - :param q: The joint angles/configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param J: The manipulator Jacobian in any frame - :type J: float ndarray(6,n) - :param H: The manipulator Hessian in any frame - :type H: float ndarray(6,n,n) - :param end: the final link or Gripper which the Hessian represents - :type end: str or ELink or Gripper - :param start: the first link which the Hessian represents - :type start: str or ELink - - :return: The manipulability Jacobian - :rtype: float ndarray(n) + This measure relates the rate of change of the manipulability to the + joint velocities of the robot. One of J or q is required. Supply J + and H if already calculated to save computation time + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + J + The manipulator Jacobian in any frame + H + The manipulator Hessian in any frame + end + the final link or Gripper which the Hessian represents + start + the first link which the Hessian represents + + Returns + ------- + jacobm + The manipulability Jacobian + + Synopsis + -------- Yoshikawa's manipulability measure .. math:: @@ -1050,22 +1148,27 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes="all"): \frac{\partial m(\vec{q})}{\partial \vec{q}} - :references: - - Kinematic Derivatives using the Elementary Transform - Sequence, J. Haviland and P. Corke - """ + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa end, start, _ = self._get_limit_links(end, start) - # path, n, _ = self.get_path(end, start) - if axes == "all": - axes = [True, True, True, True, True, True] - elif axes.startswith("trans"): - axes = [True, True, True, False, False, False] - elif axes.startswith("rot"): - axes = [False, False, False, True, True, True] - else: - raise ValueError("axes must be all, trans or rot") + # + if not isinstance(axes, list): + if axes.startswith("all"): + axes = [True, True, True, True, True, True] + elif axes.startswith("trans"): + axes = [True, True, True, False, False, False] + elif axes.startswith("rot"): + axes = [False, False, False, True, True, True] + else: + raise ValueError("axes must be all, trans or rot") if J is None: if q is None: @@ -1084,10 +1187,12 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes="all"): else: verifymatrix(H, (6, self.n, self.n)) - manipulability = self.manipulability(q, J=J, start=start, end=end, axes=axes) + manipulability = self.manipulability( + q, J=J, start=start, end=end, axes=axes # type: ignore + ) - J = J[axes, :] - H = H[:, axes, :] + J = J[axes, :] # type: ignore + H = H[:, axes, :] # type: ignore b = np.linalg.inv(J @ np.transpose(J)) Jm = np.zeros((n, 1)) @@ -1098,1122 +1203,808 @@ def jacobm(self, q=None, J=None, H=None, end=None, start=None, axes="all"): return Jm - @abstractmethod - def ets(self, *args, **kwargs) -> ETS: - pass - - def jacob0_analytical( - self, - q: ArrayLike, - representation: str = "rpy/xyz", - end: Union[str, Link, Gripper, None] = None, - start: Union[str, Link, Gripper, None] = None, - tool: Union[ndarray, SE3, None] = None, - ): - r""" - Manipulator analytical Jacobian in the ``start`` frame - - :param q: Joint coordinate vector - :type q: Arraylike - :param representation: angular representation - :type representation: str - :param end: the particular link or gripper whose velocity the Jacobian - describes, defaults to the base link - :param start: the link considered as the end-effector, defaults to the robots's end-effector - :param tool: a static tool transformation matrix to apply to the - end of end, defaults to None - - :return J: Manipulator Jacobian in the ``start`` frame - - - ``robot.jacob0_analytical(q)`` is the manipulator Jacobian matrix which maps - joint velocity to end-effector spatial velocity expressed in the - ``start`` frame. - - End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` - is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. - - ================== ================================== - ``representation`` Rotational representation - ================== ================================== - ``'rpy/xyz'`` RPY angular rates in XYZ order - ``'rpy/zyx'`` RPY angular rates in XYZ order - ``'eul'`` Euler angular rates in ZYZ order - ``'exp'`` exponential coordinate rates - ================== ================================== - - Example: - .. runblock:: pycon - >>> import roboticstoolbox as rtb - >>> puma = rtb.models.ETS.Puma560() - >>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0]) - - .. warning:: ``start`` and ``end`` must be on the same branch, - with ``start`` closest to the base. - """ # noqa - return self.ets(start, end).jacob0_analytical( - q, tool=tool, representation=representation - ) - # --------------------------------------------------------------------- # - - @property - def name(self): - """ - Get/set robot name (Robot superclass) - - - ``robot.name`` is the robot name - - :return: robot name - :rtype: str - - - ``robot.name = ...`` checks and sets therobot name - """ - return self._name - - @name.setter - def name(self, name_new): - self._name = name_new - + # --------- Collision Methods ----------------------------------------- # # --------------------------------------------------------------------- # - @property - def manufacturer(self): + def closest_point( + self, q: ArrayLike, shape: Shape, inf_dist: float = 1.0, skip: bool = False + ) -> Tuple[Union[int, None], Union[NDArray, None], Union[NDArray, None],]: """ - Get/set robot manufacturer's name (Robot superclass) + Find the closest point between robot and shape - - ``robot.manufacturer`` is the robot manufacturer's name + ``closest_point(shape, inf_dist)`` returns the minimum euclidean + distance between this robot and shape, provided it is less than + inf_dist. It will also return the points on self and shape in the + world frame which connect the line of length distance between the + shapes. If the distance is negative then the shapes are collided. + + Attributes + ---------- + shape + The shape to compare distance to + inf_dist + The minimum distance within which to consider + the shape + skip + Skip setting all shape transforms based on q, use this + option if using this method in conjuction with Swift to save time - :return: robot manufacturer's name - :rtype: str + Returns + ------- + d + distance between the robot and shape + p1 + [x, y, z] point on the robot (in the world frame) + p2 + [x, y, z] point on the shape (in the world frame) - - ``robot.manufacturer = ...`` checks and sets the manufacturer's name """ - return self._manufacturer - - @manufacturer.setter - def manufacturer(self, manufacturer_new): - self._manufacturer = manufacturer_new - # --------------------------------------------------------------------- # + if not skip: + self._update_link_tf(q) + self._propogate_scene_tree() + shape._propogate_scene_tree() - @property - def links(self): - """ - Robot links (Robot superclass) + d = 10000 + p1 = None + p2 = None - :return: A list of link objects - :rtype: list of Link subclass instances + for link in self.links: + td, tp1, tp2 = link.closest_point(shape, inf_dist, skip=True) - .. note:: It is probably more concise to index the robot object rather - than the list of links, ie. the following are equivalent:: + if td is not None and td < d: + d = td + p1 = tp1 + p2 = tp2 - robot.links[i] - robot[i] - """ - return self._links + if d == 10000: + d = None - # --------------------------------------------------------------------- # + return d, p1, p2 - @property - def base(self) -> SE3: + def iscollided(self, q, shape: Shape, skip: bool = False) -> bool: """ - Get/set robot base transform (Robot superclass) + Check if the robot is in collision with a shape - - ``robot.base`` is the robot base transform + ``iscollided(shape)`` checks if this robot and shape have collided - :return: robot tool transform + Attributes + ---------- + shape + The shape to compare distance to + skip + Skip setting all shape transforms based on q, use this + option if using this method in conjuction with Swift to save time - - ``robot.base = ...`` checks and sets the robot base transform + Returns + ------- + iscollided + True if shapes have collided """ - # return a copy, otherwise somebody with - # reference to the base can change it - - # This now returns the Scene Node transform - # self._T is a copy of SceneNode.__T - return SE3(self._T, check=False) + if not skip: + self._update_link_tf(q) + self._propogate_scene_tree() + shape._propogate_scene_tree() - @base.setter - def base(self, T: Union[np.ndarray, SE3]): + for link in self.links: + if link.iscollided(shape, skip=True): + return True if isinstance(self, rtb.Robot): - # All 3D robots - # Set the SceneNode T - if isinstance(T, SE3): - self._T = T.A - else: - self._T = T - - else: - raise ValueError("base must be set to None (no tool), SE2, or SE3") - - # --------------------------------------------------------------------- # - - @property - def tool(self) -> SE3: - """ - Get/set robot tool transform (Robot superclass) - - - ``robot.tool`` is the robot tool transform - - :return: robot tool transform + for gripper in self.grippers: + for link in gripper.links: + if link.iscollided(shape, skip=True): + return True - - ``robot.tool = ...`` checks and sets the robot tool transform + return False + def collided(self, q, shape: Shape, skip: bool = False) -> bool: """ - return SE3(self._tool, check=False) - - @tool.setter - def tool(self, T: Union[SE3, np.ndarray]): - if isinstance(T, SE3): - self._tool = T.A - else: - self._tool = T - - @property - def qlim(self): - r""" - Joint limits (Robot superclass) + Check if the robot is in collision with a shape - :return: Array of joint limit values - :rtype: ndarray(2,n) - :exception ValueError: unset limits for a prismatic joint + ``collided(shape)`` checks if this robot and shape have collided - Limits are extracted from the link objects. If joints limits are - not set for: - - - a revolute joint [-𝜋. 𝜋] is returned - - a prismatic joint an exception is raised - - Example: + Attributes + ---------- + shape + The shape to compare distance to + skip + Skip setting all shape transforms based on q, use this + option if using this method in conjuction with Swift to save time - .. runblock:: pycon + Returns + ------- + collided + True if shapes have collided - >>> import roboticstoolbox as rtb - >>> robot = rtb.models.DH.Puma560() - >>> robot.qlim """ - # TODO tidy up - limits = np.zeros((2, self.n)) - j = 0 - for link in self: - if link.isrevolute: - if link.qlim is None: - v = np.r_[-np.pi, np.pi] - else: - v = link.qlim - elif link.isprismatic: - if link.qlim is None: - raise ValueError("undefined prismatic joint limit") - else: - v = link.qlim - else: - # fixed link - continue - limits[:, j] = v - j += 1 - return limits + warn("method collided is deprecated, use iscollided instead", FutureWarning) + return self.iscollided(q, shape, skip=skip) # --------------------------------------------------------------------- # - - @property - def q(self): - """ - Get/set robot joint configuration (Robot superclass) - - - ``robot.q`` is the robot joint configuration - - :return: robot joint configuration - :rtype: ndarray(n,) - - - ``robot.q = ...`` checks and sets the joint configuration - - .. note:: ??? - """ - return self._q - - @q.setter - def q(self, q_new): - self._q = getvector(q_new, self.n) - + # --------- Constraint Methods ---------------------------------------- # # --------------------------------------------------------------------- # - @property - def qd(self) -> ndarray: + def joint_velocity_damper( + self, + ps: float = 0.05, + pi: float = 0.1, + n: Union[int, None] = None, + gain: float = 1.0, + ) -> Tuple[NDArray, NDArray]: """ - Get/set robot joint velocity (Robot superclass) + Compute the joint velocity damper for QP motion control - - ``robot.qd`` is the robot joint velocity + Formulates an inequality contraint which, when optimised for will + make it impossible for the robot to run into joint limits. Requires + the joint limits of the robot to be specified. See examples/mmc.py + for use case - :return: robot joint velocity - :rtype: ndarray(n,) + Attributes + ---------- + ps + The minimum angle (in radians) in which the joint is + allowed to approach to its limit + pi + The influence angle (in radians) in which the velocity + damper becomes active + n + The number of joints to consider. Defaults to all joints + gain + The gain for the velocity damper - - ``robot.qd = ...`` checks and sets the joint velocity + Returns + ------- + Ain + A (6,) vector inequality contraint for an optisator + Bin + b (6,) vector inequality contraint for an optisator - .. note:: ??? """ - return self._qd - - @qd.setter - def qd(self, qd_new): - self._qd = np.array(getvector(qd_new, self.n)) - # --------------------------------------------------------------------- # - - @property - def qdd(self): - """ - Get/set robot joint acceleration (Robot superclass) + if n is None: + n = self.n - - ``robot.qdd`` is the robot joint acceleration + Ain = np.zeros((n, n)) + Bin = np.zeros(n) - :return: robot joint acceleration - :rtype: ndarray(n,) + for i in range(n): + if self.q[i] - self.qlim[0, i] <= pi: + Bin[i] = -gain * (((self.qlim[0, i] - self.q[i]) + ps) / (pi - ps)) + Ain[i, i] = -1 + if self.qlim[1, i] - self.q[i] <= pi: + Bin[i] = gain * ((self.qlim[1, i] - self.q[i]) - ps) / (pi - ps) + Ain[i, i] = 1 - - ``robot.qdd = ...`` checks and sets the robot joint acceleration + return Ain, Bin - .. note:: ??? + def link_collision_damper( + self, + shape: CollisionShape, + q: ArrayLike, + di: float = 0.3, + ds: float = 0.05, + xi: float = 1.0, + end: Union[Link, None] = None, + start: Union[Link, None] = None, + collision_list: Union[List[Shape], None] = None, + ): """ - return self._qdd + Compute a collision constrain for QP motion control - @qdd.setter - def qdd(self, qdd_new): - self._qdd = getvector(qdd_new, self.n) + Formulates an inequality contraint which, when optimised for will + make it impossible for the robot to run into a collision. Requires + See examples/neo.py for use case + + Attributes + ---------- + ds + The minimum distance in which a joint is allowed to + approach the collision object shape + di + The influence distance in which the velocity + damper becomes active + xi + The gain for the velocity damper + end + The end link of the robot to consider + start + The start link of the robot to consider + collision_list + A list of shapes to consider for collision - # --------------------------------------------------------------------- # + Returns + ------- + Ain + A (6,) vector inequality contraint for an optisator + Bin + b (6,) vector inequality contraint for an optisator - @property - def control_mode(self): """ - Get/set robot control mode (Robot superclass) - - ``robot.control_type`` is the robot control mode + end, start, _ = self._get_limit_links(start=start, end=end) - :return: robot control mode - :rtype: ndarray(n,) - - - ``robot.control_type = ...`` checks and sets the robot control mode - - .. note:: ??? - """ - return self._control_mode - - @control_mode.setter - def control_mode(self, cn): - if cn == "p" or cn == "v" or cn == "a": - self._control_mode = cn - else: - raise ValueError("Control type must be one of 'p', 'v', or 'a'") + links, n, _ = self.get_path(start=start, end=end) - # --------------------------------------------------------------------- # + q = np.array(q) + j = 0 + Ain = None + bin = None - # TODO probably should be a static method - def _get_graphical_backend(self, backend=None): + def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): + d, wTlp, wTcp = link_col.closest_point(shape, di) - default = self.default_backend + if d is not None and wTlp is not None and wTcp is not None: + lpTcp = -wTlp + wTcp - # figure out the right default - if backend is None: - if isinstance(self, rtb.DHRobot): - default = "pyplot" - elif isinstance(self, rtb.ERobot2): - default = "pyplot2" - elif isinstance(self, rtb.ERobot): - if self.hasgeometry: - default = "swift" - else: - default = "pyplot" - - if backend is not None: - backend = backend.lower() - - # find the right backend, modules are imported here on an as needs - # basis - if backend == "swift" or default == "swift": # pragma nocover - # swift was requested, is it installed? - if isinstance(self, rtb.DHRobot): - raise NotImplementedError( - "Plotting in Swift is not implemented for DHRobots yet" + norm = lpTcp / d + norm_h = np.expand_dims( + np.concatenate((norm, [0.0, 0.0, 0.0])), axis=0 # type: ignore ) - try: - # yes, use it - from roboticstoolbox.backends.swift import Swift - - env = Swift() - return env - except ModuleNotFoundError: - if backend == "swift": - print("Swift is not installed, " "install it using pip or conda") - backend = "pyplot" - - elif backend == "vpython" or default == "vpython": # pragma nocover - # vpython was requested, is it installed? - if not isinstance(self, rtb.DHRobot): - raise NotImplementedError( - "Plotting in VPython is only implemented for DHRobots" - ) - try: - # yes, use it - from roboticstoolbox.backends.VPython import VPython - env = VPython() - return env - except ModuleNotFoundError: - if backend == "vpython": - print("VPython is not installed, " "install it using pip or conda") - backend = "pyplot" - if backend is None: - backend = default + # tool = (self.fkine(q, end=link).inv() * SE3(wTlp)).A[:3, 3] - if backend == "pyplot": - from roboticstoolbox.backends.PyPlot import PyPlot + # Je = self.jacob0(q, end=link, tool=tool) + # Je[:3, :] = self._T[:3, :3] @ Je[:3, :] - env = PyPlot() + # n_dim = Je.shape[1] + # dp = norm_h @ shape.v + # l_Ain = zeros((1, self.n)) - elif backend == "pyplot2": - from roboticstoolbox.backends.PyPlot import PyPlot2 + Je = self.jacobe(q, start=start, end=link, tool=link_col.T) + n_dim = Je.shape[1] + dp = norm_h @ shape.v + l_Ain = np.zeros((1, n)) - env = PyPlot2() + l_Ain[0, :n_dim] = 1 * norm_h @ Je + l_bin = (xi * (d - ds) / (di - ds)) + dp + else: # pragma nocover + l_Ain = None + l_bin = None - else: - raise ValueError("unknown backend", backend) - - return env + return l_Ain, l_bin - def plot( - self, - q, - backend=None, - block=False, - dt=0.050, - limits=None, - vellipse=False, - fellipse=False, - fig=None, - movie=None, - loop=False, - **kwargs, - ): - """ - Graphical display and animation - - :param q: The joint configuration of the robot. - :type q: float ndarray(n) - :param backend: The graphical backend to use, currently 'swift' - and 'pyplot' are implemented. Defaults to 'swift' of an ``ERobot`` - and 'pyplot` for a ``DHRobot`` - :type backend: string - :param block: Block operation of the code and keep the figure open - :type block: bool - :param dt: if q is a trajectory, this describes the delay in - seconds between frames - :type dt: float - :param limits: Custom view limits for the plot. If not supplied will - autoscale, [x1, x2, y1, y2, z1, z2] - (this option is for 'pyplot' only) - :type limits: ndarray(6) - :param vellipse: (Plot Option) Plot the velocity ellipse at the - end-effector (this option is for 'pyplot' only) - :type vellipse: bool - :param vellipse: (Plot Option) Plot the force ellipse at the - end-effector (this option is for 'pyplot' only) - :type vellipse: bool - :param jointaxes: (Plot Option) Plot an arrow indicating the axes in - which the joint revolves around(revolute joint) or translates - along (prosmatic joint) (this option is for 'pyplot' only) - :type jointaxes: bool - :param eeframe: (Plot Option) Plot the end-effector coordinate frame - at the location of the end-effector. Uses three arrows, red, - green and blue to indicate the x, y, and z-axes. - (this option is for 'pyplot' only) - :type eeframe: bool - :param shadow: (Plot Option) Plot a shadow of the robot in the x-y - plane. (this option is for 'pyplot' only) - :type shadow: bool - :param name: (Plot Option) Plot the name of the robot near its base - (this option is for 'pyplot' only) - :type name: bool - :param movie: name of file in which to save an animated GIF - (this option is for 'pyplot' only) - :type movie: str - - :return: A reference to the environment object which controls the - figure - :rtype: Swift or PyPlot - - - ``robot.plot(q, 'pyplot')`` displays a graphical view of a robot - based on the kinematic model and the joint configuration ``q``. - This is a stick figure polyline which joins the origins of the - link coordinate frames. The plot will autoscale with an aspect - ratio of 1. - - If ``q`` (m,n) representing a joint-space trajectory it will create an - animation with a pause of ``dt`` seconds between each frame. - - .. note:: - - By default this method will block until the figure is dismissed. - To avoid this set ``block=False``. - - For PyPlot, the polyline joins the origins of the link frames, - but for some Denavit-Hartenberg models those frames may not - actually be on the robot, ie. the lines to not neccessarily - represent the links of the robot. - - :seealso: :func:`teach` - """ + for link in links: + if link.isjoint: + j += 1 - env = None + if collision_list is None: + col_list = link.collision - env = self._get_graphical_backend(backend) + for c in col_list: + pass + else: + col_list = [collision_list[j - 1]] # pragma nocover - q = getmatrix(q, (None, self.n)) - self.q = q[0, :] + for link_col in col_list: + l_Ain, l_bin = indiv_calculation(link, link_col, q) # type: ignore - # Add the self to the figure in readonly mode - # Add the self to the figure in readonly mode - if q.shape[0] == 1: - env.launch(self.name + " Plot", limits=limits, fig=fig) - else: - env.launch(self.name + " Trajectory Plot", limits=limits, fig=fig) - - env.add(self, readonly=True, **kwargs) - - if vellipse: - vell = self.vellipse(centre="ee") - env.add(vell) - - if fellipse: - fell = self.fellipse(centre="ee") - env.add(fell) - - # Stop lint error - images = [] # list of images saved from each plot - - if movie is not None: - loop = False - - while True: - for qk in q: - self.q = qk - if vellipse: - vell.q = qk - if fellipse: - fell.q = qk - env.step(dt) - - if movie is not None: # pragma nocover - images.append(env.getframe()) - - if movie is not None: # pragma nocover - # save it as an animated GIF - images[0].save( - movie, - save_all=True, - append_images=images[1:], - optimize=False, - duration=dt, - loop=0, - ) - if not loop: - break - - # Keep the plot open - if block: # pragma: no cover - env.hold() + if l_Ain is not None and l_bin is not None: + if Ain is None: + Ain = l_Ain + else: + Ain = np.concatenate((Ain, l_Ain)) - return env + if bin is None: + bin = np.array(l_bin) + else: + bin = np.concatenate((bin, l_bin)) - # --------------------------------------------------------------------- # + return Ain, bin - def fellipse(self, q=None, opt="trans", unit="rad", centre=[0, 0, 0]): + def vision_collision_damper( + self, + shape: CollisionShape, + camera: Union["Robot", SE3, None] = None, + camera_n: int = 0, + q=None, + di=0.3, + ds=0.05, + xi=1.0, + end=None, + start=None, + collision_list=None, + ): # pragma nocover """ - Create a force ellipsoid object for plotting with PyPlot + Compute a vision collision constrain for QP motion control - :param q: The joint configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param opt: 'trans' or 'rot' will plot either the translational or - rotational force ellipsoid - :type opt: string - :param centre: - :type centre: list or str('ee') - - :return: An EllipsePlot object - :rtype: EllipsePlot - - - ``robot.fellipse(q)`` creates a force ellipsoid for the robot at - pose ``q``. The ellipsoid is centered at the origin. - - - ``robot.fellipse()`` as above except the joint configuration is that - stored in the robot object. - - .. note:: - - By default the ellipsoid related to translational motion is - drawn. Use ``opt='rot'`` to draw the rotational velocity - ellipsoid. - - By default the ellipsoid is drawn at the origin. The option - ``centre`` allows its origin to set to set to the specified - 3-vector, or the string "ee" ensures it is drawn at the - end-effector position. + Formulates an inequality contraint which, when optimised for will + make it impossible for the robot to run into a line of sight. + See examples/fetch_vision.py for use case + + Attributes + ---------- + camera + The camera link, either as a robotic link or SE3 + pose + camera_n + Degrees of freedom of the camera link + ds + The minimum distance in which a joint is allowed to + approach the collision object shape + di + The influence distance in which the velocity + damper becomes active + xi + The gain for the velocity damper + end + The end link of the robot to consider + start + The start link of the robot to consider + collision_list + A list of shapes to consider for collision + Returns + ------- + Ain + A (6,) vector inequality contraint for an optisator + Bin + b (6,) vector inequality contraint for an optisator """ - if isinstance(self, rtb.ERobot): # pragma nocover - raise NotImplementedError("ERobot fellipse not implemented yet") - q = getunit(q, unit) - ell = EllipsePlot(self, q, "f", opt, centre=centre) - return ell + end, start, _ = self._get_limit_links(start=start, end=end) - def vellipse(self, q=None, opt="trans", unit="rad", centre=[0, 0, 0], scale=0.1): - """ - Create a velocity ellipsoid object for plotting with PyPlot + links, n, _ = self.get_path(start=start, end=end) - :param q: The joint configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param opt: 'trans' or 'rot' will plot either the translational or - rotational velocity ellipsoid - :type opt: string - :param centre: - :type centre: list or str('ee') - - :return: An EllipsePlot object - :rtype: EllipsePlot - - - ``robot.vellipse(q)`` creates a force ellipsoid for the robot at - pose ``q``. The ellipsoid is centered at the origin. - - - ``robot.vellipse()`` as above except the joint configuration is that - stored in the robot object. - - .. note:: - - By default the ellipsoid related to translational motion is - drawn. Use ``opt='rot'`` to draw the rotational velocity - ellipsoid. - - By default the ellipsoid is drawn at the origin. The option - ``centre`` allows its origin to set to set to the specified - 3-vector, or the string "ee" ensures it is drawn at the - end-effector position. - """ - if isinstance(self, rtb.ERobot): # pragma nocover - raise NotImplementedError("ERobot vellipse not implemented yet") + q = np.array(q) + j = 0 + Ain = None + bin = None - q = getunit(q, unit) - ell = EllipsePlot(self, q, "v", opt, centre=centre, scale=scale) - return ell + def rotation_between_vectors(a, b): + a = a / np.linalg.norm(a) + b = b / np.linalg.norm(b) - def plot_ellipse( - self, - ellipse, - block=True, - limits=None, - jointaxes=True, - eeframe=True, - shadow=True, - name=True, - ): - """ - Plot the an ellipsoid - - :param block: Block operation of the code and keep the figure open - :type block: bool - :param ellipse: the ellipsoid to plot - :type ellipse: EllipsePlot - :param jointaxes: (Plot Option) Plot an arrow indicating the axes in - which the joint revolves around(revolute joint) or translates - along (prosmatic joint) - :type jointaxes: bool - :param eeframe: (Plot Option) Plot the end-effector coordinate frame - at the location of the end-effector. Uses three arrows, red, - green and blue to indicate the x, y, and z-axes. - :type eeframe: bool - :param shadow: (Plot Option) Plot a shadow of the robot in the x-y - plane - :type shadow: bool - :param name: (Plot Option) Plot the name of the robot near its base - :type name: bool - - :return: A reference to the PyPlot object which controls the - matplotlib figure - :rtype: PyPlot - - - ``robot.plot_ellipse(ellipsoid)`` displays the ellipsoid. - - .. note:: - - By default the ellipsoid is drawn at the origin. The option - ``centre`` allows its origin to set to set to the specified - 3-vector, or the string "ee" ensures it is drawn at the - end-effector position. - """ + angle = np.arccos(np.dot(a, b)) + axis = np.cross(a, b) - if not isinstance(ellipse, EllipsePlot): # pragma nocover - raise TypeError( - "ellipse must be of type " "roboticstoolbox.backend.PyPlot.EllipsePlot" - ) + return SE3.AngleAxis(angle, axis) - env = PyPlot() + if isinstance(camera, rtb.BaseRobot): + wTcp = camera.fkine(camera.q).A[:3, 3] + elif isinstance(camera, SE3): + wTcp = camera.t + else: + raise TypeError("Camera must be a robotic link or SE3 pose") - # Add the robot to the figure in readonly mode - env.launch(ellipse.robot.name + " " + ellipse.name, limits=limits) + wTtp = shape.T[:3, -1] - env.add(ellipse, jointaxes=jointaxes, eeframe=eeframe, shadow=shadow, name=name) + # Create line of sight object + los_mid = SE3((wTcp + wTtp) / 2) + los_orientation = rotation_between_vectors( + np.array([0.0, 0.0, 1.0]), wTcp - wTtp # type: ignore + ) - # Keep the plot open - if block: # pragma: no cover - env.hold() + los = Cylinder( + radius=0.001, + length=np.linalg.norm(wTcp - wTtp), # type: ignore + base=(los_mid * los_orientation), + ) - return env + def indiv_calculation(link: Link, link_col: CollisionShape, q: NDArray): + d, wTlp, wTvp = link_col.closest_point(los, di) - def plot_fellipse( - self, - q=None, - block=True, - fellipse=None, - limits=None, - opt="trans", - centre=[0, 0, 0], - jointaxes=True, - eeframe=True, - shadow=True, - name=True, - ): - """ - Plot the force ellipsoid for manipulator + if d is not None and wTlp is not None and wTvp is not None: + lpTvp = -wTlp + wTvp - :param block: Block operation of the code and keep the figure open - :type block: bool - :param q: The joint configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param fellipse: the vellocity ellipsoid to plot - :type fellipse: EllipsePlot - :param limits: Custom view limits for the plot. If not supplied will - autoscale, [x1, x2, y1, y2, z1, z2] - :type limits: ndarray(6) - :param opt: 'trans' or 'rot' will plot either the translational or - rotational force ellipsoid - :type opt: string - :param centre: The coordinates to plot the fellipse [x, y, z] or "ee" - to plot at the end-effector location - :type centre: array_like or str - :param jointaxes: (Plot Option) Plot an arrow indicating the axes in - which the joint revolves around(revolute joint) or translates - along (prosmatic joint) - :type jointaxes: bool - :param eeframe: (Plot Option) Plot the end-effector coordinate frame - at the location of the end-effector. Uses three arrows, red, - green and blue to indicate the x, y, and z-axes. - :type eeframe: bool - :param shadow: (Plot Option) Plot a shadow of the robot in the x-y - plane - :type shadow: bool - :param name: (Plot Option) Plot the name of the robot near its base - :type name: bool - - :return: A reference to the PyPlot object which controls the - matplotlib figure - :rtype: PyPlot - - - ``robot.plot_fellipse(q)`` displays the velocity ellipsoid for the - robot at pose ``q``. The plot will autoscale with an aspect ratio - of 1. - - - ``plot_fellipse()`` as above except the robot is plotted with joint - coordinates stored in the robot object. - - - ``robot.plot_fellipse(vellipse)`` specifies a custon ellipse to plot. - - .. note:: - - By default the ellipsoid related to translational motion is - drawn. Use ``opt='rot'`` to draw the rotational velocity - ellipsoid. - - By default the ellipsoid is drawn at the origin. The option - ``centre`` allows its origin to set to set to the specified - 3-vector, or the string "ee" ensures it is drawn at the - end-effector position. - """ + norm = lpTvp / d + norm_h = np.expand_dims( + np.concatenate((norm, [0.0, 0.0, 0.0])), axis=0 + ) # type: ignore - if isinstance(self, rtb.ERobot): # pragma nocover - raise NotImplementedError( - "Ellipse Plotting of ERobot's not implemented yet" - ) + tool = SE3( + (np.linalg.inv(self.fkine(q, end=link).A) @ SE3(wTlp).A)[:3, 3] + ) - if q is not None: - self.q = q + Je = self.jacob0(q, end=link, tool=tool.A) + Je[:3, :] = self._T[:3, :3] @ Je[:3, :] + n_dim = Je.shape[1] + + if isinstance(camera, "Robot"): + Jv = camera.jacob0(camera.q) + Jv[:3, :] = self._T[:3, :3] @ Jv[:3, :] + + Jv *= ( + np.linalg.norm(wTvp - shape.T[:3, -1]) / los.length + ) # type: ignore + + dpc = norm_h @ Jv + dpc = np.concatenate( + ( + dpc[0, :-camera_n], + np.zeros(self.n - (camera.n - camera_n)), + dpc[0, -camera_n:], + ) + ) + else: + dpc = np.zeros((1, self.n + camera_n)) - if fellipse is None: - fellipse = self.fellipse(q=q, opt=opt, centre=centre) + dpt = norm_h @ shape.v + dpt *= np.linalg.norm(wTvp - wTcp) / los.length # type: ignore - return self.plot_ellipse( - fellipse, - block=block, - limits=limits, - jointaxes=jointaxes, - eeframe=eeframe, - shadow=shadow, - name=name, - ) + l_Ain = np.zeros((1, self.n + camera_n)) + l_Ain[0, :n_dim] = norm_h @ Je + l_Ain -= dpc + l_bin = (xi * (d - ds) / (di - ds)) + dpt + else: + l_Ain = None + l_bin = None - def plot_vellipse( - self, - q=None, - block=True, - vellipse=None, - limits=None, - opt="trans", - centre=[0, 0, 0], - jointaxes=True, - eeframe=True, - shadow=True, - name=True, - ): - """ - Plot the velocity ellipsoid for manipulator + return l_Ain, l_bin - :param block: Block operation of the code and keep the figure open - :type block: bool - :param q: The joint configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param vellipse: the vellocity ellipsoid to plot - :type vellipse: EllipsePlot - :param limits: Custom view limits for the plot. If not supplied will - autoscale, [x1, x2, y1, y2, z1, z2] - :type limits: ndarray(6) - :param opt: 'trans' or 'rot' will plot either the translational or - rotational velocity ellipsoid - :type opt: string - :param centre: The coordinates to plot the vellipse [x, y, z] or "ee" - to plot at the end-effector location - :type centre: array_like or str - :param jointaxes: (Plot Option) Plot an arrow indicating the axes in - which the joint revolves around(revolute joint) or translates - along (prosmatic joint) - :type jointaxes: bool - :param eeframe: (Plot Option) Plot the end-effector coordinate frame - at the location of the end-effector. Uses three arrows, red, - green and blue to indicate the x, y, and z-axes. - :type eeframe: bool - :param shadow: (Plot Option) Plot a shadow of the robot in the x-y - plane - :type shadow: bool - :param name: (Plot Option) Plot the name of the robot near its base - :type name: bool - - :return: A reference to the PyPlot object which controls the - matplotlib figure - :rtype: PyPlot - - - ``robot.plot_vellipse(q)`` displays the velocity ellipsoid for the - robot at pose ``q``. The plot will autoscale with an aspect ratio - of 1. - - - ``plot_vellipse()`` as above except the robot is plotted with joint - coordinates stored in the robot object. - - - ``robot.plot_vellipse(vellipse)`` specifies a custon ellipse to plot. - - .. note:: - - By default the ellipsoid related to translational motion is - drawn. Use ``opt='rot'`` to draw the rotational velocity - ellipsoid. - - By default the ellipsoid is drawn at the origin. The option - ``centre`` allows its origin to set to set to the specified - 3-vector, or the string "ee" ensures it is drawn at the - end-effector position. - """ + for link in links: + if link.isjoint: + j += 1 - if isinstance(self, rtb.ERobot): # pragma nocover - raise NotImplementedError( - "Ellipse Plotting of ERobot's not implemented yet" - ) + if collision_list is None: + col_list = link.collision + else: + col_list = collision_list[j - 1] - if q is not None: - self.q = q + for link_col in col_list: + l_Ain, l_bin = indiv_calculation(link, link_col, q) - if vellipse is None: - vellipse = self.vellipse(q=q, opt=opt, centre=centre) + if l_Ain is not None and l_bin is not None: + if Ain is None: + Ain = l_Ain + else: + Ain = np.concatenate((Ain, l_Ain)) - return self.plot_ellipse( - vellipse, - block=block, - limits=limits, - jointaxes=jointaxes, - eeframe=eeframe, - shadow=shadow, - name=name, - ) + if bin is None: + bin = np.array(l_bin) + else: + bin = np.concatenate((bin, l_bin)) + + return Ain, bin # --------------------------------------------------------------------- # + # --------- Dynamics Methods ------------------------------------------ # + # --------------------------------------------------------------------- # - def teach( + def rne( self, - q=None, - block=True, - order="xyz", - limits=None, - jointaxes=True, - jointlabels=False, - vellipse=False, - fellipse=False, - eeframe=True, - shadow=True, - name=True, - backend=None, + q: NDArray, + qd: NDArray, + qdd: NDArray, + symbolic: bool = False, + gravity: Union[ArrayLike, None] = None, ): """ - Graphical teach pendant - - :param block: Block operation of the code and keep the figure open - :type block: bool - :param q: The joint configuration of the robot (Optional, - if not supplied will use the stored q values). - :type q: float ndarray(n) - :param limits: Custom view limits for the plot. If not supplied will - autoscale, [x1, x2, y1, y2, z1, z2] - :type limits: ndarray(6) - :param jointaxes: (Plot Option) Plot an arrow indicating the axes in - which the joint revolves around(revolute joint) or - translates along (prismatic joint) - :type jointaxes: bool - :param eeframe: (Plot Option) Plot the end-effector coordinate frame - at the location of the end-effector. Uses three arrows, red, - green and blue to indicate the x, y, and z-axes. - :type eeframe: bool - :param shadow: (Plot Option) Plot a shadow of the robot in the x-y - plane - :type shadow: bool - :param name: (Plot Option) Plot the name of the robot near its base - :type name: bool - - :return: A reference to the PyPlot object which controls the - matplotlib figure - :rtype: PyPlot - - - ``robot.teach(q)`` creates a matplotlib plot which allows the user to - "drive" a graphical robot using a graphical slider panel. The robot's - inital joint configuration is ``q``. The plot will autoscale with an - aspect ratio of 1. - - - ``robot.teach()`` as above except the robot's stored value of ``q`` - is used. - - .. note:: - - Program execution is blocked until the teach window is - dismissed. If ``block=False`` the method is non-blocking but - you need to poll the window manager to ensure that the window - remains responsive. - - The slider limits are derived from the joint limit properties. - If not set then: - - For revolute joints they are assumed to be [-pi, +pi] - - For prismatic joint they are assumed unknown and an error - occurs. - """ + Compute inverse dynamics via recursive Newton-Euler formulation - if q is None: - q = np.zeros((self.n,)) - else: - q = getvector(q, self.n) - - # Make an empty 3D figure - env = self._get_graphical_backend(backend) - - # Add the self to the figure in readonly mode - env.launch("Teach " + self.name, limits=limits) - env.add( - self, - readonly=True, - # jointaxes=jointaxes, - # jointlabels=jointlabels, - # eeframe=eeframe, - # shadow=shadow, - # name=name, - ) + ``rne_dh(q, qd, qdd)`` where the arguments have shape (n,) where n is + the number of robot joints. The result has shape (n,). - env._add_teach_panel(self, q) + ``rne_dh(q, qd, qdd)`` where the arguments have shape (m,n) where n + is the number of robot joints and where m is the number of steps in + the joint trajectory. The result has shape (m,n). - if vellipse: - vell = self.vellipse(centre="ee", scale=0.5) - env.add(vell) + ``rne_dh(p)`` where the input is a 1D array ``p`` = [q, qd, qdd] with + shape (3n,), and the result has shape (n,). - if fellipse: - fell = self.fellipse(centre="ee") - env.add(fell) + ``rne_dh(p)`` where the input is a 2D array ``p`` = [q, qd, qdd] with + shape (m,3n) and the result has shape (m,n). - # Keep the plot open - if block: # pragma: no cover - env.hold() + Parameters + ---------- + q + Joint coordinates + qd + Joint velocity + qdd + Joint acceleration + symbolic + If True, supports symbolic expressions + gravity + Gravitational acceleration, defaults to attribute + of self - return env + Returns + ------- + tau + Joint force/torques - # --------------------------------------------------------------------- # + Notes + ----- + - This version supports symbolic model parameters + - Verified against MATLAB code - def closest_point( - self, q: ArrayLike, shape: Shape, inf_dist: float = 1.0, skip: bool = False - ) -> Tuple[Union[int, None], Union[np.ndarray, None], Union[np.ndarray, None],]: """ - closest_point(shape, inf_dist) returns the minimum euclidean - distance between this robot and shape, provided it is less than - inf_dist. It will also return the points on self and shape in the - world frame which connect the line of length distance between the - shapes. If the distance is negative then the shapes are collided. - :param shape: The shape to compare distance to - :param inf_dist: The minimum distance within which to consider - the shape - :param skip: Skip setting all shape transforms based on q, use this - option if using this method in conjuction with Swift to save time + n = self.n - :returns: d, p1, p2 where d is the distance between the shapes, - p1 and p2 are the points in the world frame on the respective - shapes. The points returned are [x, y, z]. - """ + # allocate intermediate variables + Xup = SE3.Alloc(n) + Xtree = SE3.Alloc(n) - if not skip: - self._update_link_tf(q) - self._propogate_scene_tree() - shape._propogate_scene_tree() + v = SpatialVelocity.Alloc(n) + a = SpatialAcceleration.Alloc(n) + f = SpatialForce.Alloc(n) + I = SpatialInertia.Alloc(n) # noqa + s = [] # joint motion subspace - d = 10000 - p1 = None - p2 = None + # Handle trajectory case + q = getmatrix(q, (None, None)) + qd = getmatrix(qd, (None, None)) + qdd = getmatrix(qdd, (None, None)) + l, _ = q.shape # type: ignore - for link in self.links: - td, tp1, tp2 = link.closest_point(shape, inf_dist, skip=True) + if symbolic: # pragma: nocover + Q = np.empty((l, n), dtype="O") # joint torque/force + else: + Q = np.empty((l, n)) # joint torque/force - if td is not None and td < d: - d = td - p1 = tp1 - p2 = tp2 + # TODO Should the dynamic parameters of static links preceding joint be + # somehow merged with the joint? - if d == 10000: - d = None + # A temp variable to handle static joints + Ts = SE3() - return d, p1, p2 + # A counter through joints + j = 0 - def iscollided(self, q, shape, skip=False): - """ - collided(shape) checks if this robot and shape have collided - :param shape: The shape to compare distance to - :type shape: Shape - :param skip: Skip setting all shape transforms based on q, use this - option if using this method in conjuction with Swift to save time - :type skip: boolean - :returns: True if shapes have collided - :rtype: bool - """ + for k in range(l): + qk = q[k, :] + qdk = qd[k, :] + qddk = qdd[k, :] + + # initialize intermediate variables + for link in self.links: + if link.isjoint: + I[j] = SpatialInertia(m=link.m, r=link.r) + if symbolic and link.Ts is None: # pragma: nocover + Xtree[j] = SE3(np.eye(4, dtype="O"), check=False) + elif link.Ts is not None: + Xtree[j] = Ts * SE3(link.Ts, check=False) + + if link.v is not None: + s.append(link.v.s) + + # Increment the joint counter + j += 1 + + # Reset the Ts tracker + Ts = SE3() + else: # pragma nocover + # TODO Keep track of inertia and transform??? + if link.Ts is not None: + Ts *= SE3(link.Ts, check=False) + + if gravity is None: + a_grav = -SpatialAcceleration(self.gravity) + else: # pragma nocover + a_grav = -SpatialAcceleration(gravity) + + # forward recursion + for j in range(0, n): + vJ = SpatialVelocity(s[j] * qdk[j]) + + # transform from parent(j) to j + Xup[j] = SE3(self.links[j].A(qk[j])).inv() + + if self.links[j].parent is None: + v[j] = vJ + a[j] = Xup[j] * a_grav + SpatialAcceleration(s[j] * qddk[j]) + else: + jp = self.links[j].parent.jindex # type: ignore + v[j] = Xup[j] * v[jp] + vJ + a[j] = ( + Xup[j] * a[jp] + SpatialAcceleration(s[j] * qddk[j]) + v[j] @ vJ + ) - if not skip: - self._update_link_tf(q) - self._propogate_scene_tree() - shape._propogate_scene_tree() + f[j] = I[j] * a[j] + v[j] @ (I[j] * v[j]) - for link in self.links: - if link.iscollided(shape, skip=True): - return True + # backward recursion + for j in reversed(range(0, n)): + # next line could be dot(), but fails for symbolic arguments + Q[k, j] = sum(f[j].A * s[j]) - if isinstance(self, rtb.ERobot): - for gripper in self.grippers: - for link in gripper.links: - if link.iscollided(shape, skip=True): - return True + if self.links[j].parent is not None: + jp = self.links[j].parent.jindex # type: ignore + f[jp] = f[jp] + Xup[j] * f[j] - return False - - def collided(self, q, shape, skip=False): - """ - collided(shape) checks if this robot and shape have collided - :param shape: The shape to compare distance to - :type shape: Shape - :param skip: Skip setting all shape transforms based on q, use this - option if using this method in conjuction with Swift to save time - :type skip: boolean - :returns: True if shapes have collided - :rtype: bool - """ - warn("method collided is deprecated, use iscollided instead", FutureWarning) - return self.iscollided(q, shape, skip=skip) + if l == 1: + return Q[0] + else: # pragma nocover + return Q + + +# ============================================================================= # +# ================= Robot2 Class ============================================== # +# ============================================================================= # + + +class Robot2(BaseRobot[Link2]): + def __init__(self, arg, **kwargs): + if isinstance(arg, ETS2): + # we're passed an ETS string + links = [] + # chop it up into segments, a link frame after every joint + parent = None + for j, ets_j in enumerate(arg.split()): + elink = Link2(ETS2(ets_j), parent=parent, name=f"link{j:d}") + parent = elink + if ( + elink.qlim is None + and elink.v is not None + and elink.v.qlim is not None + ): # pragma nocover + elink.qlim = elink.v.qlim + links.append(elink) + + elif smb.islistof(arg, Link2): + links = arg - def joint_velocity_damper(self, ps=0.05, pi=0.1, n=None, gain=1.0): - """ - Formulates an inequality contraint which, when optimised for will - make it impossible for the robot to run into joint limits. Requires - the joint limits of the robot to be specified. See examples/mmc.py - for use case + else: # pragma nocover + raise TypeError("constructor argument must be ETS2 or list of Link2") - :param ps: The minimum angle (in radians) in which the joint is - allowed to approach to its limit - :type ps: float - :param pi: The influence angle (in radians) in which the velocity - damper becomes active - :type pi: float - :param n: The number of joints to consider. Defaults to all joints - :type n: int - :param gain: The gain for the velocity damper - :type gain: float - - :returns: Ain, Bin as the inequality contraints for an optisator - :rtype: ndarray(6), ndarray(6) - """ + super().__init__(links, **kwargs) - if n is None: - n = self.n + # Should just set it to None + self.base = SE2() # override superclass - Ain = np.zeros((n, n)) - Bin = np.zeros(n) + @property + def base(self) -> SE2: + """ + Get/set robot base transform (Robot superclass) - for i in range(n): - if self.q[i] - self.qlim[0, i] <= pi: - Bin[i] = -gain * (((self.qlim[0, i] - self.q[i]) + ps) / (pi - ps)) - Ain[i, i] = -1 - if self.qlim[1, i] - self.q[i] <= pi: - Bin[i] = gain * ((self.qlim[1, i] - self.q[i]) - ps) / (pi - ps) - Ain[i, i] = 1 + ``robot.base`` is the robot base transform - return Ain, Bin + Returns + ------- + base + robot tool transform - # --------------------------------------------------------------------- # - # Scene Graph section - # --------------------------------------------------------------------- # + - ``robot.base = ...`` checks and sets the robot base transform - def _update_link_tf(self, q: Union[ArrayLike, None] = None): - """ - This private method updates the local transform of each link within - this robot according to q (or self.q if q is none) + Notes + ----- + - The private attribute ``_base`` will be None in the case of + no base transform, but this property will return ``SE3()`` which + is an identity matrix. """ + if self._base is None: # pragma nocover + self._base = SE2() - @lru_cache(maxsize=2) - def get_link_ets(): - return [link.ets._fknm for link in self.links] + # return a copy, otherwise somebody with + # reference to the base can change it + return self._base.copy() - @lru_cache(maxsize=2) - def get_link_scene_node(): - return [link._T_reference for link in self.links] + @base.setter + def base(self, T): + if isinstance(T, SE2): + self._base = T + elif SE2.isvalid(T): # pragma nocover + self._tool = SE2(T, check=True) - Robot_link_T(get_link_ets(), get_link_scene_node(), self._q, q) + def jacob0(self, q, start=None, end=None): + return self.ets(start, end).jacob0(q) - [gripper._update_link_tf() for gripper in self.grippers] + def jacobe(self, q, start=None, end=None): + return self.ets(start, end).jacobe(q) - # --------------------------------------------------------------------- # + def fkine(self, q, end=None, start=None): + return self.ets(start, end).fkine(q) + @property + def reach(self) -> float: + r""" + Reach of the robot + + A conservative estimate of the reach of the robot. It is computed as + the sum of the translational ETs that define the link transform. + + Note + ---- + Computed on the first access. If kinematic parameters + subsequently change this will not be reflected. + + Returns + ------- + reach + Maximum reach of the robot + + Notes + ----- + - Probably an overestimate of reach + - Used by numerical inverse kinematics to scale translational + error. + - For a prismatic joint, uses ``qlim`` if it is set + + """ + + # TODO + # This should be a start, end method and compute the reach based on the + # given ets. Then use an lru_cache to speed up return + + if self._reach is None: + d_all = [] + for link in self.ee_links: + d = 0 + while True: + for et in link.ets: + if et.istranslation: + if et.isjoint: + # the length of a prismatic joint depends on the + # joint limits. They might be set in the ET + # or in the Link depending on how the robot + # was constructed + if link.qlim is not None: + d += max(link.qlim) + elif et.qlim is not None: # pragma nocover + d += max(et.qlim) + else: + d += abs(et.eta) + link = link.parent + if link is None or isinstance(link, str): + d_all.append(d) + break + + self._reach = max(d_all) + return self._reach + + def fkine_all(self, q: ArrayLike) -> SE2: + """ + Compute the pose of every link frame + + ``T = robot.fkine_all(q)`` is an SE3 instance with ``robot.nlinks + + 1`` values: + + - ``T[0]`` is the base transform + - ``T[i]`` is the pose of link whose ``number`` is ``i`` + + Parameters + ---------- + q + The joint configuration + + Returns + ------- + fkine_all + Pose of all links + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). -if __name__ == "__main__": + """ # noqa - pass + q = getvector(q) - # import roboticstoolbox as rtb + Tbase = SE2(self.base) # add base, also sets the type - # puma = rtb.models.DH.Puma560() - # a = puma.copy() + linkframes = Tbase.__class__.Alloc(self.nlinks + 1) + linkframes[0] = Tbase - # from roboticstoolbox import ET2 as ET + def recurse(Tall, Tparent, q, link): + # if joint?? + T = Tparent + while True: + T *= SE2(link.A(q[link.jindex])) - # e = ET.R() * ET.tx(1) * ET.R() * ET.tx(1) - # print(e) - # r = Robot2(e) + Tall[link.number] = T + + if link.nchildren == 0: + # no children + return + elif link.nchildren == 1: + # one child + if link in self.ee_links: # pragma nocover + # this link is an end-effector, go no further + return + link = link.children[0] + continue + else: + # multiple children + for child in link.children: + recurse(Tall, T, q, child) + return - # print(r.fkine([0, 0])) - # print(r.jacob0([0, 0])) + recurse(linkframes, Tbase, q, self.links[0]) - # r.plot([0.7, 0.7]) + return linkframes diff --git a/roboticstoolbox/robot/RobotKinematics.py b/roboticstoolbox/robot/RobotKinematics.py new file mode 100644 index 000000000..4c7a0926b --- /dev/null +++ b/roboticstoolbox/robot/RobotKinematics.py @@ -0,0 +1,1696 @@ +""" +@author: Jesse Haviland +""" + +from roboticstoolbox.robot.RobotProto import KinematicsProtocol +from roboticstoolbox.tools.types import ArrayLike, NDArray +from roboticstoolbox.robot.Link import Link +from roboticstoolbox.robot.Gripper import Gripper +from spatialmath import SE3 +from typing import Union, Tuple, overload +from typing_extensions import Literal as L + + +class RobotKinematicsMixin: + """ + The Robot Kinematics Mixin class + + This class contains kinematic methods for the ``robot`` class. All + methods contained within this class have a full implementation within the + ``ETS`` class and are simply passed through to the ``ETS`` class. + + """ + + # --------------------------------------------------------------------- # + # --------- Kinematic Methods ----------------------------------------- # + # --------------------------------------------------------------------- # + + def fkine( + self: KinematicsProtocol, + q: ArrayLike, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + include_base: bool = True, + ) -> SE3: + """ + Forward kinematics + + ``T = robot.fkine(q)`` evaluates forward kinematics for the robot at + joint configuration ``q``. + + **Trajectory operation**: + If ``q`` has multiple rows (mxn), it is considered a trajectory and the + result is an ``SE3`` instance with ``m`` values. + + Attributes + ---------- + q + Joint coordinates + end + end-effector or gripper to compute forward kinematics to + start + the link to compute forward kinematics from + tool + tool transform, optional + + Returns + ------- + The transformation matrix representing the pose of the + end-effector + + Examples + -------- + The following example makes a ``panda`` robot object, and solves for the + forward kinematics at the listed configuration. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + Notes + ----- + - For a robot with a single end-effector there is no need to + specify ``end`` + - For a robot with multiple end-effectors, the ``end`` must + be specified. + - The robot's base tool transform, if set, is incorporated + into the result. + - A tool transform, if provided, is incorporated into the result. + - Works from the end-effector link to the base + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa + + return SE3( + self.ets(start, end).fkine( + q, base=self._T, tool=tool, include_base=include_base + ), + check=False, + ) + + def jacob0( + self: KinematicsProtocol, + q: ArrayLike, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + r""" + Manipulator geometric Jacobian in the ``start`` frame + + ``robot.jacobo(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + base frame. + + End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` + is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. + + Parameters + ---------- + q + Joint coordinate vector + end + the particular link or gripper whose velocity the Jacobian + describes, defaults to the end-effector if only one is present + start + the link considered as the base frame, defaults to the robots's base frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + J0 + Manipulator Jacobian in the ``start`` frame + + Examples + -------- + The following example makes a ``Puma560`` robot object, and solves for the + base-frame Jacobian at the zero joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.Puma560() + >>> puma.jacob0([0, 0, 0, 0, 0, 0]) + + Notes + ----- + - This is the geometric Jacobian as described in texts by + Corke, Spong etal., Siciliano etal. The end-effector velocity is + described in terms of translational and angular velocity, not a + velocity twist as per the text by Lynch & Park. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + + """ # noqa + + return self.ets(start, end).jacob0(q, tool=tool) + + def jacobe( + self: KinematicsProtocol, + q: ArrayLike, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + r""" + Manipulator geometric Jacobian in the end-effector frame + + ``robot.jacobe(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + ``end`` frame. + + End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` + is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. + + Parameters + ---------- + q + Joint coordinate vector + end + the particular link or gripper whose velocity the Jacobian + describes, defaults to the end-effector if only one is present + start + the link considered as the base frame, defaults to the robots's base frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + Je + Manipulator Jacobian in the ``end`` frame + + Examples + -------- + The following example makes a ``Puma560`` robot object, and solves for the + end-effector frame Jacobian at the zero joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.Puma560() + >>> puma.jacobe([0, 0, 0, 0, 0, 0]) + + Notes + ----- + - This is the geometric Jacobian as described in texts by + Corke, Spong etal., Siciliano etal. The end-effector velocity is + described in terms of translational and angular velocity, not a + velocity twist as per the text by Lynch & Park. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + + """ # noqa + + return self.ets(start, end).jacobe(q, tool=tool) + + @overload + def hessian0( + self: KinematicsProtocol, + q: ArrayLike = ..., + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + J0: None = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: # pragma nocover + ... + + @overload + def hessian0( + self: KinematicsProtocol, + q: None = None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + J0: NDArray = ..., + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: # pragma nocover + ... + + def hessian0( + self: KinematicsProtocol, + q=None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + J0=None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + r""" + Manipulator Hessian + + The manipulator Hessian tensor maps joint acceleration to end-effector + spatial acceleration, expressed in the ``start`` frame. This + function calulcates this based on the ETS of the robot. One of J0 or q + is required. Supply J0 if already calculated to save computation time + + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + end + the final link/Gripper which the Hessian represents + start + the first link which the Hessian represents + J0 + The manipulator Jacobian in the ``start`` frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + h0 + The manipulator Hessian in the ``start`` frame + + Synopsis + -------- + This method computes the manipulator Hessian in the ``start`` frame. If + we take the time derivative of the differential kinematic relationship + .. math:: + \nu &= \mat{J}(\vec{q}) \dvec{q} \\ + \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q} + where + .. math:: + \dmat{J} = \mat{H} \dvec{q} + and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the + Hessian tensor. + + The elements of the Hessian are + .. math:: + \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} + where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements + of the spatial velocity vector. + + Similarly, we can write + .. math:: + \mat{J}_{i,j} = \frac{d u_i}{d q_j} + + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + base frame Hessian at the given joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> panda.hessian0([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa + + return self.ets(start, end).hessian0(q, J0=J0, tool=tool) + + @overload + def hessiane( + self: KinematicsProtocol, + q: ArrayLike = ..., + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + Je: None = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: # pragma nocover + ... + + @overload + def hessiane( + self: KinematicsProtocol, + q: None = None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + Je: NDArray = ..., + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: # pragma nocover + ... + + def hessiane( + self: KinematicsProtocol, + q=None, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + Je=None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + r""" + Manipulator Hessian + + The manipulator Hessian tensor maps joint acceleration to end-effector + spatial acceleration, expressed in the ``end`` coordinate frame. This + function calulcates this based on the ETS of the robot. One of J0 or q + is required. Supply J0 if already calculated to save computation time + + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + end + the final link/Gripper which the Hessian represents + start + the first link which the Hessian represents + J0 + The manipulator Jacobian in the ``end`` frame + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + he + The manipulator Hessian in ``end`` frame + + Synopsis + -------- + This method computes the manipulator Hessian in the ``end`` frame. If + we take the time derivative of the differential kinematic relationship + .. math:: + \nu &= \mat{J}(\vec{q}) \dvec{q} \\ + \alpha &= \dmat{J} \dvec{q} + \mat{J} \ddvec{q} + where + .. math:: + \dmat{J} = \mat{H} \dvec{q} + and :math:`\mat{H} \in \mathbb{R}^{6\times n \times n}` is the + Hessian tensor. + + The elements of the Hessian are + .. math:: + \mat{H}_{i,j,k} = \frac{d^2 u_i}{d q_j d q_k} + where :math:`u = \{t_x, t_y, t_z, r_x, r_y, r_z\}` are the elements + of the spatial velocity vector. + + Similarly, we can write + .. math:: + \mat{J}_{i,j} = \frac{d u_i}{d q_j} + + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + end-effector frame Hessian at the given joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> panda.hessiane([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa + + return self.ets(start, end).hessiane(q, Je=Je, tool=tool) + + def partial_fkine0( + self: KinematicsProtocol, + q: ArrayLike, + n: int = 3, + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + ): + r""" + Manipulator Forward Kinematics nth Partial Derivative + + This method computes the nth derivative of the forward kinematics where ``n`` is + greater than or equal to 3. This is an extension of the differential kinematics + where the Jacobian is the first partial derivative and the Hessian is the + second. + + Parameters + ---------- + q + The joint angles/configuration of the robot (Optional, + if not supplied will use the stored q values). + end + the final link/Gripper which the Hessian represents + start + the first link which the Hessian represents + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + A + The nth Partial Derivative of the forward kinematics + + Examples + -------- + The following example makes a ``Panda`` robot object, and solves for the + base-effector frame 4th defivative of the forward kinematics at the given + joint angle configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> panda.partial_fkine0([0, -0.3, 0, -2.2, 0, 2, 0.7854], n=4) + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + """ # noqa + + return self.ets(start, end).partial_fkine0(q, n=n) + + def jacob0_analytical( + self: KinematicsProtocol, + q: ArrayLike, + representation: L["rpy/xyz", "rpy/zyx", "eul", "exp"] = "rpy/xyz", + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + ): + r""" + Manipulator analytical Jacobian in the ``start`` frame + + ``robot.jacob0_analytical(q)`` is the manipulator Jacobian matrix which maps + joint velocity to end-effector spatial velocity expressed in the + ``start`` frame. + + Parameters + ---------- + q + Joint coordinate vector + representation + angular representation + end + the particular link or gripper whose velocity the Jacobian + describes, defaults to the base link + start + the link considered as the end-effector, defaults to the robots's end-effector + tool + a static tool transformation matrix to apply to the + end of end, defaults to None + + Returns + ------- + jacob0 + Manipulator Jacobian in the ``start`` frame + + Synopsis + -------- + End-effector spatial velocity :math:`\nu = (v_x, v_y, v_z, \omega_x, \omega_y, \omega_z)^T` + is related to joint velocity by :math:`{}^{E}\!\nu = \mathbf{J}_m(q) \dot{q}`. + + |``representation`` | Rotational representation | + |---------------------|-------------------------------------| + |``'rpy/xyz'`` | RPY angular rates in XYZ order | + |``'rpy/zyx'`` | RPY angular rates in XYZ order | + |``'eul'`` | Euler angular rates in ZYZ order | + |``'exp'`` | exponential coordinate rates | + + Examples + -------- + Makes a robot object and computes the analytic Jacobian for the given + joint configuration + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> puma = rtb.models.ETS.Puma560() + >>> puma.jacob0_analytical([0, 0, 0, 0, 0, 0]) + + """ # noqa + + return self.ets(start, end).jacob0_analytical( + q, tool=tool, representation=representation + ) + + # --------------------------------------------------------------------- # + # --------- IK Methods ------------------------------------------------ # + # --------------------------------------------------------------------- # + + def ik_LM( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[NDArray, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + k: float = 1.0, + method: L["chan", "wampler", "sugihara"] = "chan", + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast levenberg-Marquadt Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Levemberg-Marquadt method. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + k + Sets the gain value for the damping matrix Wn in the next iteration. See + synopsis + method + One of "chan", "sugihara" or "wampler". Defines which method is used + to calculate the damping matrix Wn in the ``step`` method + + Synopsis + -------- + The operation is defined by the choice of the ``method`` kwarg. + + The step is deined as + + .. math:: + + \vec{q}_{k+1} + &= + \vec{q}_k + + \left( + \mat{A}_k + \right)^{-1} + \bf{g}_k \\ + % + \mat{A}_k + &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + + + \mat{W}_n + + where :math:`\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})` is a + diagonal damping matrix. The damping matrix ensures that :math:`\mat{A}_k` is + non-singular and positive definite. The performance of the LM method largely depends + on the choice of :math:`\mat{W}_n`. + + *Chan's Method* + + Chan proposed + + .. math:: + + \mat{W}_n + = + λ E_k \mat{1}_n + + where λ is a constant which reportedly does not have much influence on performance. + Use the kwarg `k` to adjust the weighting term λ. + + *Sugihara's Method* + + Sugihara proposed + + .. math:: + + \mat{W}_n + = + E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n) + + where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`, + and :math:`l` is the length of a typical link within the manipulator. We provide the + variable `k` as a kwarg to adjust the value of :math:`w_n`. + + *Wampler's Method* + + Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg. + + Examples + -------- + The following example makes a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_LM` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_LM(Tep) + + Notes + ----- + The value for the ``k`` kwarg will depend on the ``method`` chosen and the arm you are + using. Use the following as a rough guide ``chan, k = 1.0 - 0.01``, + ``wampler, k = 0.01 - 0.0001``, and ``sugihara, k = 0.1 - 0.0001`` + + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + ik_NR + A fast numerical inverse kinematics solver using Newton-Raphson optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation + + + .. versionchanged:: 1.0.4 + Merged the Levemberg-Marquadt IK solvers into the ik_LM method + + """ # noqa + + return self.ets(start, end).ik_LM( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + k=k, + method=method, + ) + + def ik_NR( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[NDArray, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + pinv: int = True, + pinv_damping: float = 0.0, + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast numerical inverse kinematics using Newton-Raphson optimization + + ``sol = ets.ik_NR(Tep)`` are the joint coordinates (n) corresponding + to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. + This method can be used for robots with any number of degrees of freedom. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose or pose trajectory + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + initial joint configuration (default to random valid joint + configuration contrained by the joint limits of the robot) + ilimit + maximum number of iterations per search + slimit + maximum number of search attempts + tol + final error tolerance + mask + a mask vector which weights the end-effector error priority. + Corresponds to translation in X, Y and Z and rotation about X, Y and Z + respectively + joint_limits + constrain the solution to being within the joint limits of + the robot (reject solution with invalid joint configurations and perfrom + another search up to the slimit) + pinv + Use the psuedo-inverse instad of the normal matrix inverse + pinv_damping + Damping factor for the psuedo-inverse + + Returns + ------- + sol + tuple (q, success, iterations, searches, residual) + + The return value ``sol`` is a tuple with elements: + + ============ ========== =============================================== + Element Type Description + ============ ========== =============================================== + ``q`` ndarray(n) joint coordinates in units of radians or metres + ``success`` int whether a solution was found + ``iterations`` int total number of iterations + ``searches`` int total number of searches + ``residual`` float final value of cost function + ============ ========== =============================================== + + If ``success == 0`` the ``q`` values will be valid numbers, but the + solution will be in error. The amount of error is indicated by + the ``residual``. + + Synopsis + -------- + Each iteration uses the Newton-Raphson optimisation method + + .. math:: + + \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k + + Examples + -------- + The following example gets a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ik_NR(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + ik_LM + A fast numerical inverse kinematics solver using Levenberg-Marquadt optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation + + """ # noqa + + return self.ets(start, end).ik_NR( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + pinv=pinv, + pinv_damping=pinv_damping, + ) + + def ik_GN( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[NDArray, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[NDArray, None] = None, + joint_limits: bool = True, + pinv: int = True, + pinv_damping: float = 0.0, + ) -> Tuple[NDArray, int, int, int, float]: + r""" + Fast numerical inverse kinematics by Gauss-Newton optimization + + ``sol = ets.ik_GN(Tep)`` are the joint coordinates (n) corresponding + to the robot end-effector pose ``Tep`` which is an ``SE3`` or ``ndarray`` object. + This method can be used for robots with any number of degrees of freedom. This + is a fast solver implemented in C++. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose or pose trajectory + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + initial joint configuration (default to random valid joint + configuration contrained by the joint limits of the robot) + ilimit + maximum number of iterations per search + slimit + maximum number of search attempts + tol + final error tolerance + mask + a mask vector which weights the end-effector error priority. + Corresponds to translation in X, Y and Z and rotation about X, Y and Z + respectively + joint_limits + constrain the solution to being within the joint limits of + the robot (reject solution with invalid joint configurations and perfrom + another search up to the slimit) + pinv + Use the psuedo-inverse instad of the normal matrix inverse + pinv_damping + Damping factor for the psuedo-inverse + + Returns + ------- + sol + tuple (q, success, iterations, searches, residual) + + The return value ``sol`` is a tuple with elements: + + ============ ========== =============================================== + Element Type Description + ============ ========== =============================================== + ``q`` ndarray(n) joint coordinates in units of radians or metres + ``success`` int whether a solution was found + ``iterations`` int total number of iterations + ``searches`` int total number of searches + ``residual`` float final value of cost function + ============ ========== =============================================== + + If ``success == 0`` the ``q`` values will be valid numbers, but the + solution will be in error. The amount of error is indicated by + the ``residual``. + + Synopsis + -------- + Each iteration uses the Gauss-Newton optimisation method + + .. math:: + + \vec{q}_{k+1} &= \vec{q}_k + + \left( + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + \right)^{-1} + \bf{g}_k \\ + \bf{g}_k &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e + \vec{e}_k + + where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If + :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then + the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)` + is singular, the above can not be computed and the GN solution is infeasible. + + Examples + -------- + The following example gets a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ik_GN(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + ik_NR + A fast numerical inverse kinematics solver using Newton-Raphson optimisation + ik_GN + A fast numerical inverse kinematics solver using Gauss-Newton optimisation + + """ # noqa + + return self.ets(start, end).ik_GN( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + pinv=pinv, + pinv_damping=pinv_damping, + ) + + def ikine_LM( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[ArrayLike, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + k: float = 1.0, + method: L["chan", "wampler", "sugihara"] = "chan", + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Levenberg-Marquadt Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Levemberg-Marquadt method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + k + Sets the gain value for the damping matrix Wn in the next iteration. See + synopsis + method + One of "chan", "sugihara" or "wampler". Defines which method is used + to calculate the damping matrix Wn in the ``step`` method + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + The operation is defined by the choice of the ``method`` kwarg. + + The step is deined as + + .. math:: + + \vec{q}_{k+1} + &= + \vec{q}_k + + \left( + \mat{A}_k + \right)^{-1} + \bf{g}_k \\ + % + \mat{A}_k + &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + + + \mat{W}_n + + where :math:`\mat{W}_n = \text{diag}(\vec{w_n})(\vec{w_n} \in \mathbb{R}^n_{>0})` is a + diagonal damping matrix. The damping matrix ensures that :math:`\mat{A}_k` is + non-singular and positive definite. The performance of the LM method largely depends + on the choice of :math:`\mat{W}_n`. + + *Chan's Method* + + Chan proposed + + .. math:: + + \mat{W}_n + = + λ E_k \mat{1}_n + + where λ is a constant which reportedly does not have much influence on performance. + Use the kwarg `k` to adjust the weighting term λ. + + *Sugihara's Method* + + Sugihara proposed + + .. math:: + + \mat{W}_n + = + E_k \mat{1}_n + \text{diag}(\hat{\vec{w}}_n) + + where :math:`\hat{\vec{w}}_n \in \mathbb{R}^n`, :math:`\hat{w}_{n_i} = l^2 \sim 0.01 l^2`, + and :math:`l` is the length of a typical link within the manipulator. We provide the + variable `k` as a kwarg to adjust the value of :math:`w_n`. + + *Wampler's Method* + + Wampler proposed :math:`\vec{w_n}` to be a constant. This is set through the `k` kwarg. + + Examples + -------- + The following example makes a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_LM` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_LM(Tep) + + Notes + ----- + The value for the ``k`` kwarg will depend on the ``method`` chosen and the arm you are + using. Use the following as a rough guide ``chan, k = 1.0 - 0.01``, + ``wampler, k = 0.01 - 0.0001``, and ``sugihara, k = 0.1 - 0.0001`` + + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_LM` + An IK Solver class which implements the Levemberg Marquadt optimisation technique + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`Robot` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`Robot` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`Robot` class + + + .. versionchanged:: 1.0.4 + Added the Levemberg-Marquadt IK solver method on the `Robot` class + + """ # noqa + + return self.ets(start, end).ikine_LM( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + k=k, + method=method, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) + + def ikine_NR( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[ArrayLike, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Newton-Raphson Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Newton-Raphson method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + Each iteration uses the Newton-Raphson optimisation method + + .. math:: + + \vec{q}_{k+1} = \vec{q}_k + {^0\mat{J}(\vec{q}_k)}^{-1} \vec{e}_k + + Examples + -------- + The following example gets a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_NR` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_NR(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`ETS` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Newton-Raphson IK solver method on the `Robot` class + + """ # noqa + + return self.ets(start, end).ikine_NR( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + pinv=pinv, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) + + def ikine_GN( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[ArrayLike, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + pinv: bool = False, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Gauss-Newton Numerical Inverse Kinematics Solver + + A method which provides functionality to perform numerical inverse kinematics (IK) + using the Gauss-Newton method. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Note + ---- + When using this method with redundant robots (>6 DoF), ``pinv`` must be set to ``True`` + + Parameters + ---------- + Tep + The desired end-effector pose + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + pinv + If True, will use the psuedoinverse in the `step` method instead of + the normal inverse + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Synopsis + -------- + Each iteration uses the Gauss-Newton optimisation method + + .. math:: + + \vec{q}_{k+1} &= \vec{q}_k + + \left( + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e \ + {\mat{J}(\vec{q}_k)} + \right)^{-1} + \bf{g}_k \\ + \bf{g}_k &= + {\mat{J}(\vec{q}_k)}^\top + \mat{W}_e + \vec{e}_k + + where :math:`\mat{J} = {^0\mat{J}}` is the base-frame manipulator Jacobian. If + :math:`\mat{J}(\vec{q}_k)` is non-singular, and :math:`\mat{W}_e = \mat{1}_n`, then + the above provides the pseudoinverse solution. However, if :math:`\mat{J}(\vec{q}_k)` + is singular, the above can not be computed and the GN solution is infeasible. + + Examples + -------- + The following example gets a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_GN` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_GN(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`ETS` class + ikine_QP + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_QP` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Gauss-Newton IK solver method on the `Robot` class + + """ # noqa + + return self.ets(start, end).ikine_GN( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + pinv=pinv, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) + + def ikine_QP( + self: KinematicsProtocol, + Tep: Union[NDArray, SE3], + end: Union[str, Link, Gripper, None] = None, + start: Union[str, Link, Gripper, None] = None, + q0: Union[ArrayLike, None] = None, + ilimit: int = 30, + slimit: int = 100, + tol: float = 1e-6, + mask: Union[ArrayLike, None] = None, + joint_limits: bool = True, + seed: Union[int, None] = None, + kj=1.0, + ks=1.0, + kq: float = 0.0, + km: float = 0.0, + ps: float = 0.0, + pi: Union[NDArray, float] = 0.3, + **kwargs, + ): + r""" + Quadratic Programming Numerical Inverse Kinematics Solver + + A method that provides functionality to perform numerical inverse kinematics + (IK) using a quadratic progamming approach. + + See the :ref:`Inverse Kinematics Docs Page ` for more details and for a + **tutorial** on numerical IK, see `here `_. + + Parameters + ---------- + Tep + The desired end-effector pose + end + the link considered as the end-effector + start + the link considered as the base frame, defaults to the robots's base frame + q0 + The initial joint coordinate vector + ilimit + How many iterations are allowed within a search before a new search + is started + slimit + How many searches are allowed before being deemed unsuccessful + tol + Maximum allowed residual error E + mask + A 6 vector which assigns weights to Cartesian degrees-of-freedom + error priority + joint_limits + Reject solutions with joint limit violations + seed + A seed for the private RNG used to generate random joint coordinate + vectors + kj + A gain for joint velocity norm minimisation + ks + A gain which adjusts the cost of slack (intentional error) + kq + The gain for joint limit avoidance. Setting to 0.0 will remove this + completely from the solution + km + The gain for maximisation. Setting to 0.0 will remove this completely + from the solution + ps + The minimum angle/distance (in radians or metres) in which the joint is + allowed to approach to its limit + pi + The influence angle/distance (in radians or metres) in null space motion + becomes active + + Raises + ------ + ImportError + If the package ``qpsolvers`` is not installed + + Synopsis + -------- + Each iteration uses the following approach + + .. math:: + + \vec{q}_{k+1} = \vec{q}_{k} + \dot{\vec{q}}. + + where the QP is defined as + + .. math:: + + \min_x \quad f_o(\vec{x}) &= \frac{1}{2} \vec{x}^\top \mathcal{Q} \vec{x}+ \mathcal{C}^\top \vec{x}, \\ + \text{subject to} \quad \mathcal{J} \vec{x} &= \vec{\nu}, \\ + \mathcal{A} \vec{x} &\leq \mathcal{B}, \\ + \vec{x}^- &\leq \vec{x} \leq \vec{x}^+ + + with + + .. math:: + + \vec{x} &= + \begin{pmatrix} + \dvec{q} \\ \vec{\delta} + \end{pmatrix} \in \mathbb{R}^{(n+6)} \\ + \mathcal{Q} &= + \begin{pmatrix} + \lambda_q \mat{1}_{n} & \mathbf{0}_{6 \times 6} \\ \mathbf{0}_{n \times n} & \lambda_\delta \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{(n+6) \times (n+6)} \\ + \mathcal{J} &= + \begin{pmatrix} + \mat{J}(\vec{q}) & \mat{1}_{6} + \end{pmatrix} \in \mathbb{R}^{6 \times (n+6)} \\ + \mathcal{C} &= + \begin{pmatrix} + \mat{J}_m \\ \bf{0}_{6 \times 1} + \end{pmatrix} \in \mathbb{R}^{(n + 6)} \\ + \mathcal{A} &= + \begin{pmatrix} + \mat{1}_{n \times n + 6} \\ + \end{pmatrix} \in \mathbb{R}^{(l + n) \times (n + 6)} \\ + \mathcal{B} &= + \eta + \begin{pmatrix} + \frac{\rho_0 - \rho_s} + {\rho_i - \rho_s} \\ + \vdots \\ + \frac{\rho_n - \rho_s} + {\rho_i - \rho_s} + \end{pmatrix} \in \mathbb{R}^{n} \\ + \vec{x}^{-, +} &= + \begin{pmatrix} + \dvec{q}^{-, +} \\ + \vec{\delta}^{-, +} + \end{pmatrix} \in \mathbb{R}^{(n+6)}, + + where :math:`\vec{\delta} \in \mathbb{R}^6` is the slack vector, + :math:`\lambda_\delta \in \mathbb{R}^+` is a gain term which adjusts the + cost of the norm of the slack vector in the optimiser, + :math:`\dvec{q}^{-,+}` are the minimum and maximum joint velocities, and + :math:`\dvec{\delta}^{-,+}` are the minimum and maximum slack velocities. + + Examples + -------- + The following example gets a ``panda`` robot object, makes a goal + pose ``Tep``, and then solves for the joint coordinates which result in the pose + ``Tep`` using the `ikine_QP` method. + + .. runblock:: pycon + >>> import roboticstoolbox as rtb + >>> panda = rtb.models.Panda() + >>> Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2, 0.7854]) + >>> panda.ikine_QP(Tep) + + Notes + ----- + When using the this method, the initial joint coordinates :math:`q_0`, should correspond + to a non-singular manipulator pose, since it uses the manipulator Jacobian. + + This class supports null-space motion to assist with maximising manipulability and + avoiding joint limits. These are enabled by setting kq and km to non-zero values. + + References + ---------- + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part I: + Kinematics, Velocity, and Applications." arXiv preprint arXiv:2207.01796 (2022). + - J. Haviland, and P. Corke. "Manipulator Differential Kinematics Part II: + Acceleration and Advanced Applications." arXiv preprint arXiv:2207.01794 (2022). + + See Also + -------- + :py:class:`~roboticstoolbox.robot.IK.IK_NR` + An IK Solver class which implements the Newton-Raphson optimisation technique + ikine_LM + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_LM` class as a method within the :py:class:`ETS` class + ikine_GN + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_GN` class as a method within the :py:class:`ETS` class + ikine_NR + Implements the :py:class:`~roboticstoolbox.robot.IK.IK_NR` class as a method within the :py:class:`ETS` class + + + .. versionchanged:: 1.0.4 + Added the Quadratic Programming IK solver method on the `Robot` class + + """ # noqa: E501 + + return self.ets(start, end).ikine_QP( + Tep=Tep, + q0=q0, + ilimit=ilimit, + slimit=slimit, + tol=tol, + joint_limits=joint_limits, + mask=mask, + seed=seed, + ks=ks, + kj=kj, + kq=kq, + km=km, + ps=ps, + pi=pi, + **kwargs, + ) diff --git a/roboticstoolbox/robot/RobotProto.py b/roboticstoolbox/robot/RobotProto.py new file mode 100644 index 000000000..0f9768c53 --- /dev/null +++ b/roboticstoolbox/robot/RobotProto.py @@ -0,0 +1,113 @@ +import numpy as np +from roboticstoolbox.tools.types import ArrayLike, NDArray + +from typing import Any, Callable, Generic, List, Set, TypeVar, Union, Dict, Tuple, Type +from typing_extensions import Protocol, Self + +from roboticstoolbox.robot.Link import Link, BaseLink +from roboticstoolbox.robot.Gripper import Gripper +from roboticstoolbox.robot.ETS import ETS +from spatialmath import SE3 +import roboticstoolbox as rtb + + +class KinematicsProtocol(Protocol): + @property + def _T(self) -> NDArray: + ... + + def ets( + self, + start: Union[Link, Gripper, str, None] = None, + end: Union[Link, Gripper, str, None] = None, + ) -> ETS: + ... + + +class RobotProto(Protocol): + @property + def links(self) -> List[BaseLink]: + ... + + @property + def n(self) -> int: + ... + + @property + def q(self) -> NDArray: + ... + + @property + def name(self) -> str: + ... + + @name.setter + def name(self, new_name: str): + ... + + @property + def gravity(self) -> NDArray: + ... + + def dynchanged(self): + ... + + def jacobe( + self, + q: ArrayLike, + end: Union[str, BaseLink, Gripper, None] = None, + start: Union[str, BaseLink, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + ... + + def jacob0( + self, + q: ArrayLike, + end: Union[str, BaseLink, Gripper, None] = None, + start: Union[str, BaseLink, Gripper, None] = None, + tool: Union[NDArray, SE3, None] = None, + ) -> NDArray: + ... + + def copy(self) -> Self: + ... + + def accel(self, q, qd, torque, gravity=None) -> NDArray: + ... + + def nofriction(self, coulomb: bool, viscous: bool) -> Self: + ... + + def _fdyn( + self, + t: float, + x: NDArray, + torqfun: Callable[[Any], NDArray], + targs: Dict, + ) -> NDArray: + ... + + def rne( + self, + q: NDArray, + qd: NDArray, + qdd: NDArray, + symbolic: bool = False, + gravity: Union[None, ArrayLike] = None, + ) -> NDArray: + ... + + def gravload( + self, q: Union[None, ArrayLike] = None, gravity: Union[None, ArrayLike] = None + ): + ... + + def pay( + self, + W: ArrayLike, + q: Union[NDArray, None] = None, + J: Union[NDArray, None] = None, + frame: int = 1, + ): + ... diff --git a/roboticstoolbox/robot/__init__.py b/roboticstoolbox/robot/__init__.py index 3be2454f1..ecf0b51f2 100644 --- a/roboticstoolbox/robot/__init__.py +++ b/roboticstoolbox/robot/__init__.py @@ -1,4 +1,4 @@ -from roboticstoolbox.robot.Robot import Robot +from roboticstoolbox.robot.Robot import BaseRobot, Robot, Robot2 from roboticstoolbox.robot.Link import Link, Link2 from roboticstoolbox.robot.DHRobot import SerialLink, DHRobot from roboticstoolbox.robot.DHLink import ( @@ -8,23 +8,19 @@ RevoluteMDH, PrismaticMDH, ) -from roboticstoolbox.robot.PoERobot import ( - PoELink, - PoERobot, - PoERevolute, - PoEPrismatic -) +from roboticstoolbox.robot.PoERobot import PoELink, PoERobot, PoERevolute, PoEPrismatic from roboticstoolbox.robot.ERobot import ERobot, ERobot2 -# from roboticstoolbox.robot.FastRobot import FastRobot from roboticstoolbox.robot.ELink import ELink, ELink2 from roboticstoolbox.robot.ETS import ETS, ETS2 from roboticstoolbox.robot.Gripper import Gripper -# from roboticstoolbox.robot.KinematicCache import KinematicCache from roboticstoolbox.robot.ET import ET, ET2 +from roboticstoolbox.robot.IK import IKSolution, IKSolver, IK_LM, IK_NR, IK_GN, IK_QP + __all__ = [ "Robot", + "Robot2", "SerialLink", "DHRobot", "Link", @@ -33,22 +29,26 @@ "PrismaticDH", "RevoluteMDH", "PrismaticMDH", - "ERobot", + "BaseRobot", "ELink", "ELink2", "Link", "Link2", "ERobot", "ERobot2", - # "FastRobot", "ETS", "ETS2", "Gripper", - # "KinematicCache", "PoERobot", "PoELink", "PoEPrismatic", "PoERevolute", "ET", "ET2", + "IKSolution", + "IKSolver", + "IK_LM", + "IK_NR", + "IK_GN", + "IK_QP", ] diff --git a/roboticstoolbox/tools/__init__.py b/roboticstoolbox/tools/__init__.py index e943b3051..54e9da371 100644 --- a/roboticstoolbox/tools/__init__.py +++ b/roboticstoolbox/tools/__init__.py @@ -1,5 +1,5 @@ from roboticstoolbox.tools.null import null -from roboticstoolbox.tools.p_servo import p_servo, angle_axis +from roboticstoolbox.tools.p_servo import p_servo, angle_axis, angle_axis_python from roboticstoolbox.tools.Ticker import Ticker from roboticstoolbox.tools.urdf import * # noqa from roboticstoolbox.tools.trajectory import ( @@ -22,10 +22,12 @@ ) from roboticstoolbox.tools.plot import xplot from roboticstoolbox.tools.params import rtb_set_param, rtb_get_param +from roboticstoolbox.tools.types import ArrayLike, NDArray, PyArrayLike __all__ = [ "null", "p_servo", + "angle_axis_python", "angle_axis", "Ticker", "quintic", @@ -46,4 +48,7 @@ "rtb_path_to_datafile", "rtb_set_param", "rtb_get_param", + "PyArrayLike", + "ArrayLike", + "NDArray", ] diff --git a/roboticstoolbox/tools/data.py b/roboticstoolbox/tools/data.py index d3e168b6c..a7f25716a 100644 --- a/roboticstoolbox/tools/data.py +++ b/roboticstoolbox/tools/data.py @@ -103,7 +103,7 @@ def rtb_load_data(filename, handler, **kwargs): :seealso: :func:`path_to_datafile` """ - path = rtb_path_to_datafile(filename) + path = rtb_path_to_datafile(filename, local=False) return handler(path, **kwargs) def rtb_path_to_datafile(*filename, local=True): diff --git a/roboticstoolbox/tools/p_servo.py b/roboticstoolbox/tools/p_servo.py index e1b785464..d0c3e24ec 100644 --- a/roboticstoolbox/tools/p_servo.py +++ b/roboticstoolbox/tools/p_servo.py @@ -4,11 +4,22 @@ from spatialmath import SE3, base import math from typing import Union +from roboticstoolbox.fknm import Angle_Axis ArrayLike = Union[list, np.ndarray, tuple, set] def angle_axis(T, Td): + + try: + e = Angle_Axis(T, Td) + except BaseException: + e = angle_axis_python(T, Td) + + return e + + +def angle_axis_python(T, Td): e = np.empty(6) e[:3] = Td[:3, -1] - T[:3, -1] R = Td[:3, :3] @ T[:3, :3].T diff --git a/roboticstoolbox/tools/plot.py b/roboticstoolbox/tools/plot.py index c201672a4..5c3f2f074 100644 --- a/roboticstoolbox/tools/plot.py +++ b/roboticstoolbox/tools/plot.py @@ -1,6 +1,7 @@ import numpy as np import matplotlib.pyplot as plt + def xplot( x, y=None, @@ -101,7 +102,7 @@ def xplot( plt.grid(grid) ax.set_xlabel("Time (s)") ax.set_ylabel("Joint coordinates (rad,m)") - ax.set_xlim(t[0], t[-1]) + # ax.set_xlim(t[0], t[-1]) fails with RVC3 Sec 3.3.3 plt.show(block=block) diff --git a/roboticstoolbox/tools/trajectory.py b/roboticstoolbox/tools/trajectory.py index 380020765..1cff59ad7 100644 --- a/roboticstoolbox/tools/trajectory.py +++ b/roboticstoolbox/tools/trajectory.py @@ -48,6 +48,10 @@ def __init__(self, name, t, s, sd=None, sdd=None, istime=False): with respect to ``t``. .. note:: Data is stored with timesteps as rows and axes as columns. + + :References: + + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. """ self.name = name self.t = t @@ -57,7 +61,10 @@ def __init__(self, name, t, s, sd=None, sdd=None, istime=False): self.istime = istime def __str__(self): - s = f"Trajectory created by {self.name}: {len(self)} time steps x {self.naxes} axes" + s = ( + f"Trajectory created by {self.name}: {len(self)} time steps x" + f" {self.naxes} axes" + ) return s def __repr__(self): @@ -162,11 +169,15 @@ def plot(self, block=False, plotargs=None, textargs=None): if textargs is not None: textopts = {**textopts, **textargs} + nplots = 3 + if self.name == "mstraj": + nplots = 1 + plt.figure() - ax = plt.subplot(3, 1, 1) + ax = plt.subplot(nplots, 1, 1) # plot position - if self.name == "quintic": + if self.name in "quintic": ax.plot(self.t, self.s, **plotopts) elif self.name == "trapezoidal": @@ -214,29 +225,30 @@ def plot(self, block=False, plotargs=None, textargs=None): else: ax.set_ylabel("$q(k)$", **textopts) - # plot velocity - ax = plt.subplot(3, 1, 2) - ax.plot(self.t, self.sd, **plotopts) - ax.grid(True) - ax.set_xlim(0, max(self.t)) + if nplots > 1: + # plot velocity + ax = plt.subplot(3, 1, 2) + ax.plot(self.t, self.sd, **plotopts) + ax.grid(True) + ax.set_xlim(0, max(self.t)) - if self.istime: - ax.set_ylabel("$\dot{{q}}(t)$", **textopts) - else: - ax.set_ylabel("$dq/dk$", **textopts) + if self.istime: + ax.set_ylabel("$\dot{{q}}(t)$", **textopts) + else: + ax.set_ylabel("$dq/dk$", **textopts) - # plot acceleration - ax = plt.subplot(3, 1, 3) - ax.plot(self.t, self.sdd, **plotopts) - ax.grid(True) - ax.set_xlim(0, max(self.t)) + # plot acceleration + ax = plt.subplot(3, 1, 3) + ax.plot(self.t, self.sdd, **plotopts) + ax.grid(True) + ax.set_xlim(0, max(self.t)) - if self.istime: - ax.set_ylabel(f"$\ddot{{q}}(t)$", **textopts) - ax.set_xlabel("t (seconds)") - else: - ax.set_ylabel("$d^2q/dk^2$", **textopts) - ax.set_xlabel("k (step)") + if self.istime: + ax.set_ylabel(f"$\ddot{{q}}(t)$", **textopts) + ax.set_xlabel("t (seconds)") + else: + ax.set_ylabel("$d^2q/dk^2$", **textopts) + ax.set_xlabel("k (step)") plt.show(block=block) @@ -253,6 +265,9 @@ def qplot(self, **kwargs): xplot(self.t, self.q, **kwargs) +# -------------------------------------------------------------------------- # + + def quintic(q0, qf, t, qd0=0, qdf=0): """ Generate scalar polynomial trajectory @@ -268,7 +283,7 @@ def quintic(q0, qf, t, qd0=0, qdf=0): :param qdf: final velocity, optional :type q0: float :return: trajectory - :rtype: Trajectory instance + :rtype: :class:`Trajectory` instance - ``tg = quintic(q0, q1, m)`` is a scalar trajectory (Mx1) that varies smoothly from ``q0`` to ``qf`` using a quintic polynomial. The initial @@ -289,13 +304,31 @@ def quintic(q0, qf, t, qd0=0, qdf=0): The return value is an object that contains position, velocity and acceleration data. + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import quintic + >>> tg = quintic(1, 2, 10) + >>> tg + >>> len(tg) + >>> tg.q + >>> tg.plot() + + .. plot:: + + from roboticstoolbox import quintic + tg = quintic(1, 2, 10) + tg.plot() + + .. note:: The time vector T is assumed to be monotonically increasing, and time scaling is based on the first and last element. - References: + :References: + + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. - - Robotics, Vision & Control, Chap 3, - P. Corke, Springer 2011. :seealso: :func:`trapezoidal`, :func:`mtraj`. """ @@ -321,16 +354,49 @@ def quintic(q0, qf, t, qd0=0, qdf=0): def quintic_func(q0, qf, T, qd0=0, qdf=0): + """ + Quintic scalar polynomial as a function + + :param q0: initial value + :type q0: float + :param qf: final value + :type qf: float + :param T: trajectory time + :type T: float + :param qd0: initial velocity, defaults to 0 + :type q0: float, optional + :param qdf: final velocity, defaults to 0 + :type q0: float, optional + :return: polynomial function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` + :rtype: callable + + Returns a function which computes the specific quintic polynomial, and its + derivatives, as described by the parameters. + + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import quintic_func + >>> f = quintic_func(1, 2, 5) + >>> f(0) + >>> f(5) + >>> f(2.5) + + :seealso: :func:`quintic` :func:`trapezoidal_func` + """ # solve for the polynomial coefficients using least squares + # fmt: off X = [ - [0, 0, 0, 0, 0, 1], - [T**5, T**4, T**3, T**2, T, 1], - [0, 0, 0, 0, 1, 0], - [5 * T**4, 4 * T**3, 3 * T**2, 2 * T, 1, 0], - [0, 0, 0, 2, 0, 0], - [20 * T**3, 12 * T**2, 6 * T, 2, 0, 0], + [ 0.0, 0.0, 0.0, 0.0, 0.0, 1.0], + [ T**5, T**4, T**3, T**2, T, 1.0], + [ 0.0, 0.0, 0.0, 0.0, 1.0, 0.0], + [ 5.0 * T**4, 4.0 * T**3, 3.0 * T**2, 2.0 * T, 1.0, 0.0], + [ 0.0, 0.0, 0.0, 2.0, 0.0, 0.0], + [20.0 * T**3, 12.0 * T**2, 6.0 * T, 2.0, 0.0, 0.0], ] + # fmt: on coeffs, resid, rank, s = np.linalg.lstsq( X, np.r_[q0, qf, qd0, qdf, 0, 0], rcond=None ) @@ -350,10 +416,16 @@ def quintic_func(q0, qf, T, qd0=0, qdf=0): def lspb(*args, **kwargs): + """ + .. warning:: Deprecated, use ``trapezoidal`` instead. + """ warnings.warn("lsp is deprecated, use trapezoidal", FutureWarning) return trapezoidal(*args, **kwargs) +# -------------------------------------------------------------------------- # + + def trapezoidal(q0, qf, t, V=None): """ Scalar trapezoidal trajectory @@ -367,7 +439,7 @@ def trapezoidal(q0, qf, t, V=None): :param V: velocity of linear segment, optional :type V: float :return: trajectory - :rtype: Trajectory instance + :rtype: :class:`Trajectory` instance Computes a trapezoidal trajectory, which has a linear motion segment with parabolic blends. @@ -393,19 +465,34 @@ def trapezoidal(q0, qf, t, V=None): The return value is an object that contains position, velocity and acceleration data. + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import trapezoidal + >>> tg = trapezoidal(1, 2, 10) + >>> tg + >>> len(tg) + >>> tg.q + + .. plot:: + + from roboticstoolbox import trapezoidal + tg = trapezoidal(1, 2, 10) + tg.plot() + .. note:: - - For some values of V no solution is possible and an error is flagged. + - For some values of ``V`` no solution is possible and an error is flagged. - The time vector, if given, is assumed to be monotonically increasing, and time scaling is based on the first and last element. - ``tg`` has an extra attribute ``xblend`` which is the blend duration. :References: - - Robotics, Vision & Control, Chap 3, - P. Corke, Springer 2011. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. - :seealso: :func:`quintic`, :func:`mtraj`. + :seealso: :func:`quintic`, :func:`trapezoidal_func` :func:`mtraj`. """ if isinstance(t, int): @@ -432,27 +519,51 @@ def trapezoidal(q0, qf, t, V=None): return traj -def trapezoidal_func(q0, qf, tf, V=None): +def trapezoidal_func(q0, qf, T, V=None): + r""" + Trapezoidal scalar profile as a function + + :param q0: initial value + :type q0: float + :param qf: final value + :type qf: float + :param T: maximum time + :type T: float + :param V: velocity of linear segment + :type V: float, optional + :return: trapezoidal profile function :math:`f: t \mapsto (q(t), \dot{q}(t), \ddot{q}(t))` + :rtype: callable + + Returns a function which computes the specific trapezoidal profile, and its + derivatives, as described by the parameters. + + Example: + + .. runblock:: pycon + + >>> from roboticstoolbox import trapezoidal_func + >>> f = trapezoidal_func(1, 2, 5) + >>> f(0) + >>> f(5) + >>> f(2.5) + """ if V is None: # if velocity not specified, compute it - V = (qf - q0) / tf * 1.5 + V = (qf - q0) / T * 1.5 else: V = abs(V) * np.sign(qf - q0) - if abs(V) < (abs(qf - q0) / tf): + if abs(V) < (abs(qf - q0) / T): raise ValueError("V too small") - elif abs(V) > (2 * abs(qf - q0) / tf): + elif abs(V) > (2 * abs(qf - q0) / T): raise ValueError("V too big") - if q0 == qf: - # Commented these because they arent used anywhere - # s = np.ones((len(t), len(t))) @ q0 - # sd = np.zeros((len(t), len(t))) - # sdd = np.zeros((len(t), len(t))) - return - - tb = (q0 - qf + V * tf) / V - a = V / tb + if V == 0: + tb = np.inf + a = 0 + else: + tb = (q0 - qf + V * T) / V + a = V / tb def trapezoidalfunc(t): @@ -472,15 +583,15 @@ def trapezoidalfunc(t): pk = q0 + a / 2 * tk**2 pdk = a * tk pddk = a - elif tk <= (tf - tb): + elif tk <= (T - tb): # linear motion - pk = (qf + q0 - V * tf) / 2 + V * tk + pk = (qf + q0 - V * T) / 2 + V * tk pdk = V pddk = 0 - elif tk <= tf: + elif tk <= T: # final blend - pk = qf - a / 2 * tf**2 + a * tf * tk - a / 2 * tk**2 - pdk = a * tf - a * tk + pk = qf - a / 2 * T**2 + a * T * tk - a / 2 * tk**2 + pdk = a * T - a * tk pddk = -a else: pk = qf @@ -503,6 +614,75 @@ def trapezoidalfunc(t): # -------------------------------------------------------------------------- # +def mtraj(tfunc, q0, qf, t): + """ + Multi-axis trajectory + + :param tfunc: a 1D trajectory function, eg. :func:`quintic` or :func:`trapezoidal` + :type tfunc: callable + :param q0: initial configuration + :type q0: ndarray(m) + :param qf: final configuration + :type qf: ndarray(m) + :param t: time vector or number of steps + :type t: array_like or int + :raises TypeError: ``tfunc`` is not callable + :raises ValueError: length of ``q0`` and ``qf`` are different + :return: trajectory + :rtype: :class:`Trajectory` instance + + - ``tg = mtraj(func, q0, qf, n)`` is a multi-axis trajectory varying + from configuration ``q0`` (M) to ``qf`` (M) according to the scalar trajectory + function ``tfunc`` in ``n`` steps. + + - ``tg = mtraj(func, q0, qf, t)`` as above but ``t`` is a uniformly-spaced time + vector + + The scalar trajectory function is applied to each axis:: + + tg = tfunc(s0, sF, n) + + and possible values of ``tfunc`` include ``trapezoidal`` for a trapezoidal trajectory, or + ``quintic`` for a polynomial trajectory. + + The return value is an object that contains position, velocity and + acceleration data. + + .. note:: The time vector, if given, is assumed to be monotonically increasing, and + time scaling is based on the first and last element. + + :References: + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. + + :seealso: :func:`quintic`, :func:`trapezoidal` + """ + + if not callable(tfunc): + raise TypeError("first argument must be a function reference") + + q0 = getvector(q0) + qf = getvector(qf) + if len(q0) != len(qf): + raise ValueError("must be same number of elements in q0 and qf") + + traj = [] + for i in range(len(q0)): + # for each axis + traj.append(tfunc(q0[i], qf[i], t)) + + x = traj[0].t + y = np.array([tg.s for tg in traj]).T + yd = np.array([tg.sd for tg in traj]).T + ydd = np.array([tg.sdd for tg in traj]).T + + istime = traj[0].istime + + return Trajectory("mtraj", x, y, yd, ydd, istime) + + +# -------------------------------------------------------------------------- # + + def jtraj(q0, qf, t, qd0=None, qd1=None): """ Compute a joint-space trajectory @@ -518,7 +698,7 @@ def jtraj(q0, qf, t, qd0=None, qd1=None): :param qd1: final velocity, defaults to zero :type qd1: array_like(n), optional :return: trajectory - :rtype: Trajectory instance + :rtype: :class:`Trajectory` instance - ``tg = jtraj(q0, qf, N)`` is a joint space trajectory where the joint coordinates vary from ``q0`` (M) to ``qf`` (M). A quintic (5th order) @@ -531,11 +711,12 @@ def jtraj(q0, qf, t, qd0=None, qd1=None): The return value is an object that contains position, velocity and acceleration data. - Notes: + .. note:: The time vector, if given, scales the velocity and acceleration outputs + assuming that the time vector starts at zero and increases linearly. + + :References: - - The time vector, if given, scales the velocity and acceleration outputs - assuming that the time vector starts at zero and increases - linearly. + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. :seealso: :func:`ctraj`, :func:`qplot`, :func:`~SerialLink.jtraj` """ @@ -595,69 +776,6 @@ def jtraj(q0, qf, t, qd0=None, qd1=None): return Trajectory("jtraj", tv, qt, qdt, qddt, istime=True) -def mtraj(tfunc, q0, qf, t): - """ - Multi-axis trajectory - - :param tfunc: a 1D trajectory function, eg. :func:`quintic` or :func:`trapezoidal` - :type tfunc: callable - :param q0: initial configuration - :type q0: ndarray(m) - :param qf: final configuration - :type qf: ndarray(m) - :param t: time vector or number of steps - :type t: array_like or int - :raises TypeError: ``tfunc`` is not callable - :raises ValueError: length of ``q0`` and ``qf`` are different - :return: trajectory - :rtype: Trajectory instance - - - ``tg = mtraj(func, q0, qf, n)`` is a multi-axis trajectory varying - from configuration ``q0`` (M) to ``qf`` (M) according to the scalar trajectory - function ``tfunc`` in ``n``` steps. - - - ``tg = mtraj(func, q0, qf, t)`` as above but ``t`` is a uniformly-spaced time - vector - - The scalar trajectory function is applied to each axis:: - - tg = tfunc(s0, sF, n) - - and possible values of TFUNC include ``trapezoidal`` for a trapezoidal trajectory, or - ``quintic`` for a polynomial trajectory. - - The return value is an object that contains position, velocity and - acceleration data. - - .. note:: The time vector, if given, is assumed to be monotonically increasing, and - time scaling is based on the first and last element. - - :seealso: :func:`quintic`, :func:`trapezoidal` - """ - - if not callable(tfunc): - raise TypeError("first argument must be a function reference") - - q0 = getvector(q0) - qf = getvector(qf) - if len(q0) != len(qf): - raise ValueError("must be same number of elements in q0 and qf") - - traj = [] - for i in range(len(q0)): - # for each axis - traj.append(tfunc(q0[i], qf[i], t)) - - x = traj[0].t - y = np.array([tg.s for tg in traj]).T - yd = np.array([tg.sd for tg in traj]).T - ydd = np.array([tg.sdd for tg in traj]).T - - istime = traj[0].istime - - return Trajectory("mtraj", x, y, yd, ydd, istime) - - # -------------------------------------------------------------------------- # @@ -695,17 +813,15 @@ def ctraj(T0, T1, t=None, s=None): >>> len(tg) 20 - Notes: - - - In the second case ``s`` could be generated by a scalar trajectory - generator such as ``quintic`` or ``trapezoidal`` (default). - - Orientation interpolation is performed using unit-quaternion - interpolation. + .. note:: - Reference: + - In the second case ``s`` could be generated by a scalar trajectory + generator such as ``quintic`` or ``trapezoidal`` (default). + - Orientation interpolation is performed using unit-quaternion + interpolation. - - Robotics, Vision & Control, Sec 3.1.5, - Peter Corke, Springer 2011 + :References: + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. :seealso: :func:`~roboticstoolbox.trajectory.trapezoidal`, :func:`~spatialmath.unitquaternion.interp` @@ -725,6 +841,8 @@ def ctraj(T0, T1, t=None, s=None): def cmstraj(): + # Cartesian version of mstraj, via points are SE3 + # perhaps refactor mstraj to allow interpolation of any type pass @@ -765,7 +883,7 @@ def mstraj( :param verbose: print debug information, defaults to False :type verbose: bool, optional :return: trajectory - :rtype: Trajectory instance + :rtype: :class:`Trajectory` instance Computes a trajectory for N axes moving smoothly through a set of viapoints. @@ -784,7 +902,7 @@ def mstraj( #. In terms of segment time where ``tsegment`` is an array of segment times which is the number of via points minus one:: - ``traj = mstraj(viapoints, dt, tacc, tsegment=TS)`` + traj = mstraj(viapoints, dt, tacc, tsegment=TS) #. Governed by the speed of the slowest axis for the segment. The axis speed is a scalar (all axes have the same speed) or an N-vector of speed @@ -804,22 +922,24 @@ def mstraj( The trajectory proper is (``traj.t``, ``traj.q``). The trajectory is a matrix has one row per time step, and one column per axis. - Notes: + .. note:: - - Only one of ``qdmag`` or ``tsegment`` can be specified - - If ``tacc`` is greater than zero then the path smoothly accelerates - between segments using a polynomial blend. This means that the the via - point is not actually reached. - - The path length K is a function of the number of via - points and the time or velocity limits that apply. - - Can be used to create joint space trajectories where each axis is a - joint coordinate. - - Can be used to create Cartesian trajectories where the "axes" - correspond to translation and orientation in RPY or Euler angle form. - - If ``qdmax`` is a scalar then all axes are assumed to have the same - maximum speed. - - ``tg`` has extra attributes ``arrive``, ``info`` and ``via`` + - Only one of ``qdmag`` or ``tsegment`` can be specified + - If ``tacc`` is greater than zero then the path smoothly accelerates + between segments using a polynomial blend. This means that the the via + point is not actually reached. + - The path length K is a function of the number of via + points and the time or velocity limits that apply. + - Can be used to create joint space trajectories where each axis is a + joint coordinate. + - Can be used to create Cartesian trajectories where the "axes" + correspond to translation and orientation in RPY or Euler angle form. + - If ``qdmax`` is a scalar then all axes are assumed to have the same + maximum speed. + - ``tg`` has extra attributes ``arrive``, ``info`` and ``via`` + :References: + - Robotics, Vision & Control in Python, 3e, P. Corke, Springer 2023, Chap 3. :seealso: :func:`trapezoidal`, :func:`ctraj`, :func:`mtraj` """ @@ -970,10 +1090,13 @@ def mrange(start, stop, step): qd = dq / tseg # add the blend polynomial - qb = jtraj(q0, q_prev + tacc2 * qd, mrange(0, taccx, dt), qd0=qd_prev, qd1=qd).s - if verbose: # pragma nocover - print(qb) - tg = np.vstack([tg, qb[1:, :]]) + if taccx > 0: + qb = jtraj( + q0, q_prev + tacc2 * qd, mrange(0, taccx, dt), qd0=qd_prev, qd1=qd + ).s + if verbose: # pragma nocover + print(qb) + tg = np.vstack([tg, qb[1:, :]]) clock = clock + taccx # update the clock @@ -990,8 +1113,9 @@ def mrange(start, stop, step): qd_prev = qd # add the final blend - qb = jtraj(q0, q_next, mrange(0, tacc2, dt), qd0=qd_prev, qd1=qdf).s - tg = np.vstack([tg, qb[1:, :]]) + if tacc2 > 0: + qb = jtraj(q0, q_next, mrange(0, tacc2, dt), qd0=qd_prev, qd1=qdf).s + tg = np.vstack([tg, qb[1:, :]]) infolist.append(info(None, tseg, clock)) @@ -1001,9 +1125,6 @@ def mrange(start, stop, step): traj.via = viapoints return traj - # return namedtuple( - # 'mstraj', 't q arrive info via')( - # dt * np.arange(0, tg.shape[0]), tg, arrive, infolist, viapoints) if __name__ == "__main__": @@ -1020,8 +1141,14 @@ def mrange(start, stop, step): # t.plot(block=True) from roboticstoolbox import * + from spatialmath import SO2 + + # puma = models.DH.Puma560() - puma = models.DH.Puma560() + # traj = jtraj(puma.qz, puma.qr, 100) + # traj.plot(block=True) - traj = jtraj(puma.qz, puma.qr, 100) - traj.plot(block=True) + via = SO2(30, unit="deg") * np.array([[-1, 1, 1, -1, -1], [1, 1, -1, -1, 1]]) + traj0 = mstraj(via.T, dt=0.2, tacc=0.5, qdmax=[2, 1]) + xplot(traj0.q[:, 0], traj0.q[:, 1], color="red") + traj0.plot(block=True) diff --git a/roboticstoolbox/tools/types.py b/roboticstoolbox/tools/types.py new file mode 100644 index 000000000..193984cae --- /dev/null +++ b/roboticstoolbox/tools/types.py @@ -0,0 +1,13 @@ +#!/usr/bin/env python +""" +@author Jesse Haviland +""" + +from typing import Tuple, Union, List, Set +from numpy import ndarray + +NDArray = ndarray + +PyArrayLike = Union[List[float], Tuple[float], Set[float]] + +ArrayLike = Union[NDArray, PyArrayLike] diff --git a/rtb-data/rtbdata/data/hershey.json b/rtb-data/rtbdata/data/hershey.json new file mode 100644 index 000000000..3b69d16e3 --- /dev/null +++ b/rtb-data/rtbdata/data/hershey.json @@ -0,0 +1 @@ +{" ": {"strokes": [], "width": 0.64, "top": 0.0, "bottom": 0.0}, "!": {"strokes": [[[0.2, 0.12], [0.2, -0.44]], [[0.2, -0.64], [0.16, -0.68], [0.2, -0.72], [0.24, -0.68], [0.2, -0.64]]], "width": 0.4, "top": 0.72, "bottom": -0.12}, "\"": {"strokes": [[[0.16, 0.12], [0.16, -0.16]], [[0.48, 0.12], [0.48, -0.16]]], "width": 0.64, "top": 0.16, "bottom": -0.12}, "#": {"strokes": [[[0.44, 0.28], [0.16, -1.0]], [[0.68, 0.28], [0.4, -1.0]], [[0.16, -0.24], [0.72, -0.24]], [[0.12, -0.48], [0.68, -0.48]]], "width": 0.84, "top": 1.0, "bottom": -0.28}, "$": {"strokes": [[[0.32, 0.28], [0.32, -0.88]], [[0.48, 0.28], [0.48, -0.88]], [[0.68, 0.0], [0.6, 0.08], [0.48, 0.12], [0.32, 0.12], [0.2, 0.08], [0.12, 0.0], [0.12, -0.08], [0.16, -0.16], [0.2, -0.2], [0.28, -0.24], [0.52, -0.32], [0.6, -0.36], [0.64, -0.4], [0.68, -0.48], [0.68, -0.6], [0.6, -0.68], [0.48, -0.72], [0.32, -0.72], [0.2, -0.68], [0.12, -0.6]]], "width": 0.8, "top": 0.88, "bottom": -0.28}, "%": {"strokes": [[[0.84, 0.12], [0.12, -0.72]], [[0.32, 0.12], [0.4, 0.04], [0.4, -0.04], [0.36, -0.12], [0.28, -0.16], [0.2, -0.16], [0.12, -0.08], [0.12, 0.0], [0.16, 0.08], [0.24, 0.12], [0.32, 0.12], [0.4, 0.08], [0.52, 0.04], [0.64, 0.04], [0.76, 0.08], [0.84, 0.12]], [[0.68, -0.44], [0.6, -0.48], [0.56, -0.56], [0.56, -0.64], [0.64, -0.72], [0.72, -0.72], [0.8, -0.68], [0.84, -0.6], [0.84, -0.52], [0.76, -0.44], [0.68, -0.44]]], "width": 0.96, "top": 0.72, "bottom": -0.12}, "&": {"strokes": [[[0.92, -0.24], [0.92, -0.2], [0.88, -0.16], [0.84, -0.16], [0.8, -0.2], [0.76, -0.28], [0.68, -0.48], [0.6, -0.6], [0.52, -0.68], [0.44, -0.72], [0.28, -0.72], [0.2, -0.68], [0.16, -0.64], [0.12, -0.56], [0.12, -0.48], [0.16, -0.4], [0.2, -0.36], [0.48, -0.2], [0.52, -0.16], [0.56, -0.08], [0.56, 0.0], [0.52, 0.08], [0.44, 0.12], [0.36, 0.08], [0.32, 0.0], [0.32, -0.08], [0.36, -0.2], [0.44, -0.32], [0.64, -0.6], [0.72, -0.68], [0.8, -0.72], [0.88, -0.72], [0.92, -0.68], [0.92, -0.64]]], "width": 1.04, "top": 0.72, "bottom": -0.12}, "'": {"strokes": [[[0.2, 0.04], [0.16, 0.08], [0.2, 0.12], [0.24, 0.08], [0.24, 0.0], [0.2, -0.08], [0.16, -0.12]]], "width": 0.4, "top": 0.12, "bottom": -0.12}, "(": {"strokes": [[[0.44, 0.28], [0.36, 0.2], [0.28, 0.08], [0.2, -0.08], [0.16, -0.28], [0.16, -0.44], [0.2, -0.64], [0.28, -0.8], [0.36, -0.92], [0.44, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, ")": {"strokes": [[[0.12, 0.28], [0.2, 0.2], [0.28, 0.08], [0.36, -0.08], [0.4, -0.28], [0.4, -0.44], [0.36, -0.64], [0.28, -0.8], [0.2, -0.92], [0.12, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, "*": {"strokes": [[[0.32, 0.12], [0.32, -0.36]], [[0.12, 0.0], [0.52, -0.24]], [[0.52, 0.0], [0.12, -0.24]]], "width": 0.64, "top": 0.36, "bottom": -0.12}, "+": {"strokes": [[[0.52, 0.0], [0.52, -0.72]], [[0.16, -0.36], [0.88, -0.36]]], "width": 1.04, "top": 0.72, "bottom": 0.0}, ",": {"strokes": [[[0.24, -0.68], [0.2, -0.72], [0.16, -0.68], [0.2, -0.64], [0.24, -0.68], [0.24, -0.76], [0.2, -0.84], [0.16, -0.88]]], "width": 0.4, "top": 0.88, "bottom": 0.64}, "-": {"strokes": [[[0.16, -0.36], [0.88, -0.36]]], "width": 1.04, "top": 0.36, "bottom": 0.36}, ".": {"strokes": [[[0.2, -0.64], [0.16, -0.68], [0.2, -0.72], [0.24, -0.68], [0.2, -0.64]]], "width": 0.4, "top": 0.72, "bottom": 0.64}, "/": {"strokes": [[[0.8, 0.28], [0.08, -1.0]]], "width": 0.88, "top": 1.0, "bottom": -0.28}, "0": {"strokes": [[[0.36, 0.12], [0.24, 0.08], [0.16, -0.04], [0.12, -0.24], [0.12, -0.36], [0.16, -0.56], [0.24, -0.68], [0.36, -0.72], [0.44, -0.72], [0.56, -0.68], [0.64, -0.56], [0.68, -0.36], [0.68, -0.24], [0.64, -0.04], [0.56, 0.08], [0.44, 0.12], [0.36, 0.12]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "1": {"strokes": [[[0.24, -0.04], [0.32, 0.0], [0.44, 0.12], [0.44, -0.72]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "2": {"strokes": [[[0.16, -0.08], [0.16, -0.04], [0.2, 0.04], [0.24, 0.08], [0.32, 0.12], [0.48, 0.12], [0.56, 0.08], [0.6, 0.04], [0.64, -0.04], [0.64, -0.12], [0.6, -0.2], [0.52, -0.32], [0.12, -0.72], [0.68, -0.72]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "3": {"strokes": [[[0.2, 0.12], [0.64, 0.12], [0.4, -0.2], [0.52, -0.2], [0.6, -0.24], [0.64, -0.28], [0.68, -0.4], [0.68, -0.48], [0.64, -0.6], [0.56, -0.68], [0.44, -0.72], [0.32, -0.72], [0.2, -0.68], [0.16, -0.64], [0.12, -0.56]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "4": {"strokes": [[[0.52, 0.12], [0.12, -0.44], [0.72, -0.44]], [[0.52, 0.12], [0.52, -0.72]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "5": {"strokes": [[[0.6, 0.12], [0.2, 0.12], [0.16, -0.24], [0.2, -0.2], [0.32, -0.16], [0.44, -0.16], [0.56, -0.2], [0.64, -0.28], [0.68, -0.4], [0.68, -0.48], [0.64, -0.6], [0.56, -0.68], [0.44, -0.72], [0.32, -0.72], [0.2, -0.68], [0.16, -0.64], [0.12, -0.56]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "6": {"strokes": [[[0.64, 0.0], [0.6, 0.08], [0.48, 0.12], [0.4, 0.12], [0.28, 0.08], [0.2, -0.04], [0.16, -0.24], [0.16, -0.44], [0.2, -0.6], [0.28, -0.68], [0.4, -0.72], [0.44, -0.72], [0.56, -0.68], [0.64, -0.6], [0.68, -0.48], [0.68, -0.44], [0.64, -0.32], [0.56, -0.24], [0.44, -0.2], [0.4, -0.2], [0.28, -0.24], [0.2, -0.32], [0.16, -0.44]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "7": {"strokes": [[[0.68, 0.12], [0.28, -0.72]], [[0.12, 0.12], [0.68, 0.12]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "8": {"strokes": [[[0.32, 0.12], [0.2, 0.08], [0.16, 0.0], [0.16, -0.08], [0.2, -0.16], [0.28, -0.2], [0.44, -0.24], [0.56, -0.28], [0.64, -0.36], [0.68, -0.44], [0.68, -0.56], [0.64, -0.64], [0.6, -0.68], [0.48, -0.72], [0.32, -0.72], [0.2, -0.68], [0.16, -0.64], [0.12, -0.56], [0.12, -0.44], [0.16, -0.36], [0.24, -0.28], [0.36, -0.24], [0.52, -0.2], [0.6, -0.16], [0.64, -0.08], [0.64, 0.0], [0.6, 0.08], [0.48, 0.12], [0.32, 0.12]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "9": {"strokes": [[[0.64, -0.16], [0.6, -0.28], [0.52, -0.36], [0.4, -0.4], [0.36, -0.4], [0.24, -0.36], [0.16, -0.28], [0.12, -0.16], [0.12, -0.12], [0.16, 0.0], [0.24, 0.08], [0.36, 0.12], [0.4, 0.12], [0.52, 0.08], [0.6, 0.0], [0.64, -0.16], [0.64, -0.36], [0.6, -0.56], [0.52, -0.68], [0.4, -0.72], [0.32, -0.72], [0.2, -0.68], [0.16, -0.6]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, ":": {"strokes": [[[0.2, -0.16], [0.16, -0.2], [0.2, -0.24], [0.24, -0.2], [0.2, -0.16]], [[0.2, -0.64], [0.16, -0.68], [0.2, -0.72], [0.24, -0.68], [0.2, -0.64]]], "width": 0.4, "top": 0.72, "bottom": 0.16}, ";": {"strokes": [[[0.2, -0.16], [0.16, -0.2], [0.2, -0.24], [0.24, -0.2], [0.2, -0.16]], [[0.24, -0.68], [0.2, -0.72], [0.16, -0.68], [0.2, -0.64], [0.24, -0.68], [0.24, -0.76], [0.2, -0.84], [0.16, -0.88]]], "width": 0.4, "top": 0.88, "bottom": 0.16}, "<": {"strokes": [[[0.8, 0.0], [0.16, -0.36], [0.8, -0.72]]], "width": 0.96, "top": 0.72, "bottom": 0.0}, "=": {"strokes": [[[0.16, -0.24], [0.88, -0.24]], [[0.16, -0.48], [0.88, -0.48]]], "width": 1.04, "top": 0.48, "bottom": 0.24}, ">": {"strokes": [[[0.16, 0.0], [0.8, -0.36], [0.16, -0.72]]], "width": 0.96, "top": 0.72, "bottom": 0.0}, "?": {"strokes": [[[0.12, -0.08], [0.12, -0.04], [0.16, 0.04], [0.2, 0.08], [0.28, 0.12], [0.44, 0.12], [0.52, 0.08], [0.56, 0.04], [0.6, -0.04], [0.6, -0.12], [0.56, -0.2], [0.52, -0.24], [0.36, -0.32], [0.36, -0.44]], [[0.36, -0.64], [0.32, -0.68], [0.36, -0.72], [0.4, -0.68], [0.36, -0.64]]], "width": 0.72, "top": 0.72, "bottom": -0.12}, "@": {"strokes": [[[0.72, -0.2], [0.68, -0.12], [0.6, -0.08], [0.48, -0.08], [0.4, -0.12], [0.36, -0.16], [0.32, -0.28], [0.32, -0.4], [0.36, -0.48], [0.44, -0.52], [0.56, -0.52], [0.64, -0.48], [0.68, -0.4]], [[0.48, -0.08], [0.4, -0.16], [0.36, -0.28], [0.36, -0.4], [0.4, -0.48], [0.44, -0.52]], [[0.72, -0.08], [0.68, -0.4], [0.68, -0.48], [0.76, -0.52], [0.84, -0.52], [0.92, -0.44], [0.96, -0.32], [0.96, -0.24], [0.92, -0.12], [0.88, -0.04], [0.8, 0.04], [0.72, 0.08], [0.6, 0.12], [0.48, 0.12], [0.36, 0.08], [0.28, 0.04], [0.2, -0.04], [0.16, -0.12], [0.12, -0.24], [0.12, -0.36], [0.16, -0.48], [0.2, -0.56], [0.28, -0.64], [0.36, -0.68], [0.48, -0.72], [0.6, -0.72], [0.72, -0.68], [0.8, -0.64], [0.84, -0.6]], [[0.76, -0.08], [0.72, -0.4], [0.72, -0.48], [0.76, -0.52]]], "width": 1.08, "top": 0.72, "bottom": -0.12}, "A": {"strokes": [[[0.36, 0.12], [0.04, -0.72]], [[0.36, 0.12], [0.68, -0.72]], [[0.16, -0.44], [0.56, -0.44]]], "width": 0.72, "top": 0.72, "bottom": -0.12}, "B": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.52, 0.12], [0.64, 0.08], [0.68, 0.04], [0.72, -0.04], [0.72, -0.12], [0.68, -0.2], [0.64, -0.24], [0.52, -0.28]], [[0.16, -0.28], [0.52, -0.28], [0.64, -0.32], [0.68, -0.36], [0.72, -0.44], [0.72, -0.56], [0.68, -0.64], [0.64, -0.68], [0.52, -0.72], [0.16, -0.72]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "C": {"strokes": [[[0.72, -0.08], [0.68, 0.0], [0.6, 0.08], [0.52, 0.12], [0.36, 0.12], [0.28, 0.08], [0.2, 0.0], [0.16, -0.08], [0.12, -0.2], [0.12, -0.4], [0.16, -0.52], [0.2, -0.6], [0.28, -0.68], [0.36, -0.72], [0.52, -0.72], [0.6, -0.68], [0.68, -0.6], [0.72, -0.52]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "D": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.44, 0.12], [0.56, 0.08], [0.64, 0.0], [0.68, -0.08], [0.72, -0.2], [0.72, -0.4], [0.68, -0.52], [0.64, -0.6], [0.56, -0.68], [0.44, -0.72], [0.16, -0.72]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "E": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.68, 0.12]], [[0.16, -0.28], [0.48, -0.28]], [[0.16, -0.72], [0.68, -0.72]]], "width": 0.76, "top": 0.72, "bottom": -0.12}, "F": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.68, 0.12]], [[0.16, -0.28], [0.48, -0.28]]], "width": 0.72, "top": 0.72, "bottom": -0.12}, "G": {"strokes": [[[0.72, -0.08], [0.68, 0.0], [0.6, 0.08], [0.52, 0.12], [0.36, 0.12], [0.28, 0.08], [0.2, 0.0], [0.16, -0.08], [0.12, -0.2], [0.12, -0.4], [0.16, -0.52], [0.2, -0.6], [0.28, -0.68], [0.36, -0.72], [0.52, -0.72], [0.6, -0.68], [0.68, -0.6], [0.72, -0.52], [0.72, -0.4]], [[0.52, -0.4], [0.72, -0.4]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "H": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.72, 0.12], [0.72, -0.72]], [[0.16, -0.28], [0.72, -0.28]]], "width": 0.88, "top": 0.72, "bottom": -0.12}, "I": {"strokes": [[[0.16, 0.12], [0.16, -0.72]]], "width": 0.32, "top": 0.72, "bottom": -0.12}, "J": {"strokes": [[[0.48, 0.12], [0.48, -0.52], [0.44, -0.64], [0.4, -0.68], [0.32, -0.72], [0.24, -0.72], [0.16, -0.68], [0.12, -0.64], [0.08, -0.52], [0.08, -0.44]]], "width": 0.64, "top": 0.72, "bottom": -0.12}, "K": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.72, 0.12], [0.16, -0.44]], [[0.36, -0.24], [0.72, -0.72]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "L": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, -0.72], [0.64, -0.72]]], "width": 0.68, "top": 0.72, "bottom": -0.12}, "M": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.48, -0.72]], [[0.8, 0.12], [0.48, -0.72]], [[0.8, 0.12], [0.8, -0.72]]], "width": 0.96, "top": 0.72, "bottom": -0.12}, "N": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.72, -0.72]], [[0.72, 0.12], [0.72, -0.72]]], "width": 0.88, "top": 0.72, "bottom": -0.12}, "O": {"strokes": [[[0.36, 0.12], [0.28, 0.08], [0.2, 0.0], [0.16, -0.08], [0.12, -0.2], [0.12, -0.4], [0.16, -0.52], [0.2, -0.6], [0.28, -0.68], [0.36, -0.72], [0.52, -0.72], [0.6, -0.68], [0.68, -0.6], [0.72, -0.52], [0.76, -0.4], [0.76, -0.2], [0.72, -0.08], [0.68, 0.0], [0.6, 0.08], [0.52, 0.12], [0.36, 0.12]]], "width": 0.88, "top": 0.72, "bottom": -0.12}, "P": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.52, 0.12], [0.64, 0.08], [0.68, 0.04], [0.72, -0.04], [0.72, -0.16], [0.68, -0.24], [0.64, -0.28], [0.52, -0.32], [0.16, -0.32]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "Q": {"strokes": [[[0.36, 0.12], [0.28, 0.08], [0.2, 0.0], [0.16, -0.08], [0.12, -0.2], [0.12, -0.4], [0.16, -0.52], [0.2, -0.6], [0.28, -0.68], [0.36, -0.72], [0.52, -0.72], [0.6, -0.68], [0.68, -0.6], [0.72, -0.52], [0.76, -0.4], [0.76, -0.2], [0.72, -0.08], [0.68, 0.0], [0.6, 0.08], [0.52, 0.12], [0.36, 0.12]], [[0.48, -0.56], [0.72, -0.8]]], "width": 0.88, "top": 0.8, "bottom": -0.12}, "R": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, 0.12], [0.52, 0.12], [0.64, 0.08], [0.68, 0.04], [0.72, -0.04], [0.72, -0.12], [0.68, -0.2], [0.64, -0.24], [0.52, -0.28], [0.16, -0.28]], [[0.44, -0.28], [0.72, -0.72]]], "width": 0.84, "top": 0.72, "bottom": -0.12}, "S": {"strokes": [[[0.68, 0.0], [0.6, 0.08], [0.48, 0.12], [0.32, 0.12], [0.2, 0.08], [0.12, 0.0], [0.12, -0.08], [0.16, -0.16], [0.2, -0.2], [0.28, -0.24], [0.52, -0.32], [0.6, -0.36], [0.64, -0.4], [0.68, -0.48], [0.68, -0.6], [0.6, -0.68], [0.48, -0.72], [0.32, -0.72], [0.2, -0.68], [0.12, -0.6]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "T": {"strokes": [[[0.32, 0.12], [0.32, -0.72]], [[0.04, 0.12], [0.6, 0.12]]], "width": 0.64, "top": 0.72, "bottom": -0.12}, "U": {"strokes": [[[0.16, 0.12], [0.16, -0.48], [0.2, -0.6], [0.28, -0.68], [0.4, -0.72], [0.48, -0.72], [0.6, -0.68], [0.68, -0.6], [0.72, -0.48], [0.72, 0.12]]], "width": 0.88, "top": 0.72, "bottom": -0.12}, "V": {"strokes": [[[0.04, 0.12], [0.36, -0.72]], [[0.68, 0.12], [0.36, -0.72]]], "width": 0.72, "top": 0.72, "bottom": -0.12}, "W": {"strokes": [[[0.08, 0.12], [0.28, -0.72]], [[0.48, 0.12], [0.28, -0.72]], [[0.48, 0.12], [0.68, -0.72]], [[0.88, 0.12], [0.68, -0.72]]], "width": 0.96, "top": 0.72, "bottom": -0.12}, "X": {"strokes": [[[0.12, 0.12], [0.68, -0.72]], [[0.68, 0.12], [0.12, -0.72]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "Y": {"strokes": [[[0.04, 0.12], [0.36, -0.28], [0.36, -0.72]], [[0.68, 0.12], [0.36, -0.28]]], "width": 0.72, "top": 0.72, "bottom": -0.12}, "Z": {"strokes": [[[0.68, 0.12], [0.12, -0.72]], [[0.12, 0.12], [0.68, 0.12]], [[0.12, -0.72], [0.68, -0.72]]], "width": 0.8, "top": 0.72, "bottom": -0.12}, "[": {"strokes": [[[0.16, 0.28], [0.16, -1.0]], [[0.2, 0.28], [0.2, -1.0]], [[0.16, 0.28], [0.44, 0.28]], [[0.16, -1.0], [0.44, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, "\\": {"strokes": [[[0.0, 0.12], [0.56, -0.84]]], "width": 0.56, "top": 0.84, "bottom": -0.12}, "]": {"strokes": [[[0.36, 0.28], [0.36, -1.0]], [[0.4, 0.28], [0.4, -1.0]], [[0.12, 0.28], [0.4, 0.28]], [[0.12, -1.0], [0.4, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, "^": {"strokes": [[[0.24, -0.12], [0.32, 0.0], [0.4, -0.12]], [[0.12, -0.24], [0.32, -0.04], [0.52, -0.24]], [[0.32, -0.04], [0.32, -0.72]]], "width": 0.64, "top": 0.72, "bottom": 0.0}, "_": {"strokes": [[[0.0, -0.8], [0.64, -0.8]]], "width": 0.64, "top": 0.8, "bottom": 0.8}, "`": {"strokes": [[[0.24, 0.12], [0.2, 0.08], [0.16, 0.0], [0.16, -0.08], [0.2, -0.12], [0.24, -0.08], [0.2, -0.04]]], "width": 0.4, "top": 0.12, "bottom": -0.12}, "a": {"strokes": [[[0.6, -0.16], [0.6, -0.72]], [[0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.76, "top": 0.72, "bottom": 0.16}, "b": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, -0.28], [0.24, -0.2], [0.32, -0.16], [0.44, -0.16], [0.52, -0.2], [0.6, -0.28], [0.64, -0.4], [0.64, -0.48], [0.6, -0.6], [0.52, -0.68], [0.44, -0.72], [0.32, -0.72], [0.24, -0.68], [0.16, -0.6]]], "width": 0.76, "top": 0.72, "bottom": -0.12}, "c": {"strokes": [[[0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.72, "top": 0.72, "bottom": 0.16}, "d": {"strokes": [[[0.6, 0.12], [0.6, -0.72]], [[0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.76, "top": 0.72, "bottom": -0.12}, "e": {"strokes": [[[0.12, -0.4], [0.6, -0.4], [0.6, -0.32], [0.56, -0.24], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.72, "top": 0.72, "bottom": 0.16}, "f": {"strokes": [[[0.4, 0.12], [0.32, 0.12], [0.24, 0.08], [0.2, -0.04], [0.2, -0.72]], [[0.08, -0.16], [0.36, -0.16]]], "width": 0.48, "top": 0.72, "bottom": -0.12}, "g": {"strokes": [[[0.6, -0.16], [0.6, -0.8], [0.56, -0.92], [0.52, -0.96], [0.44, -1.0], [0.32, -1.0], [0.24, -0.96]], [[0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.76, "top": 1.0, "bottom": 0.16}, "h": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.16, -0.32], [0.28, -0.2], [0.36, -0.16], [0.48, -0.16], [0.56, -0.2], [0.6, -0.32], [0.6, -0.72]]], "width": 0.76, "top": 0.72, "bottom": -0.12}, "i": {"strokes": [[[0.12, 0.12], [0.16, 0.08], [0.2, 0.12], [0.16, 0.16], [0.12, 0.12]], [[0.16, -0.16], [0.16, -0.72]]], "width": 0.32, "top": 0.72, "bottom": -0.16}, "j": {"strokes": [[[0.2, 0.12], [0.24, 0.08], [0.28, 0.12], [0.24, 0.16], [0.2, 0.12]], [[0.24, -0.16], [0.24, -0.84], [0.2, -0.96], [0.12, -1.0], [0.04, -1.0]]], "width": 0.4, "top": 1.0, "bottom": -0.16}, "k": {"strokes": [[[0.16, 0.12], [0.16, -0.72]], [[0.56, -0.16], [0.16, -0.56]], [[0.32, -0.4], [0.6, -0.72]]], "width": 0.68, "top": 0.72, "bottom": -0.12}, "l": {"strokes": [[[0.16, 0.12], [0.16, -0.72]]], "width": 0.32, "top": 0.72, "bottom": -0.12}, "m": {"strokes": [[[0.16, -0.16], [0.16, -0.72]], [[0.16, -0.32], [0.28, -0.2], [0.36, -0.16], [0.48, -0.16], [0.56, -0.2], [0.6, -0.32], [0.6, -0.72]], [[0.6, -0.32], [0.72, -0.2], [0.8, -0.16], [0.92, -0.16], [1.0, -0.2], [1.04, -0.32], [1.04, -0.72]]], "width": 1.2, "top": 0.72, "bottom": 0.16}, "n": {"strokes": [[[0.16, -0.16], [0.16, -0.72]], [[0.16, -0.32], [0.28, -0.2], [0.36, -0.16], [0.48, -0.16], [0.56, -0.2], [0.6, -0.32], [0.6, -0.72]]], "width": 0.76, "top": 0.72, "bottom": 0.16}, "o": {"strokes": [[[0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6], [0.64, -0.48], [0.64, -0.4], [0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16]]], "width": 0.76, "top": 0.72, "bottom": 0.16}, "p": {"strokes": [[[0.16, -0.16], [0.16, -1.0]], [[0.16, -0.28], [0.24, -0.2], [0.32, -0.16], [0.44, -0.16], [0.52, -0.2], [0.6, -0.28], [0.64, -0.4], [0.64, -0.48], [0.6, -0.6], [0.52, -0.68], [0.44, -0.72], [0.32, -0.72], [0.24, -0.68], [0.16, -0.6]]], "width": 0.76, "top": 1.0, "bottom": 0.16}, "q": {"strokes": [[[0.6, -0.16], [0.6, -1.0]], [[0.6, -0.28], [0.52, -0.2], [0.44, -0.16], [0.32, -0.16], [0.24, -0.2], [0.16, -0.28], [0.12, -0.4], [0.12, -0.48], [0.16, -0.6], [0.24, -0.68], [0.32, -0.72], [0.44, -0.72], [0.52, -0.68], [0.6, -0.6]]], "width": 0.76, "top": 1.0, "bottom": 0.16}, "r": {"strokes": [[[0.16, -0.16], [0.16, -0.72]], [[0.16, -0.4], [0.2, -0.28], [0.28, -0.2], [0.36, -0.16], [0.48, -0.16]]], "width": 0.52, "top": 0.72, "bottom": 0.16}, "s": {"strokes": [[[0.56, -0.28], [0.52, -0.2], [0.4, -0.16], [0.28, -0.16], [0.16, -0.2], [0.12, -0.28], [0.16, -0.36], [0.24, -0.4], [0.44, -0.44], [0.52, -0.48], [0.56, -0.56], [0.56, -0.6], [0.52, -0.68], [0.4, -0.72], [0.28, -0.72], [0.16, -0.68], [0.12, -0.6]]], "width": 0.68, "top": 0.72, "bottom": 0.16}, "t": {"strokes": [[[0.2, 0.12], [0.2, -0.56], [0.24, -0.68], [0.32, -0.72], [0.4, -0.72]], [[0.08, -0.16], [0.36, -0.16]]], "width": 0.48, "top": 0.72, "bottom": -0.12}, "u": {"strokes": [[[0.16, -0.16], [0.16, -0.56], [0.2, -0.68], [0.28, -0.72], [0.4, -0.72], [0.48, -0.68], [0.6, -0.56]], [[0.6, -0.16], [0.6, -0.72]]], "width": 0.76, "top": 0.72, "bottom": 0.16}, "v": {"strokes": [[[0.08, -0.16], [0.32, -0.72]], [[0.56, -0.16], [0.32, -0.72]]], "width": 0.64, "top": 0.72, "bottom": 0.16}, "w": {"strokes": [[[0.12, -0.16], [0.28, -0.72]], [[0.44, -0.16], [0.28, -0.72]], [[0.44, -0.16], [0.6, -0.72]], [[0.76, -0.16], [0.6, -0.72]]], "width": 0.88, "top": 0.72, "bottom": 0.16}, "x": {"strokes": [[[0.12, -0.16], [0.56, -0.72]], [[0.56, -0.16], [0.12, -0.72]]], "width": 0.68, "top": 0.72, "bottom": 0.16}, "y": {"strokes": [[[0.08, -0.16], [0.32, -0.72]], [[0.56, -0.16], [0.32, -0.72], [0.24, -0.88], [0.16, -0.96], [0.08, -1.0], [0.04, -1.0]]], "width": 0.64, "top": 1.0, "bottom": 0.16}, "z": {"strokes": [[[0.56, -0.16], [0.12, -0.72]], [[0.12, -0.16], [0.56, -0.16]], [[0.12, -0.72], [0.56, -0.72]]], "width": 0.68, "top": 0.72, "bottom": 0.16}, "{": {"strokes": [[[0.36, 0.28], [0.28, 0.24], [0.24, 0.2], [0.2, 0.12], [0.2, 0.04], [0.24, -0.04], [0.28, -0.08], [0.32, -0.16], [0.32, -0.24], [0.24, -0.32]], [[0.28, 0.24], [0.24, 0.16], [0.24, 0.08], [0.28, 0.0], [0.32, -0.04], [0.36, -0.12], [0.36, -0.2], [0.32, -0.28], [0.16, -0.36], [0.32, -0.44], [0.36, -0.52], [0.36, -0.6], [0.32, -0.68], [0.28, -0.72], [0.24, -0.8], [0.24, -0.88], [0.28, -0.96]], [[0.24, -0.4], [0.32, -0.48], [0.32, -0.56], [0.28, -0.64], [0.24, -0.68], [0.2, -0.76], [0.2, -0.84], [0.24, -0.92], [0.28, -0.96], [0.36, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, "|": {"strokes": [[[0.16, 0.28], [0.16, -1.0]]], "width": 0.32, "top": 1.0, "bottom": -0.28}, "}": {"strokes": [[[0.2, 0.28], [0.28, 0.24], [0.32, 0.2], [0.36, 0.12], [0.36, 0.04], [0.32, -0.04], [0.28, -0.08], [0.24, -0.16], [0.24, -0.24], [0.32, -0.32]], [[0.28, 0.24], [0.32, 0.16], [0.32, 0.08], [0.28, 0.0], [0.24, -0.04], [0.2, -0.12], [0.2, -0.2], [0.24, -0.28], [0.4, -0.36], [0.24, -0.44], [0.2, -0.52], [0.2, -0.6], [0.24, -0.68], [0.28, -0.72], [0.32, -0.8], [0.32, -0.88], [0.28, -0.96]], [[0.32, -0.4], [0.24, -0.48], [0.24, -0.56], [0.28, -0.64], [0.32, -0.68], [0.36, -0.76], [0.36, -0.84], [0.32, -0.92], [0.28, -0.96], [0.2, -1.0]]], "width": 0.56, "top": 1.0, "bottom": -0.28}, "~": {"strokes": [[[0.12, -0.48], [0.12, -0.4], [0.16, -0.28], [0.24, -0.24], [0.32, -0.24], [0.4, -0.28], [0.56, -0.4], [0.64, -0.44], [0.72, -0.44], [0.8, -0.4], [0.84, -0.32]], [[0.12, -0.4], [0.16, -0.32], [0.24, -0.28], [0.32, -0.28], [0.4, -0.32], [0.56, -0.44], [0.64, -0.48], [0.72, -0.48], [0.8, -0.44], [0.84, -0.32], [0.84, -0.24]]], "width": 0.96, "top": 0.48, "bottom": 0.24}, "\u007f": {"strokes": [[[0.24, 0.12], [0.16, 0.08], [0.12, 0.0], [0.12, -0.08], [0.16, -0.16], [0.24, -0.2], [0.32, -0.2], [0.4, -0.16], [0.44, -0.08], [0.44, 0.0], [0.4, 0.08], [0.32, 0.12], [0.24, 0.12]]], "width": 0.56, "top": 0.2, "bottom": -0.12}} \ No newline at end of file diff --git a/rtb-data/rtbdata/data/killian-small.toro b/rtb-data/rtbdata/data/killian-small.toro new file mode 100644 index 000000000..0eb8aeaf4 --- /dev/null +++ b/rtb-data/rtbdata/data/killian-small.toro @@ -0,0 +1,5936 @@ +VERTEX2 0 1.008240 -0.016781 0.005957 +VERTEX2 1 2.090063 0.008002 0.015650 +VERTEX2 2 3.117849 -0.027274 0.023100 +VERTEX2 3 4.198081 0.087164 0.035227 +VERTEX2 4 5.279355 0.111386 0.045086 +VERTEX2 5 6.263466 0.126602 -0.005163 +VERTEX2 6 7.283441 0.076036 -0.015700 +VERTEX2 7 8.357930 0.058120 -0.015358 +VERTEX2 8 9.390035 -0.012710 0.017520 +VERTEX2 9 10.408369 -0.056070 -0.208266 +VERTEX2 10 11.431652 -0.393503 -0.373306 +VERTEX2 11 12.421098 -0.716382 -0.409338 +VERTEX2 12 12.938025 -1.136982 -0.920065 +VERTEX2 13 13.179887 -1.862584 -1.469585 +VERTEX2 14 13.122046 -2.788254 -1.652690 +VERTEX2 15 13.051261 -3.758466 -1.687129 +VERTEX2 16 12.965443 -4.701408 -1.708143 +VERTEX2 17 12.831314 -5.844807 -1.648737 +VERTEX2 18 12.820085 -6.955505 -1.575464 +VERTEX2 19 12.747445 -7.921361 -1.591606 +VERTEX2 20 12.652820 -8.957570 -1.621075 +VERTEX2 21 12.563742 -10.037374 -1.610888 +VERTEX2 22 12.425200 -11.095818 -1.631360 +VERTEX2 23 12.387697 -12.208372 -1.474633 +VERTEX2 24 12.510614 -13.263473 -1.500328 +VERTEX2 25 12.547478 -14.384032 -1.511643 +VERTEX2 26 12.585983 -15.530428 -1.522659 +VERTEX2 27 12.620192 -16.542624 -1.541081 +VERTEX2 28 12.684799 -17.625564 -1.549389 +VERTEX2 29 12.773076 -18.773939 -1.567421 +VERTEX2 30 12.704193 -19.809580 -1.517939 +VERTEX2 31 12.724701 -20.841986 -1.462831 +VERTEX2 32 12.816564 -21.864708 -1.470615 +VERTEX2 33 12.894463 -22.846646 -1.457601 +VERTEX2 34 12.954367 -23.936482 -1.443297 +VERTEX2 35 13.063404 -24.967079 -1.445467 +VERTEX2 36 13.163316 -26.009839 -1.465642 +VERTEX2 37 13.251729 -27.182310 -1.500353 +VERTEX2 38 13.285664 -28.264933 -1.508484 +VERTEX2 39 13.169921 -29.271690 -1.511428 +VERTEX2 40 13.220708 -30.363185 -1.469856 +VERTEX2 41 13.244028 -31.361364 -1.480117 +VERTEX2 42 13.330487 -32.520368 -1.451720 +VERTEX2 43 13.428325 -33.524649 -1.354528 +VERTEX2 44 13.639648 -34.564084 -1.313412 +VERTEX2 45 13.869435 -35.270398 -1.294624 +VERTEX2 46 14.063414 -36.366230 -1.364240 +VERTEX2 47 14.251332 -37.523363 -1.430199 +VERTEX2 48 14.343207 -38.637434 -1.440433 +VERTEX2 49 14.447920 -39.716769 -1.448148 +VERTEX2 50 14.585242 -40.830508 -1.485941 +VERTEX2 51 14.724153 -41.866809 -1.501403 +VERTEX2 52 14.726488 -42.907917 -1.443042 +VERTEX2 53 14.823526 -43.927803 -1.367939 +VERTEX2 54 14.981288 -44.878699 -1.373292 +VERTEX2 55 15.138925 -46.001730 -1.361288 +VERTEX2 56 15.360413 -46.940584 -1.393816 +VERTEX2 57 15.490248 -47.914339 -1.391594 +VERTEX2 58 15.632656 -48.932275 -1.397703 +VERTEX2 59 15.809021 -49.985997 -1.809499 +VERTEX2 60 15.509627 -50.443970 -2.359617 +VERTEX2 61 14.576048 -50.811841 -2.776838 +VERTEX2 62 13.615460 -50.906042 -2.975414 +VERTEX2 63 12.567983 -51.065337 -2.960918 +VERTEX2 64 11.468120 -51.234896 -2.960536 +VERTEX2 65 10.472796 -51.509353 -2.964961 +VERTEX2 66 9.542766 -51.715317 -2.955000 +VERTEX2 67 8.497177 -51.798926 -2.992378 +VERTEX2 68 7.576233 -51.904089 -2.916409 +VERTEX2 69 6.502860 -52.066274 -2.715093 +VERTEX2 70 5.876391 -52.527963 -2.133180 +VERTEX2 71 5.698428 -53.030921 -1.599277 +VERTEX2 72 5.836598 -54.067286 -1.348954 +VERTEX2 73 6.153189 -55.069875 -1.245237 +VERTEX2 74 6.433631 -56.048897 -1.262576 +VERTEX2 75 6.574598 -57.073560 -1.541454 +VERTEX2 76 6.567743 -58.117504 -1.498989 +VERTEX2 77 6.704653 -59.162898 -1.406075 +VERTEX2 78 6.957326 -60.164197 -1.184749 +VERTEX2 79 7.393262 -61.031874 -1.181012 +VERTEX2 80 7.672845 -61.965672 -1.319956 +VERTEX2 81 7.862141 -63.102090 -1.315962 +VERTEX2 82 8.141905 -64.095752 -1.316535 +VERTEX2 83 8.442421 -65.110110 -0.966047 +VERTEX2 84 9.040948 -65.725306 -0.381081 +VERTEX2 85 9.932431 -65.947867 0.018307 +VERTEX2 86 11.026853 -65.658594 0.482614 +VERTEX2 87 12.099780 -65.185528 0.519458 +VERTEX2 88 13.217687 -64.707835 0.365286 +VERTEX2 89 14.185202 -64.468174 0.314768 +VERTEX2 90 15.255082 -64.186406 0.305942 +VERTEX2 91 16.374322 -63.863361 0.303244 +VERTEX2 92 17.288609 -63.648433 0.317155 +VERTEX2 93 18.387802 -63.383950 0.266784 +VERTEX2 94 19.367190 -63.145902 0.284683 +VERTEX2 95 20.453099 -62.960865 0.256754 +VERTEX2 96 21.480818 -62.790378 0.285499 +VERTEX2 97 22.483118 -62.589602 0.308919 +VERTEX2 98 23.557854 -62.260877 0.255520 +VERTEX2 99 24.559478 -62.156030 0.307987 +VERTEX2 100 25.542875 -61.837153 0.318215 +VERTEX2 101 26.575526 -61.540645 0.313647 +VERTEX2 102 27.616899 -61.286296 0.269309 +VERTEX2 103 28.573987 -60.896154 0.282736 +VERTEX2 104 29.344278 -60.715098 0.802537 +VERTEX2 105 29.551735 -60.421796 1.402466 +VERTEX2 106 29.288525 -59.363432 1.819818 +VERTEX2 107 28.966070 -58.270255 1.817081 +VERTEX2 108 28.754937 -57.368249 1.864675 +VERTEX2 109 28.507110 -56.242217 1.875264 +VERTEX2 110 28.237129 -55.266163 1.888845 +VERTEX2 111 27.829469 -54.311928 1.890472 +VERTEX2 112 27.462585 -53.382465 1.879860 +VERTEX2 113 27.192238 -52.407305 1.882016 +VERTEX2 114 26.911631 -51.378475 1.882295 +VERTEX2 115 26.581491 -50.239206 1.878990 +VERTEX2 116 26.366239 -49.283653 1.879661 +VERTEX2 117 25.978948 -48.326244 2.559307 +VERTEX2 118 25.608446 -48.221833 3.101474 +VERTEX2 119 24.534377 -48.164350 -3.028371 +VERTEX2 120 23.503318 -48.575211 -2.608228 +VERTEX2 121 22.481145 -48.988694 -2.667090 +VERTEX2 122 21.492852 -49.459712 -2.711562 +VERTEX2 123 20.403282 -49.843922 -2.751670 +VERTEX2 124 19.424624 -50.137600 -2.775336 +VERTEX2 125 18.424219 -50.472598 -2.814718 +VERTEX2 126 17.333376 -50.723360 -2.848980 +VERTEX2 127 16.188435 -50.987573 -2.859474 +VERTEX2 128 16.150214 -51.034416 2.774819 +VERTEX2 129 16.167405 -51.068062 2.109424 +VERTEX2 130 16.089502 -50.688858 2.695130 +VERTEX2 131 15.982641 -50.754879 -3.072390 +VERTEX2 132 15.999507 -50.750054 -2.553160 +VERTEX2 133 15.992195 -50.903820 -2.066507 +VERTEX2 134 15.832386 -51.471325 -1.802266 +VERTEX2 135 15.405456 -52.104880 -2.285996 +VERTEX2 136 14.781060 -52.520514 -2.775315 +VERTEX2 137 13.791840 -52.822041 -2.829657 +VERTEX2 138 12.761070 -53.100652 -2.872518 +VERTEX2 139 11.693490 -53.378296 -2.900971 +VERTEX2 140 10.599741 -53.683762 -2.867162 +VERTEX2 141 9.665815 -53.970688 -2.898514 +VERTEX2 142 8.645049 -54.235902 -2.765542 +VERTEX2 143 7.785350 -54.606957 -2.795521 +VERTEX2 144 6.826513 -54.997322 -2.821555 +VERTEX2 145 5.834286 -55.204012 -2.869487 +VERTEX2 146 4.756893 -55.460505 -2.700736 +VERTEX2 147 4.066131 -55.912177 -2.916603 +VERTEX2 148 3.080462 -55.936586 -3.063989 +VERTEX2 149 1.890546 -55.896695 2.995809 +VERTEX2 150 1.016470 -55.391388 2.601001 +VERTEX2 151 0.015376 -54.940734 2.933865 +VERTEX2 152 -1.063878 -54.985608 -2.955187 +VERTEX2 153 -2.153494 -55.222194 -2.821403 +VERTEX2 154 -3.194127 -55.527837 -2.838283 +VERTEX2 155 -4.317544 -55.767247 -2.853126 +VERTEX2 156 -5.382061 -56.211125 -2.758207 +VERTEX2 157 -6.263237 -56.587050 -2.759432 +VERTEX2 158 -7.343751 -56.971626 -2.785844 +VERTEX2 159 -8.307123 -57.358488 -2.838371 +VERTEX2 160 -9.250689 -57.547279 -2.846990 +VERTEX2 161 -10.254520 -57.794398 -2.872479 +VERTEX2 162 -11.276054 -58.063423 -3.021660 +VERTEX2 163 -11.835539 -58.027444 2.670329 +VERTEX2 164 -12.194630 -57.711566 2.061700 +VERTEX2 165 -12.499979 -56.709034 1.878335 +VERTEX2 166 -12.785593 -55.784677 1.865888 +VERTEX2 167 -13.003253 -54.684468 1.841270 +VERTEX2 168 -13.273789 -53.796647 1.824587 +VERTEX2 169 -13.536941 -52.815377 1.855737 +VERTEX2 170 -13.841870 -51.828197 1.834979 +VERTEX2 171 -14.123021 -50.633410 1.791524 +VERTEX2 172 -14.360512 -49.667636 1.797597 +VERTEX2 173 -14.669303 -48.574262 1.870169 +VERTEX2 174 -15.018924 -47.630526 2.302147 +VERTEX2 175 -15.553998 -47.348254 2.882151 +VERTEX2 176 -16.745220 -47.337385 -2.919785 +VERTEX2 177 -17.721507 -47.535555 -2.868577 +VERTEX2 178 -18.730949 -47.862912 -2.845349 +VERTEX2 179 -19.735254 -48.099349 -2.844935 +VERTEX2 180 -20.855164 -48.481322 -2.853469 +VERTEX2 181 -22.026535 -48.878493 -2.861011 +VERTEX2 182 -23.092013 -49.188546 -2.795976 +VERTEX2 183 -24.099955 -49.528785 -2.773702 +VERTEX2 184 -25.048721 -49.789481 -2.783216 +VERTEX2 185 -26.055363 -50.127794 -2.792660 +VERTEX2 186 -27.186338 -50.541496 -2.797316 +VERTEX2 187 -28.209293 -50.846528 -2.807702 +VERTEX2 188 -29.297332 -51.164863 -2.818369 +VERTEX2 189 -30.386741 -51.540572 -2.845861 +VERTEX2 190 -31.495528 -51.888010 -2.852081 +VERTEX2 191 -32.541539 -52.095374 -2.872906 +VERTEX2 192 -33.622688 -52.380996 -2.829846 +VERTEX2 193 -34.557039 -52.711533 -2.845908 +VERTEX2 194 -35.472800 -52.981530 -2.860645 +VERTEX2 195 -36.417938 -53.275704 -2.875574 +VERTEX2 196 -37.419389 -53.575711 -2.886566 +VERTEX2 197 -38.589294 -53.787176 -3.027585 +VERTEX2 198 -39.728524 -53.839826 -2.984433 +VERTEX2 199 -40.825682 -54.208842 -2.838286 +VERTEX2 200 -41.874836 -54.481910 -2.811836 +VERTEX2 201 -42.829134 -54.708449 -2.809857 +VERTEX2 202 -43.811986 -54.986384 -2.815980 +VERTEX2 203 -44.662164 -55.309274 -2.824075 +VERTEX2 204 -45.840173 -55.568451 -2.853925 +VERTEX2 205 -47.056529 -55.885263 -2.856867 +VERTEX2 206 -47.950783 -56.139023 -2.865900 +VERTEX2 207 -49.058728 -56.415536 -2.920628 +VERTEX2 208 -50.065981 -56.535160 -2.946878 +VERTEX2 209 -51.210229 -56.717534 -2.961929 +VERTEX2 210 -52.299182 -56.956183 -2.977034 +VERTEX2 211 -53.444712 -57.136053 -2.817289 +VERTEX2 212 -54.497455 -57.528783 -2.823305 +VERTEX2 213 -55.590814 -57.901049 -2.844889 +VERTEX2 214 -56.598647 -58.124950 -2.920992 +VERTEX2 215 -57.641972 -58.380614 -2.917396 +VERTEX2 216 -58.645819 -58.604480 -2.964229 +VERTEX2 217 -59.747160 -58.892478 -2.916627 +VERTEX2 218 -60.796508 -59.001085 -2.938724 +VERTEX2 219 -61.849414 -59.257590 -2.851686 +VERTEX2 220 -62.950748 -59.521447 -2.796376 +VERTEX2 221 -64.083350 -59.839055 -2.806865 +VERTEX2 222 -65.140193 -60.164452 -2.814055 +VERTEX2 223 -66.184464 -60.534842 -2.911880 +VERTEX2 224 -67.237302 -60.684503 -2.927984 +VERTEX2 225 -68.225498 -60.818688 -2.919470 +VERTEX2 226 -69.357122 -61.063118 -2.905971 +VERTEX2 227 -70.299440 -61.325921 -2.745101 +VERTEX2 228 -70.790886 -61.653650 -2.238237 +VERTEX2 229 -71.131143 -62.117773 -1.634401 +VERTEX2 230 -71.065283 -63.205377 -1.333323 +VERTEX2 231 -70.998665 -64.255091 -1.316285 +VERTEX2 232 -70.721976 -65.289205 -1.301794 +VERTEX2 233 -70.515124 -66.220646 -1.274295 +VERTEX2 234 -70.234954 -67.284594 -1.289044 +VERTEX2 235 -69.878178 -68.299236 -1.268940 +VERTEX2 236 -69.606727 -69.352210 -1.233475 +VERTEX2 237 -69.347937 -70.336372 -1.249105 +VERTEX2 238 -69.121069 -71.346886 -1.226283 +VERTEX2 239 -68.841309 -72.406592 -1.263046 +VERTEX2 240 -68.606205 -73.474217 -1.294494 +VERTEX2 241 -68.355459 -74.596213 -1.291691 +VERTEX2 242 -68.038471 -75.689552 -1.283756 +VERTEX2 243 -67.821668 -76.610779 -1.153053 +VERTEX2 244 -67.433754 -77.118924 -0.543026 +VERTEX2 245 -66.830584 -77.327144 0.041847 +VERTEX2 246 -65.761457 -77.324774 0.028550 +VERTEX2 247 -64.723742 -77.143462 0.232234 +VERTEX2 248 -63.684560 -76.923020 0.280020 +VERTEX2 249 -62.701744 -76.657904 0.314743 +VERTEX2 250 -61.565039 -76.258973 0.326626 +VERTEX2 251 -60.561923 -75.935818 0.321594 +VERTEX2 252 -59.539253 -75.652853 0.327023 +VERTEX2 253 -58.494777 -75.331023 0.312806 +VERTEX2 254 -57.496093 -75.051482 0.240034 +VERTEX2 255 -56.505694 -74.887113 0.205210 +VERTEX2 256 -55.430974 -74.656151 0.172897 +VERTEX2 257 -54.340802 -74.418258 0.166286 +VERTEX2 258 -53.221089 -74.234015 0.171115 +VERTEX2 259 -52.098294 -74.083722 0.178937 +VERTEX2 260 -51.006327 -73.910298 0.183623 +VERTEX2 261 -49.959604 -73.732055 0.202207 +VERTEX2 262 -48.938584 -73.479027 0.230777 +VERTEX2 263 -47.894684 -73.229898 0.243532 +VERTEX2 264 -46.787894 -72.974227 0.241713 +VERTEX2 265 -45.731104 -72.695732 0.215528 +VERTEX2 266 -44.513513 -72.374785 0.196031 +VERTEX2 267 -43.566280 -72.224871 0.219884 +VERTEX2 268 -42.440445 -72.015631 0.232328 +VERTEX2 269 -41.382079 -71.824369 0.201525 +VERTEX2 270 -40.295572 -71.529974 0.209240 +VERTEX2 271 -39.231567 -71.311401 0.223604 +VERTEX2 272 -38.149046 -71.039557 0.244408 +VERTEX2 273 -37.097962 -70.857044 0.250587 +VERTEX2 274 -35.965391 -70.569979 0.255983 +VERTEX2 275 -34.839719 -70.388848 0.150469 +VERTEX2 276 -33.697146 -70.286672 0.169535 +VERTEX2 277 -32.559602 -70.111882 0.170801 +VERTEX2 278 -31.463958 -69.925777 0.192641 +VERTEX2 279 -30.410324 -69.719843 0.239432 +VERTEX2 280 -29.382831 -69.477944 0.273413 +VERTEX2 281 -28.309068 -69.096219 0.306172 +VERTEX2 282 -27.357965 -68.851042 0.306112 +VERTEX2 283 -26.296107 -68.593551 0.312731 +VERTEX2 284 -25.164290 -68.270192 0.270328 +VERTEX2 285 -24.112405 -68.028145 0.266985 +VERTEX2 286 -23.102599 -67.804929 0.280495 +VERTEX2 287 -22.050913 -67.573333 0.247837 +VERTEX2 288 -21.002390 -67.217840 0.271110 +VERTEX2 289 -19.887187 -66.827350 0.297729 +VERTEX2 290 -18.791663 -66.504102 0.321893 +VERTEX2 291 -17.793220 -66.199801 0.346693 +VERTEX2 292 -16.818649 -65.798006 0.359026 +VERTEX2 293 -15.777252 -65.444128 0.242828 +VERTEX2 294 -14.751679 -65.248115 0.266346 +VERTEX2 295 -13.697321 -64.954706 0.327047 +VERTEX2 296 -12.682665 -64.458291 0.585299 +VERTEX2 297 -12.350531 -64.019553 1.204511 +VERTEX2 298 -12.307365 -63.501044 1.734248 +VERTEX2 299 -12.767132 -62.226472 2.038537 +VERTEX2 300 -13.107059 -61.264499 2.026086 +VERTEX2 301 -13.455645 -60.253312 1.934507 +VERTEX2 302 -13.826390 -59.213417 1.899089 +VERTEX2 303 -14.195723 -58.291045 1.889085 +VERTEX2 304 -14.384925 -57.420633 1.870683 +VERTEX2 305 -14.743250 -56.342969 1.874592 +VERTEX2 306 -15.050991 -55.380521 1.904069 +VERTEX2 307 -15.321971 -54.326206 1.877283 +VERTEX2 308 -15.641208 -53.235931 1.852019 +VERTEX2 309 -15.900098 -52.241939 1.869238 +VERTEX2 310 -16.218969 -51.189182 1.836397 +VERTEX2 311 -16.569162 -50.267910 1.914266 +VERTEX2 312 -16.939306 -49.260797 2.117135 +VERTEX2 313 -17.247160 -48.909631 2.737219 +VERTEX2 314 -18.045510 -48.794033 -3.069096 +VERTEX2 315 -19.010128 -48.861748 -2.739084 +VERTEX2 316 -19.966724 -49.311821 -2.771330 +VERTEX2 317 -20.954526 -49.658232 -2.777384 +VERTEX2 318 -21.960740 -50.030088 -2.782633 +VERTEX2 319 -22.914312 -50.390089 -2.791163 +VERTEX2 320 -23.949308 -50.767074 -2.785230 +VERTEX2 321 -24.968616 -51.016281 -2.809777 +VERTEX2 322 -25.857685 -51.249006 -2.840584 +VERTEX2 323 -26.811996 -51.573479 -2.855088 +VERTEX2 324 -27.734998 -51.903940 -2.793065 +VERTEX2 325 -28.669162 -52.291260 -2.827645 +VERTEX2 326 -29.606613 -52.581147 -2.829850 +VERTEX2 327 -30.636488 -52.940678 -2.836468 +VERTEX2 328 -31.685042 -53.281576 -2.791907 +VERTEX2 329 -32.683471 -53.623201 -2.751609 +VERTEX2 330 -33.647676 -53.995110 -2.802525 +VERTEX2 331 -34.763515 -54.419046 -2.824483 +VERTEX2 332 -35.744629 -54.758219 -2.827822 +VERTEX2 333 -36.625337 -55.035796 -2.815258 +VERTEX2 334 -37.645755 -55.379659 -2.813822 +VERTEX2 335 -38.601433 -55.764130 -2.824954 +VERTEX2 336 -39.592643 -56.059759 -2.840291 +VERTEX2 337 -40.568189 -56.446263 -2.738817 +VERTEX2 338 -41.605547 -56.799473 -2.780067 +VERTEX2 339 -42.565839 -57.082565 -2.778245 +VERTEX2 340 -43.740740 -57.361379 -2.787918 +VERTEX2 341 -44.643424 -57.591320 -2.805531 +VERTEX2 342 -45.671401 -57.941805 -2.811499 +VERTEX2 343 -46.615148 -58.211402 -2.780536 +VERTEX2 344 -47.542416 -58.477735 -2.773363 +VERTEX2 345 -48.717285 -58.880747 -2.796343 +VERTEX2 346 -49.797117 -59.238327 -2.812705 +VERTEX2 347 -50.654067 -59.557317 -2.686396 +VERTEX2 348 -51.464402 -59.897886 -2.666487 +VERTEX2 349 -52.467840 -60.294421 -2.747878 +VERTEX2 350 -53.501070 -60.774773 -2.757943 +VERTEX2 351 -54.520158 -61.121251 -2.761028 +VERTEX2 352 -55.357342 -61.407430 -2.806216 +VERTEX2 353 -56.404943 -61.748541 -2.812735 +VERTEX2 354 -57.367798 -62.069701 -2.824076 +VERTEX2 355 -58.418551 -62.342374 -2.826832 +VERTEX2 356 -59.421110 -62.757961 -2.846309 +VERTEX2 357 -60.311076 -63.105122 -2.862816 +VERTEX2 358 -61.285948 -63.381475 -2.820296 +VERTEX2 359 -62.349723 -63.819694 -2.701418 +VERTEX2 360 -63.347127 -64.138640 -2.788849 +VERTEX2 361 -64.290573 -64.489736 -2.789909 +VERTEX2 362 -65.373587 -64.783572 -2.789702 +VERTEX2 363 -66.363595 -65.093395 -2.804221 +VERTEX2 364 -67.461166 -65.429777 -2.815342 +VERTEX2 365 -68.477978 -65.761979 -2.847690 +VERTEX2 366 -69.477031 -66.081591 -2.881290 +VERTEX2 367 -70.466928 -66.302183 -2.882588 +VERTEX2 368 -71.401807 -66.538565 -2.888061 +VERTEX2 369 -72.396438 -66.721685 -2.900583 +VERTEX2 370 -73.354350 -66.974283 -2.907596 +VERTEX2 371 -74.366864 -67.299699 -2.899617 +VERTEX2 372 -75.394338 -67.581346 -2.906070 +VERTEX2 373 -76.289324 -67.780432 -2.878256 +VERTEX2 374 -77.319981 -67.997089 -2.902894 +VERTEX2 375 -78.354677 -68.264954 -2.858804 +VERTEX2 376 -79.463513 -68.560297 -2.859194 +VERTEX2 377 -80.309924 -68.865896 -2.869112 +VERTEX2 378 -81.376150 -69.196455 -2.868809 +VERTEX2 379 -82.431554 -69.430631 -2.868779 +VERTEX2 380 -83.363386 -69.702588 -2.868053 +VERTEX2 381 -84.394886 -70.028866 -2.845347 +VERTEX2 382 -85.363300 -70.243487 -2.839324 +VERTEX2 383 -86.263817 -70.464716 -2.842727 +VERTEX2 384 -87.256148 -70.813633 -2.841378 +VERTEX2 385 -88.209188 -71.074882 -2.858859 +VERTEX2 386 -89.280059 -71.316665 -2.863231 +VERTEX2 387 -90.279482 -71.611132 -2.858954 +VERTEX2 388 -91.316182 -71.831996 -2.860275 +VERTEX2 389 -92.299913 -72.132681 -2.864060 +VERTEX2 390 -93.322668 -72.430282 -2.859610 +VERTEX2 391 -94.551864 -72.746774 -2.828931 +VERTEX2 392 -95.494485 -73.022126 -2.833226 +VERTEX2 393 -96.471659 -73.323917 -2.838208 +VERTEX2 394 -97.587670 -73.703045 -2.834397 +VERTEX2 395 -98.506850 -74.007872 -2.816562 +VERTEX2 396 -99.464495 -74.360131 -2.810470 +VERTEX2 397 -100.401307 -74.643576 -2.808508 +VERTEX2 398 -101.475441 -74.916531 -2.819985 +VERTEX2 399 -102.449168 -75.150800 -2.841903 +VERTEX2 400 -103.393805 -75.423058 -2.761160 +VERTEX2 401 -104.306436 -75.775338 -2.738045 +VERTEX2 402 -105.269967 -76.239515 -2.656362 +VERTEX2 403 -106.207714 -76.754555 -2.672241 +VERTEX2 404 -107.185927 -77.158800 -2.765370 +VERTEX2 405 -108.334604 -77.504302 -2.829966 +VERTEX2 406 -109.165292 -77.767718 -2.826012 +VERTEX2 407 -110.133838 -78.001368 -2.817600 +VERTEX2 408 -111.152102 -78.342920 -2.819934 +VERTEX2 409 -112.096928 -78.625696 -2.828763 +VERTEX2 410 -113.073267 -78.893445 -2.853484 +VERTEX2 411 -113.935820 -79.042937 -2.956660 +VERTEX2 412 -114.955614 -79.291628 -2.777161 +VERTEX2 413 -115.954776 -79.545771 -2.775916 +VERTEX2 414 -116.840698 -79.864052 -2.783121 +VERTEX2 415 -117.735615 -80.200549 -2.791338 +VERTEX2 416 -118.785184 -80.516079 -2.789667 +VERTEX2 417 -119.799362 -80.866113 -2.783250 +VERTEX2 418 -120.853026 -81.160604 -2.793145 +VERTEX2 419 -121.806709 -81.477627 -2.676826 +VERTEX2 420 -122.404192 -81.979328 -2.160181 +VERTEX2 421 -122.525921 -82.393581 -1.505018 +VERTEX2 422 -122.100500 -83.335251 -1.092563 +VERTEX2 423 -121.634188 -84.272987 -1.075451 +VERTEX2 424 -121.238665 -85.155201 -1.066227 +VERTEX2 425 -120.769919 -86.116573 -1.134072 +VERTEX2 426 -120.277623 -87.014785 -1.143347 +VERTEX2 427 -119.860612 -88.046759 -1.207475 +VERTEX2 428 -119.488261 -89.036845 -1.201393 +VERTEX2 429 -119.141321 -89.999387 -1.187910 +VERTEX2 430 -118.690631 -90.972973 -1.183429 +VERTEX2 431 -118.305911 -91.898478 -1.148986 +VERTEX2 432 -117.812862 -92.937662 -1.188610 +VERTEX2 433 -117.486094 -93.909582 -1.283120 +VERTEX2 434 -117.251104 -94.949995 -1.271953 +VERTEX2 435 -117.010378 -95.978696 -1.261249 +VERTEX2 436 -116.659865 -97.050539 -1.238549 +VERTEX2 437 -116.530348 -97.519240 -0.636845 +VERTEX2 438 -116.271485 -97.609004 -0.140884 +VERTEX2 439 -116.033700 -97.619534 0.333524 +VERTEX2 440 -114.970547 -97.345580 0.316875 +VERTEX2 441 -113.982889 -97.077404 0.314081 +VERTEX2 442 -113.010190 -96.863703 0.307977 +VERTEX2 443 -111.828566 -96.571635 0.305587 +VERTEX2 444 -110.740313 -96.257205 0.299786 +VERTEX2 445 -109.912121 -95.988505 0.294562 +VERTEX2 446 -108.763914 -95.775156 0.269697 +VERTEX2 447 -107.778337 -95.659582 0.147659 +VERTEX2 448 -106.702039 -95.491727 0.056783 +VERTEX2 449 -105.625273 -95.274742 0.201571 +VERTEX2 450 -104.613708 -94.852696 0.282592 +VERTEX2 451 -103.630140 -94.552234 0.307289 +VERTEX2 452 -102.609098 -94.194334 0.299115 +VERTEX2 453 -101.533016 -93.850159 0.295830 +VERTEX2 454 -100.654150 -93.657524 0.292570 +VERTEX2 455 -99.654429 -93.302190 0.292456 +VERTEX2 456 -98.656555 -93.116380 0.288344 +VERTEX2 457 -97.560024 -92.852199 0.272574 +VERTEX2 458 -96.474108 -92.517907 0.281438 +VERTEX2 459 -95.554421 -92.261763 0.262333 +VERTEX2 460 -94.524320 -92.052250 0.251707 +VERTEX2 461 -93.520459 -91.823818 0.253978 +VERTEX2 462 -92.504607 -91.463767 0.249691 +VERTEX2 463 -91.552805 -91.298112 0.265374 +VERTEX2 464 -90.489136 -91.082643 0.295261 +VERTEX2 465 -89.461634 -90.783796 0.273007 +VERTEX2 466 -88.352773 -90.523144 0.335303 +VERTEX2 467 -87.383756 -90.155482 0.319717 +VERTEX2 468 -86.348674 -89.949180 0.304350 +VERTEX2 469 -85.405215 -89.692956 0.305295 +VERTEX2 470 -84.403289 -89.417853 0.275600 +VERTEX2 471 -83.287523 -89.090927 0.261866 +VERTEX2 472 -82.245317 -88.788002 0.240111 +VERTEX2 473 -81.091911 -88.584699 0.212399 +VERTEX2 474 -79.954916 -88.296884 0.223371 +VERTEX2 475 -78.902878 -88.010047 0.276296 +VERTEX2 476 -77.878425 -87.714523 0.260738 +VERTEX2 477 -76.839364 -87.469304 0.262543 +VERTEX2 478 -75.781851 -87.190568 0.246004 +VERTEX2 479 -74.810395 -87.009223 0.239643 +VERTEX2 480 -73.772177 -86.819963 0.229672 +VERTEX2 481 -72.660271 -86.556892 0.254016 +VERTEX2 482 -71.600055 -86.271103 0.338287 +VERTEX2 483 -70.554695 -85.956794 0.312668 +VERTEX2 484 -69.517694 -85.634982 0.283189 +VERTEX2 485 -68.445508 -85.370957 0.263992 +VERTEX2 486 -67.258261 -85.124540 0.254417 +VERTEX2 487 -66.254665 -84.914176 0.227185 +VERTEX2 488 -65.295153 -84.639537 0.446126 +VERTEX2 489 -64.416513 -84.086980 0.837882 +VERTEX2 490 -63.947808 -83.311818 1.332931 +VERTEX2 491 -63.967622 -82.382716 1.760926 +VERTEX2 492 -64.351218 -81.343636 2.149314 +VERTEX2 493 -64.812628 -80.387914 2.063290 +VERTEX2 494 -65.237382 -79.315906 1.916126 +VERTEX2 495 -65.539808 -78.280680 1.873054 +VERTEX2 496 -65.887575 -77.202503 1.842686 +VERTEX2 497 -66.179205 -76.115875 1.876578 +VERTEX2 498 -66.351393 -74.966689 1.846495 +VERTEX2 499 -66.611589 -73.997576 1.805227 +VERTEX2 500 -66.760863 -72.926542 1.771933 +VERTEX2 501 -67.003874 -71.914743 1.756479 +VERTEX2 502 -67.074995 -70.982801 1.765324 +VERTEX2 503 -67.352203 -69.968426 1.883117 +VERTEX2 504 -67.684963 -69.023235 2.158900 +VERTEX2 505 -68.233412 -68.638537 2.685893 +VERTEX2 506 -68.755230 -68.512729 -3.067849 +VERTEX2 507 -69.808973 -68.585665 -3.038018 +VERTEX2 508 -70.867326 -68.652298 -2.872126 +VERTEX2 509 -71.856746 -68.917940 -2.869189 +VERTEX2 510 -72.959057 -69.140685 -2.849472 +VERTEX2 511 -74.011049 -69.340544 -2.832776 +VERTEX2 512 -74.998957 -69.689818 -2.847269 +VERTEX2 513 -76.016406 -69.972757 -2.821392 +VERTEX2 514 -76.921881 -70.268314 -2.801662 +VERTEX2 515 -77.989918 -70.713739 -2.778695 +VERTEX2 516 -78.842617 -70.983022 -2.706869 +VERTEX2 517 -79.902111 -71.387415 -3.135365 +VERTEX2 518 -80.969278 -71.339046 -2.990904 +VERTEX2 519 -81.996331 -71.514366 -2.992369 +VERTEX2 520 -83.002764 -71.607539 -2.989518 +VERTEX2 521 -83.963072 -71.755488 -2.971855 +VERTEX2 522 -85.051528 -71.911363 -2.962141 +VERTEX2 523 -86.187892 -72.162016 -2.970519 +VERTEX2 524 -87.084952 -72.409509 -2.898577 +VERTEX2 525 -88.025612 -72.616575 -2.888932 +VERTEX2 526 -88.991999 -72.812411 -2.865426 +VERTEX2 527 -89.966629 -73.156086 -2.830986 +VERTEX2 528 -90.929668 -73.542939 -2.847589 +VERTEX2 529 -91.985701 -73.773976 -2.924275 +VERTEX2 530 -92.956889 -73.969245 -2.903332 +VERTEX2 531 -93.906909 -74.114183 -2.909689 +VERTEX2 532 -94.948700 -74.329242 -2.899005 +VERTEX2 533 -95.950209 -74.599380 -2.888248 +VERTEX2 534 -97.025433 -74.875535 -2.880659 +VERTEX2 535 -98.111343 -75.122055 -2.878848 +VERTEX2 536 -99.108147 -75.327683 -2.969054 +VERTEX2 537 -100.073240 -75.402257 3.078750 +VERTEX2 538 -101.129847 -75.284034 2.737425 +VERTEX2 539 -101.952366 -74.625707 2.384641 +VERTEX2 540 -102.604433 -73.813172 2.012275 +VERTEX2 541 -103.029329 -72.818571 1.920817 +VERTEX2 542 -103.401930 -71.693815 1.902164 +VERTEX2 543 -103.710438 -70.660711 1.878746 +VERTEX2 544 -103.908848 -69.524102 1.849152 +VERTEX2 545 -104.176412 -68.612237 1.968753 +VERTEX2 546 -104.429854 -67.588531 1.860674 +VERTEX2 547 -104.666129 -66.615013 1.890992 +VERTEX2 548 -104.882911 -65.476500 1.878909 +VERTEX2 549 -105.185364 -64.508857 1.855687 +VERTEX2 550 -105.428166 -63.577221 1.879636 +VERTEX2 551 -105.744978 -62.524033 1.857817 +VERTEX2 552 -106.048992 -61.528199 1.834538 +VERTEX2 553 -106.314721 -60.533577 1.803126 +VERTEX2 554 -106.552545 -59.506438 1.675303 +VERTEX2 555 -106.397934 -58.902067 1.148807 +VERTEX2 556 -105.596407 -58.372950 0.756911 +VERTEX2 557 -105.110043 -57.799544 1.433081 +VERTEX2 558 -105.177069 -56.906768 1.898152 +VERTEX2 559 -105.568282 -55.970256 1.940126 +VERTEX2 560 -105.865081 -54.983479 1.934070 +VERTEX2 561 -106.233284 -53.937320 1.887215 +VERTEX2 562 -106.487323 -52.974296 1.875360 +VERTEX2 563 -106.743953 -51.865478 1.862810 +VERTEX2 564 -107.003911 -50.868726 1.872531 +VERTEX2 565 -107.309615 -49.926669 1.908013 +VERTEX2 566 -107.600218 -48.963449 1.894498 +VERTEX2 567 -107.788954 -48.044644 1.873391 +VERTEX2 568 -108.081633 -46.951901 1.843183 +VERTEX2 569 -108.319113 -45.898072 1.902168 +VERTEX2 570 -108.630981 -44.856381 1.889333 +VERTEX2 571 -108.974921 -43.866933 1.880496 +VERTEX2 572 -109.314267 -42.947803 1.895430 +VERTEX2 573 -109.701262 -41.899061 1.882194 +VERTEX2 574 -110.079134 -40.947382 1.885853 +VERTEX2 575 -110.346825 -39.937017 1.863635 +VERTEX2 576 -110.633357 -38.953478 1.838944 +VERTEX2 577 -110.836683 -37.985728 1.848779 +VERTEX2 578 -111.148772 -37.051086 1.901337 +VERTEX2 579 -111.404324 -36.002684 1.726155 +VERTEX2 580 -111.319936 -35.559247 1.166188 +VERTEX2 581 -111.077026 -35.143342 0.520643 +VERTEX2 582 -110.052920 -34.599052 0.460843 +VERTEX2 583 -109.084563 -34.220492 0.444239 +VERTEX2 584 -108.294499 -33.787602 0.458626 +VERTEX2 585 -107.711043 -33.497807 1.120686 +VERTEX2 586 -107.664564 -33.201528 1.680261 +VERTEX2 587 -107.970290 -32.538280 2.104497 +VERTEX2 588 -108.475139 -31.618843 1.984801 +VERTEX2 589 -108.898674 -30.589889 1.871449 +VERTEX2 590 -109.108419 -29.685800 1.839961 +VERTEX2 591 -109.468562 -28.850533 2.532727 +VERTEX2 592 -109.762195 -28.735937 3.127220 +VERTEX2 593 -110.222247 -28.820095 -2.558082 +VERTEX2 594 -110.510896 -28.995300 -2.000138 +VERTEX2 595 -110.841526 -29.887642 -2.084611 +VERTEX2 596 -111.122638 -30.181460 -2.788179 +VERTEX2 597 -111.568507 -30.148903 2.996387 +VERTEX2 598 -111.651673 -30.001401 2.472655 +VERTEX2 599 -111.815031 -29.862034 1.947609 +VERTEX2 600 -111.854984 -29.647703 1.430593 +VERTEX2 601 -111.832530 -29.528429 0.923593 +VERTEX2 602 -110.965531 -28.912671 0.485813 +VERTEX2 603 -110.028603 -28.389575 0.435080 +VERTEX2 604 -109.166882 -27.973504 0.400372 +VERTEX2 605 -108.140426 -27.478618 0.381822 +VERTEX2 606 -107.108782 -27.166123 0.359235 +VERTEX2 607 -106.037706 -26.857676 0.312218 +VERTEX2 608 -105.102787 -26.594767 0.278640 +VERTEX2 609 -103.982409 -26.444375 0.240721 +VERTEX2 610 -103.125598 -26.211697 0.318973 +VERTEX2 611 -102.163147 -26.015194 0.288089 +VERTEX2 612 -101.087298 -25.701239 0.338070 +VERTEX2 613 -100.101429 -25.404698 0.327750 +VERTEX2 614 -99.104659 -25.168220 0.285805 +VERTEX2 615 -98.124757 -24.843206 0.381403 +VERTEX2 616 -97.046619 -24.473080 0.345336 +VERTEX2 617 -96.157123 -24.166004 0.329151 +VERTEX2 618 -95.106203 -23.862626 0.308324 +VERTEX2 619 -94.275751 -23.611608 0.307317 +VERTEX2 620 -93.259952 -23.354988 0.274329 +VERTEX2 621 -92.135857 -23.069997 0.231365 +VERTEX2 622 -91.081499 -22.821736 0.200989 +VERTEX2 623 -90.126321 -22.412047 0.631742 +VERTEX2 624 -89.228267 -21.897857 0.604240 +VERTEX2 625 -88.304561 -21.325576 0.599738 +VERTEX2 626 -87.410055 -20.746368 0.569630 +VERTEX2 627 -86.489654 -20.131088 0.548060 +VERTEX2 628 -85.505729 -19.557210 0.525367 +VERTEX2 629 -84.633418 -19.185174 0.515931 +VERTEX2 630 -83.678141 -18.643114 0.509814 +VERTEX2 631 -82.701141 -18.225433 0.361358 +VERTEX2 632 -81.665417 -17.854734 0.352621 +VERTEX2 633 -80.656813 -17.536604 0.323348 +VERTEX2 634 -79.755351 -17.292909 0.373979 +VERTEX2 635 -78.790119 -16.945118 0.363213 +VERTEX2 636 -77.839006 -16.567066 0.445756 +VERTEX2 637 -76.852276 -16.105896 0.463497 +VERTEX2 638 -75.803326 -15.653064 0.435638 +VERTEX2 639 -74.753014 -15.237590 0.421462 +VERTEX2 640 -73.775488 -14.802358 0.382056 +VERTEX2 641 -72.759074 -14.452834 0.359852 +VERTEX2 642 -71.967813 -14.239521 0.315512 +VERTEX2 643 -70.902478 -13.956313 0.396024 +VERTEX2 644 -70.003909 -13.573342 0.392040 +VERTEX2 645 -69.045851 -13.294735 0.374075 +VERTEX2 646 -68.016651 -12.924330 0.431343 +VERTEX2 647 -67.188457 -12.515408 0.407105 +VERTEX2 648 -66.296011 -12.055641 0.379763 +VERTEX2 649 -65.261689 -11.711285 0.435254 +VERTEX2 650 -64.350136 -11.270179 0.475965 +VERTEX2 651 -63.398817 -10.917403 0.463267 +VERTEX2 652 -62.529620 -10.386244 0.453433 +VERTEX2 653 -61.554106 -9.999061 0.440119 +VERTEX2 654 -60.579618 -9.587226 0.426824 +VERTEX2 655 -59.680856 -9.182081 0.491746 +VERTEX2 656 -58.686904 -8.793215 0.487355 +VERTEX2 657 -57.800438 -8.393581 0.460693 +VERTEX2 658 -56.683020 -7.843101 0.443449 +VERTEX2 659 -55.750795 -7.446201 0.417142 +VERTEX2 660 -54.825190 -7.068476 0.414108 +VERTEX2 661 -53.884711 -6.707555 0.408105 +VERTEX2 662 -52.897798 -6.316469 0.397160 +VERTEX2 663 -51.796771 -5.901225 0.439595 +VERTEX2 664 -50.866984 -5.548376 0.373425 +VERTEX2 665 -49.923106 -5.182480 0.470981 +VERTEX2 666 -49.050733 -4.858071 0.473040 +VERTEX2 667 -47.970537 -4.227156 0.457692 +VERTEX2 668 -46.996670 -3.804165 0.451184 +VERTEX2 669 -46.174304 -3.395310 0.450266 +VERTEX2 670 -45.310984 -3.097879 0.431272 +VERTEX2 671 -44.315055 -2.662630 0.411587 +VERTEX2 672 -43.327833 -2.272489 0.283900 +VERTEX2 673 -42.811669 -2.224422 0.613759 +VERTEX2 674 -41.817559 -1.518084 0.684466 +VERTEX2 675 -41.065164 -0.923368 0.521682 +VERTEX2 676 -40.070200 -0.456129 0.501458 +VERTEX2 677 -39.141200 -0.047370 0.553041 +VERTEX2 678 -38.118484 0.571249 0.548286 +VERTEX2 679 -37.355088 1.136513 0.584394 +VERTEX2 680 -36.515583 1.724321 0.578423 +VERTEX2 681 -35.680713 2.224792 0.411873 +VERTEX2 682 -34.673200 2.711955 0.422368 +VERTEX2 683 -33.645753 3.152159 0.409265 +VERTEX2 684 -32.778581 3.456995 0.427245 +VERTEX2 685 -31.757561 4.010273 0.485273 +VERTEX2 686 -30.846757 4.482206 0.483396 +VERTEX2 687 -29.872637 4.926649 0.470360 +VERTEX2 688 -28.853373 5.317735 0.456389 +VERTEX2 689 -27.971623 5.832271 0.528129 +VERTEX2 690 -26.971383 6.225382 0.510023 +VERTEX2 691 -26.126650 6.727704 0.569899 +VERTEX2 692 -25.257615 7.297352 0.577179 +VERTEX2 693 -24.441648 7.778022 0.563690 +VERTEX2 694 -23.550347 8.308159 0.524783 +VERTEX2 695 -22.552085 8.758487 0.293424 +VERTEX2 696 -22.189434 8.687772 -0.212522 +VERTEX2 697 -21.782485 8.559746 -0.777155 +VERTEX2 698 -21.219945 7.637630 -1.054431 +VERTEX2 699 -20.734626 6.727793 -1.097394 +VERTEX2 700 -20.368218 5.749727 -1.134107 +VERTEX2 701 -19.950066 4.827813 -1.168915 +VERTEX2 702 -19.557526 3.834601 -1.193655 +VERTEX2 703 -19.249352 2.688149 -1.208006 +VERTEX2 704 -18.866008 1.754947 -1.122535 +VERTEX2 705 -18.484742 0.795002 -1.056407 +VERTEX2 706 -17.938993 -0.022524 -0.789348 +VERTEX2 707 -17.198693 -0.759250 -0.776826 +VERTEX2 708 -16.378434 -1.417847 -0.486710 +VERTEX2 709 -15.421314 -1.718888 -0.088855 +VERTEX2 710 -14.319095 -1.786181 -0.087073 +VERTEX2 711 -13.314771 -1.770520 0.326239 +VERTEX2 712 -12.458286 -1.255326 0.797255 +VERTEX2 713 -11.812364 -0.501557 0.939701 +VERTEX2 714 -10.927888 0.144840 0.572651 +VERTEX2 715 -9.944920 0.576655 0.495011 +VERTEX2 716 -8.973795 1.005967 0.485189 +VERTEX2 717 -8.085445 1.462267 0.461218 +VERTEX2 718 -7.217548 1.827593 0.460865 +VERTEX2 719 -6.365436 2.288586 0.468044 +VERTEX2 720 -5.383691 2.787576 0.476460 +VERTEX2 721 -4.453776 3.286768 0.486655 +VERTEX2 722 -3.491320 3.726062 0.492256 +VERTEX2 723 -2.541684 4.155684 0.483398 +VERTEX2 724 -1.705996 4.373351 -0.172852 +VERTEX2 725 -1.447211 4.271539 -0.773921 +VERTEX2 726 -0.811860 3.562916 -0.874747 +VERTEX2 727 -0.283408 2.627357 -1.147143 +VERTEX2 728 0.185530 1.761043 -1.159123 +VERTEX2 729 0.606469 0.685242 -1.190632 +VERTEX2 730 0.968776 -0.268902 -1.216110 +VERTEX2 731 1.212882 -1.259252 -1.177476 +VERTEX2 732 1.632965 -2.201782 -1.119937 +VERTEX2 733 2.045327 -3.118735 -1.066650 +VERTEX2 734 2.524432 -3.952661 -1.065459 +VERTEX2 735 2.951064 -4.842543 -1.090504 +VERTEX2 736 3.396455 -5.807293 -0.996971 +VERTEX2 737 4.005359 -6.811348 -1.036576 +VERTEX2 738 4.507774 -7.658014 -1.051568 +VERTEX2 739 5.011099 -8.599866 -1.072978 +VERTEX2 740 5.467110 -9.556987 -1.099400 +VERTEX2 741 5.925610 -10.466247 -1.091965 +VERTEX2 742 6.348505 -11.406837 -1.093282 +VERTEX2 743 6.818450 -12.404328 -1.120436 +VERTEX2 744 7.169369 -13.294367 -1.069367 +VERTEX2 745 7.638252 -14.335852 -1.086525 +VERTEX2 746 8.062189 -15.173614 -1.088559 +VERTEX2 747 8.513022 -16.061276 -1.130711 +VERTEX2 748 8.929249 -17.203375 -1.141100 +VERTEX2 749 9.296888 -18.222661 -1.154784 +VERTEX2 750 9.740918 -19.183007 -1.128882 +VERTEX2 751 10.044635 -20.198984 -1.154525 +VERTEX2 752 10.510465 -20.992261 -1.169729 +VERTEX2 753 10.826988 -21.937892 -1.145905 +VERTEX2 754 11.318861 -23.029909 -1.179778 +VERTEX2 755 11.683275 -23.849937 -1.140554 +VERTEX2 756 12.055945 -24.827353 -1.175322 +VERTEX2 757 12.576836 -25.854005 -1.199057 +VERTEX2 758 12.925771 -26.904804 -1.106526 +VERTEX2 759 13.384546 -27.808906 -1.057045 +VERTEX2 760 13.848898 -28.705783 -1.055641 +VERTEX2 761 14.281456 -29.458175 -1.064876 +VERTEX2 762 14.729941 -30.464058 -1.089387 +VERTEX2 763 15.327283 -31.515181 -1.098943 +VERTEX2 764 15.741869 -32.439606 -1.108561 +VERTEX2 765 16.146131 -33.454788 -1.052516 +VERTEX2 766 16.641985 -34.402668 -1.100241 +VERTEX2 767 17.066336 -35.300430 -1.120158 +VERTEX2 768 17.473492 -36.247261 -1.112094 +VERTEX2 769 17.944936 -37.234178 -1.123777 +VERTEX2 770 18.363571 -38.138326 -1.123265 +VERTEX2 771 18.747752 -39.152808 -1.124384 +VERTEX2 772 19.291662 -40.109767 -1.050114 +VERTEX2 773 19.640831 -40.718428 -1.617973 +VERTEX2 774 19.537131 -41.018139 -2.162860 +VERTEX2 775 18.750140 -41.582134 -2.559061 +VERTEX2 776 17.835831 -42.106142 -2.599522 +VERTEX2 777 16.879144 -42.462374 -2.778435 +VERTEX2 778 15.936466 -42.883720 -2.817129 +VERTEX2 779 14.877790 -43.226130 -2.791989 +VERTEX2 780 13.996095 -43.438214 -2.831797 +VERTEX2 781 12.979602 -43.718964 -2.860836 +VERTEX2 782 11.935909 -43.931873 -2.898337 +VERTEX2 783 10.939856 -44.249941 -2.801934 +VERTEX2 784 9.947140 -44.586437 -2.847040 +VERTEX2 785 8.967427 -44.870618 -2.806426 +VERTEX2 786 7.923985 -45.198071 -2.866464 +VERTEX2 787 6.849836 -45.585675 -2.907209 +VERTEX2 788 5.894695 -45.650119 2.768152 +VERTEX2 789 5.688715 -45.592271 2.205952 +VERTEX2 790 5.607919 -45.444992 1.611997 +VERTEX2 791 5.786098 -44.706961 1.600640 +VERTEX2 792 5.630210 -44.450801 2.151285 +VERTEX2 793 5.461889 -44.256325 2.963387 +VERTEX2 794 5.124760 -44.253473 -2.727019 +VERTEX2 795 4.944484 -44.443694 -2.068051 +VERTEX2 796 4.998977 -44.595563 -1.364575 +VERTEX2 797 5.449370 -45.206989 -0.928455 +VERTEX2 798 5.827265 -45.883001 -1.026469 +VERTEX2 799 5.943369 -46.060328 -0.430101 +VERTEX2 800 6.169236 -46.147640 0.250950 +VERTEX2 801 7.122424 -45.743814 0.331225 +VERTEX2 802 8.144669 -45.523534 0.209232 +VERTEX2 803 9.130019 -45.221806 0.224335 +VERTEX2 804 10.069618 -45.254480 -0.374940 +VERTEX2 805 10.356856 -45.425749 -0.980225 +VERTEX2 806 10.437160 -45.822298 -1.573649 +VERTEX2 807 10.360942 -46.382483 -1.288699 +VERTEX2 808 10.777828 -47.425253 -1.177600 +VERTEX2 809 11.134366 -48.389276 -1.179777 +VERTEX2 810 11.462050 -49.306445 -1.229629 +VERTEX2 811 11.830209 -50.370802 -1.237418 +VERTEX2 812 12.128135 -51.271544 -1.219833 +VERTEX2 813 12.492008 -52.300605 -1.219328 +VERTEX2 814 12.642317 -53.363986 -1.418919 +VERTEX2 815 12.838250 -54.430023 -1.284315 +VERTEX2 816 13.119712 -55.436655 -1.265238 +VERTEX2 817 13.377569 -56.383606 -1.258136 +VERTEX2 818 13.760895 -57.249105 -1.224672 +VERTEX2 819 14.077886 -57.836516 -0.546752 +VERTEX2 820 14.332954 -57.952482 0.150566 +VERTEX2 821 15.307246 -57.799498 0.157393 +VERTEX2 822 16.341858 -57.533678 0.295936 +VERTEX2 823 17.271065 -57.317391 0.326615 +VERTEX2 824 18.182750 -57.006414 0.323216 +VERTEX2 825 19.223871 -56.788052 0.254628 +VERTEX2 826 20.279076 -56.502109 0.269024 +VERTEX2 827 21.257419 -56.187679 0.386444 +VERTEX2 828 22.227110 -55.859091 0.419157 +VERTEX2 829 23.222713 -55.285575 0.457027 +VERTEX2 830 24.228600 -54.925252 0.313156 +VERTEX2 831 25.200817 -54.650402 0.341220 +VERTEX2 832 26.204626 -54.303954 0.344306 +VERTEX2 833 27.183480 -53.960582 0.369962 +VERTEX2 834 28.165407 -53.548729 0.289874 +VERTEX2 835 29.190664 -53.228397 0.282363 +VERTEX2 836 30.107979 -52.929679 0.307685 +VERTEX2 837 31.041040 -52.551175 0.314767 +VERTEX2 838 31.958542 -52.184897 0.297063 +VERTEX2 839 32.908485 -51.956040 0.312139 +VERTEX2 840 33.916711 -51.534564 0.754835 +VERTEX2 841 34.046040 -51.277476 1.403099 +VERTEX2 842 34.015555 -51.065587 2.093642 +VERTEX2 843 33.575675 -50.108243 1.862024 +VERTEX2 844 33.262949 -49.140277 1.853157 +VERTEX2 845 33.090561 -47.987251 1.779603 +VERTEX2 846 32.933591 -47.039269 1.752036 +VERTEX2 847 32.835633 -45.936983 1.756328 +VERTEX2 848 32.740798 -44.799405 1.757304 +VERTEX2 849 32.616613 -43.877984 1.743434 +VERTEX2 850 32.360095 -42.826519 1.849266 +VERTEX2 851 32.021743 -41.768567 1.840338 +VERTEX2 852 31.827693 -40.701953 1.847123 +VERTEX2 853 31.610466 -39.692694 1.823526 +VERTEX2 854 31.525194 -39.050372 2.399674 +VERTEX2 855 31.222914 -38.967948 3.054561 +VERTEX2 856 30.193841 -39.116811 -2.927786 +VERTEX2 857 29.243233 -39.253916 -2.944297 +VERTEX2 858 28.115913 -39.427968 -2.944044 +VERTEX2 859 27.171272 -39.568219 -2.961479 +VERTEX2 860 26.189022 -39.748217 -2.996831 +VERTEX2 861 25.184917 -39.933838 -2.941597 +VERTEX2 862 24.240487 -40.129709 -2.966714 +VERTEX2 863 23.181214 -40.275713 -2.782117 +VERTEX2 864 22.155443 -40.701105 -2.781931 +VERTEX2 865 21.347872 -40.894255 2.983378 +VERTEX2 866 21.234361 -40.723880 2.387985 +VERTEX2 867 21.074506 -40.458459 1.785964 +VERTEX2 868 20.847222 -39.472089 1.891497 +VERTEX2 869 20.573175 -38.470074 1.795151 +VERTEX2 870 20.270137 -37.595919 1.987924 +VERTEX2 871 19.830341 -36.690614 1.926972 +VERTEX2 872 19.506657 -35.648927 1.902051 +VERTEX2 873 19.226346 -34.674540 1.889643 +VERTEX2 874 18.858371 -33.635050 1.858227 +VERTEX2 875 18.598882 -32.563017 1.860551 +VERTEX2 876 18.289747 -31.474978 1.842094 +VERTEX2 877 18.045627 -30.276582 1.849678 +VERTEX2 878 17.911874 -29.488163 1.809213 +VERTEX2 879 17.639473 -28.488843 1.779384 +VERTEX2 880 17.376502 -27.574492 1.814367 +VERTEX2 881 17.093783 -26.651800 1.803557 +VERTEX2 882 16.768877 -25.602048 1.840457 +VERTEX2 883 16.498222 -24.661964 1.820856 +VERTEX2 884 16.184712 -23.671118 1.792262 +VERTEX2 885 16.022827 -22.576902 1.769776 +VERTEX2 886 15.730187 -21.579912 1.824583 +VERTEX2 887 15.441709 -20.602338 1.929090 +VERTEX2 888 15.108095 -19.603239 1.890715 +VERTEX2 889 14.769923 -18.570380 1.837150 +VERTEX2 890 14.548097 -17.526556 1.823625 +VERTEX2 891 14.326613 -16.572367 1.790793 +VERTEX2 892 13.995306 -15.407476 1.770434 +VERTEX2 893 13.842391 -14.466947 1.870411 +VERTEX2 894 13.508077 -13.415279 1.862701 +VERTEX2 895 13.189558 -12.448695 1.854285 +VERTEX2 896 12.943536 -11.453953 1.851105 +VERTEX2 897 12.826828 -10.375176 1.807489 +VERTEX2 898 12.466025 -9.105165 2.006027 +VERTEX2 899 12.138744 -8.247482 1.967578 +VERTEX2 900 11.799192 -7.322278 1.740761 +VERTEX2 901 11.767240 -6.458497 1.713103 +VERTEX2 902 11.617196 -6.031952 2.236776 +VERTEX2 903 11.337365 -5.808904 2.765123 +VERTEX2 904 11.049457 -5.732454 -2.973478 +VERTEX2 905 10.710275 -5.835707 -2.458054 +VERTEX2 906 10.328555 -6.328244 -1.940967 +VERTEX2 907 10.215747 -6.625746 -1.386622 +VERTEX2 908 10.410027 -7.229467 -0.826145 +VERTEX2 909 10.691902 -7.550665 -0.288539 +VERTEX2 910 11.047339 -7.657900 0.237066 +VERTEX2 911 11.401544 -7.497388 0.781544 +VERTEX2 912 11.624291 -7.083253 1.377170 +VERTEX2 913 11.628558 -6.794081 1.908955 +VERTEX2 914 11.314203 -6.391258 2.365578 +VERTEX2 915 10.538520 -5.797522 2.452131 +VERTEX2 916 10.037958 -5.016738 1.980252 +VERTEX2 917 9.874723 -3.976920 1.603182 +VERTEX2 918 9.762112 -2.924157 1.700292 +VERTEX2 919 9.537763 -1.814071 1.887701 +VERTEX2 920 9.260552 -0.789992 1.848260 +VERTEX2 921 9.017917 0.038960 1.824955 +VERTEX2 922 8.770520 1.002706 1.955158 +VERTEX2 923 8.355792 1.998094 1.942264 +VERTEX2 924 7.818028 2.925625 2.152752 +VERTEX2 925 7.365577 3.926398 1.964424 +VERTEX2 926 6.957535 4.894045 1.942004 +VERTEX2 927 6.572909 5.812353 2.389206 +VERTEX2 928 6.366747 6.018779 2.982606 +VERTEX2 929 5.874523 5.979527 -2.817885 +VERTEX2 930 4.905972 5.737518 -2.821163 +VERTEX2 931 3.978530 5.442234 -2.823458 +VERTEX2 932 3.059499 5.214363 -2.833339 +VERTEX2 933 2.125379 4.887542 -2.847395 +VERTEX2 934 1.086683 4.666774 -2.810571 +VERTEX2 935 0.089157 4.309178 -2.676197 +VERTEX2 936 -0.692631 3.882178 -2.704922 +VERTEX2 937 -1.640625 3.511432 -2.662882 +VERTEX2 938 -2.501364 3.007616 -2.640380 +VERTEX2 939 -3.557662 2.404287 -2.655211 +VERTEX2 940 -4.499287 1.952039 -2.670864 +VERTEX2 941 -5.455754 1.454198 -2.607059 +VERTEX2 942 -6.347579 0.950799 -2.603373 +VERTEX2 943 -7.337974 0.525728 -2.785958 +VERTEX2 944 -7.725044 0.446618 3.000490 +VERTEX2 945 -8.831563 0.623225 2.891560 +VERTEX2 946 -9.635375 1.111672 2.322818 +VERTEX2 947 -10.202500 1.957791 2.164150 +VERTEX2 948 -10.700863 2.826521 2.117414 +VERTEX2 949 -11.093832 3.819091 1.995883 +VERTEX2 950 -11.506849 4.827906 1.973593 +VERTEX2 951 -11.954524 5.669608 2.015571 +VERTEX2 952 -12.301900 6.513464 1.986005 +VERTEX2 953 -12.724617 7.430392 2.015901 +VERTEX2 954 -13.177916 8.316496 2.017274 +VERTEX2 955 -13.687522 9.197918 2.149674 +VERTEX2 956 -14.260878 10.091345 2.103036 +VERTEX2 957 -14.758709 10.881951 2.082076 +VERTEX2 958 -15.224537 11.816852 2.041826 +VERTEX2 959 -15.685922 12.728904 1.998046 +VERTEX2 960 -16.047475 13.668611 1.900640 +VERTEX2 961 -16.426683 14.638792 2.026097 +VERTEX2 962 -16.820505 15.621409 2.018174 +VERTEX2 963 -17.249213 16.561832 1.977608 +VERTEX2 964 -17.675635 17.554233 1.937436 +VERTEX2 965 -18.064695 18.604608 1.569098 +VERTEX2 966 -17.867612 19.600624 1.334269 +VERTEX2 967 -17.580105 20.637099 1.307593 +VERTEX2 968 -17.372341 21.711126 1.292724 +VERTEX2 969 -17.096180 22.803929 1.179156 +VERTEX2 970 -16.570328 23.640285 1.076787 +VERTEX2 971 -15.928560 24.518248 0.590199 +VERTEX2 972 -14.857016 24.918112 0.264754 +VERTEX2 973 -13.832273 25.158291 0.242519 +VERTEX2 974 -12.653131 25.379296 0.230371 +VERTEX2 975 -11.632195 25.689841 0.543659 +VERTEX2 976 -10.650228 26.214261 0.540982 +VERTEX2 977 -9.627430 26.696872 0.512193 +VERTEX2 978 -8.604595 27.257678 0.472758 +VERTEX2 979 -7.695458 27.706929 0.434596 +VERTEX2 980 -6.729587 28.144883 0.423763 +VERTEX2 981 -5.847336 28.534149 0.399525 +VERTEX2 982 -5.411524 28.767556 0.920800 +VERTEX2 983 -5.229821 29.081980 1.510847 +VERTEX2 984 -5.416699 30.061758 1.997364 +VERTEX2 985 -5.803625 31.008944 1.827384 +VERTEX2 986 -6.273948 32.539996 1.810853 +VERTEX2 987 -6.605560 33.264615 2.106543 +VERTEX2 988 -7.024798 34.199694 1.973091 +VERTEX2 989 -7.294046 35.023475 1.955598 +VERTEX2 990 -7.692316 36.084622 1.940789 +VERTEX2 991 -8.089086 37.117146 1.946995 +VERTEX2 992 -8.532466 38.148100 1.956177 +VERTEX2 993 -8.918437 39.115103 1.946284 +VERTEX2 994 -9.278693 40.097094 1.938543 +VERTEX2 995 -9.639592 41.135376 1.949090 +VERTEX2 996 -10.067404 42.239250 2.032140 +VERTEX2 997 -10.565370 43.291580 2.006754 +VERTEX2 998 -10.962801 44.237287 2.036893 +VERTEX2 999 -11.454667 45.187178 2.016306 +VERTEX2 1000 -11.847811 46.085056 1.966131 +VERTEX2 1001 -12.331706 47.089836 1.954851 +VERTEX2 1002 -12.648076 47.825923 1.942343 +VERTEX2 1003 -13.008559 48.775388 1.966320 +VERTEX2 1004 -13.425564 49.762603 1.945580 +VERTEX2 1005 -13.780507 50.788126 1.949102 +VERTEX2 1006 -14.110084 51.645687 1.919460 +VERTEX2 1007 -14.527918 52.694868 2.038122 +VERTEX2 1008 -15.021920 53.646588 2.037063 +VERTEX2 1009 -15.400400 54.547564 2.021402 +VERTEX2 1010 -15.742122 55.520356 1.717783 +VERTEX2 1011 -15.953325 56.486835 1.940145 +VERTEX2 1012 -16.356335 57.393700 1.985451 +VERTEX2 1013 -16.765672 58.310783 1.968080 +VERTEX2 1014 -17.064131 59.317438 1.970180 +VERTEX2 1015 -17.529794 60.544712 1.954119 +VERTEX2 1016 -17.820822 61.430267 1.954023 +VERTEX2 1017 -18.180066 62.260635 1.941742 +VERTEX2 1018 -18.589569 63.420748 1.944746 +VERTEX2 1019 -18.948467 64.500146 1.930335 +VERTEX2 1020 -19.276762 65.486368 1.950711 +VERTEX2 1021 -19.619527 66.592325 1.931619 +VERTEX2 1022 -19.978944 67.461043 1.927867 +VERTEX2 1023 -20.316204 68.520431 1.902544 +VERTEX2 1024 -20.646223 69.445223 1.892662 +VERTEX2 1025 -20.962935 70.566064 1.889259 +VERTEX2 1026 -21.187161 71.592070 1.794035 +VERTEX2 1027 -21.623042 72.677838 2.024902 +VERTEX2 1028 -21.987120 73.623739 2.042299 +VERTEX2 1029 -22.423894 74.582956 2.004787 +VERTEX2 1030 -22.790913 75.524710 1.992352 +VERTEX2 1031 -23.191623 76.472900 1.973783 +VERTEX2 1032 -23.553992 77.425574 1.964460 +VERTEX2 1033 -23.929125 78.544173 1.963692 +VERTEX2 1034 -24.264038 79.457993 1.985476 +VERTEX2 1035 -24.660779 80.484953 1.966639 +VERTEX2 1036 -25.088135 81.525625 1.962145 +VERTEX2 1037 -25.434869 82.425345 1.948874 +VERTEX2 1038 -25.769139 83.352200 1.927372 +VERTEX2 1039 -26.121357 84.325153 1.924494 +VERTEX2 1040 -26.419813 85.234626 1.906503 +VERTEX2 1041 -26.756720 86.271541 1.898345 +VERTEX2 1042 -26.973187 87.278571 1.894068 +VERTEX2 1043 -27.240824 88.288858 1.894550 +VERTEX2 1044 -27.526470 89.381912 1.897295 +VERTEX2 1045 -27.864168 90.271641 1.846584 +VERTEX2 1046 -28.160915 91.250847 2.043719 +VERTEX2 1047 -28.626187 92.094351 2.050906 +VERTEX2 1048 -29.081921 92.934070 2.026245 +VERTEX2 1049 -29.542465 94.045304 1.936116 +VERTEX2 1050 -30.009870 95.082392 1.928048 +VERTEX2 1051 -30.310723 96.064997 1.940592 +VERTEX2 1052 -30.579954 96.953159 1.935424 +VERTEX2 1053 -30.887061 97.943262 1.987327 +VERTEX2 1054 -31.223929 98.310803 2.653710 +VERTEX2 1055 -31.659574 98.335215 -2.883936 +VERTEX2 1056 -32.626689 98.002176 -2.869536 +VERTEX2 1057 -33.637580 97.712524 -2.826093 +VERTEX2 1058 -34.574819 97.454953 -2.839632 +VERTEX2 1059 -35.049464 97.331199 2.900532 +VERTEX2 1060 -35.397942 97.556540 2.208816 +VERTEX2 1061 -35.870796 98.408915 2.072980 +VERTEX2 1062 -36.297326 99.274043 2.068580 +VERTEX2 1063 -36.683967 100.204882 2.071153 +VERTEX2 1064 -37.212466 101.172889 2.164530 +VERTEX2 1065 -37.625718 102.089676 1.914936 +VERTEX2 1066 -37.940360 103.067945 1.985856 +VERTEX2 1067 -38.323161 104.017864 1.993000 +VERTEX2 1068 -38.726004 105.006767 1.996469 +VERTEX2 1069 -39.086972 105.959174 2.000283 +VERTEX2 1070 -39.494232 106.890603 1.788362 +VERTEX2 1071 -39.319719 107.912990 1.424471 +VERTEX2 1072 -39.123318 108.895817 1.405892 +VERTEX2 1073 -39.108845 109.837872 1.830495 +VERTEX2 1074 -39.324658 110.831941 1.828206 +VERTEX2 1075 -39.682475 111.930761 2.007835 +VERTEX2 1076 -40.204783 113.120336 2.025194 +VERTEX2 1077 -40.688636 114.070848 2.272601 +VERTEX2 1078 -41.204401 114.470795 2.776903 +VERTEX2 1079 -41.800698 114.555349 -2.975282 +VERTEX2 1080 -42.956086 114.113374 -2.670005 +VERTEX2 1081 -43.955945 113.494478 -2.657713 +VERTEX2 1082 -44.908845 113.049845 -2.665400 +VERTEX2 1083 -45.896084 112.607264 -2.653994 +VERTEX2 1084 -46.701447 112.082364 -2.619164 +VERTEX2 1085 -47.669411 111.613951 -2.800366 +VERTEX2 1086 -48.733927 111.228152 -2.771516 +VERTEX2 1087 -49.701990 110.929434 -2.802699 +VERTEX2 1088 -50.754755 110.571283 -2.764876 +VERTEX2 1089 -51.707414 110.276639 -2.732224 +VERTEX2 1090 -52.632316 109.893526 -2.720419 +VERTEX2 1091 -53.711410 109.495071 -2.680542 +VERTEX2 1092 -54.609122 109.047326 -2.633067 +VERTEX2 1093 -55.488943 108.567775 -2.610133 +VERTEX2 1094 -56.385156 108.113595 -2.591509 +VERTEX2 1095 -57.289250 107.585857 -2.576412 +VERTEX2 1096 -58.045455 107.110013 -2.623392 +VERTEX2 1097 -58.992338 106.647443 -2.626362 +VERTEX2 1098 -59.790945 106.191078 -2.605258 +VERTEX2 1099 -60.670421 105.614944 -2.643042 +VERTEX2 1100 -61.694317 105.114509 -2.632703 +VERTEX2 1101 -62.489848 104.650379 -2.616696 +VERTEX2 1102 -63.488722 104.190435 -2.668015 +VERTEX2 1103 -64.495715 103.647961 -2.679017 +VERTEX2 1104 -65.366746 103.139910 -2.653564 +VERTEX2 1105 -66.400191 102.632543 -2.630064 +VERTEX2 1106 -67.447570 102.039094 -2.606624 +VERTEX2 1107 -68.386976 101.486468 -2.611035 +VERTEX2 1108 -69.363619 100.983255 -2.713806 +VERTEX2 1109 -70.253142 100.671780 -2.708073 +VERTEX2 1110 -71.212442 100.351206 -2.650639 +VERTEX2 1111 -72.139548 99.847287 -2.621973 +VERTEX2 1112 -73.019635 99.363101 -2.622701 +VERTEX2 1113 -73.935766 98.951237 -2.606720 +VERTEX2 1114 -74.940495 98.407311 -2.593503 +VERTEX2 1115 -75.750570 97.787341 -2.356434 +VERTEX2 1116 -76.509888 97.074037 -2.157007 +VERTEX2 1117 -76.993707 96.122730 -1.911177 +VERTEX2 1118 -77.313797 95.149629 -1.716622 +VERTEX2 1119 -77.479689 94.085595 -1.598273 +VERTEX2 1120 -77.512538 93.056909 -1.692812 +VERTEX2 1121 -77.602104 92.026017 -1.725182 +VERTEX2 1122 -77.869322 91.047892 -1.765628 +VERTEX2 1123 -78.102325 90.036529 -1.734897 +VERTEX2 1124 -78.345399 88.976600 -1.705912 +VERTEX2 1125 -78.432474 88.046684 -1.331722 +VERTEX2 1126 -78.165849 87.013443 -1.146575 +VERTEX2 1127 -77.844390 86.341241 -0.599874 +VERTEX2 1128 -77.348419 85.912679 -1.203014 +VERTEX2 1129 -76.866587 85.121502 -1.011353 +VERTEX2 1130 -76.756069 84.888313 -1.557190 +VERTEX2 1131 -76.762478 84.804302 -2.186756 +VERTEX2 1132 -76.916294 84.748197 -2.864291 +VERTEX2 1133 -77.079382 84.856196 2.730831 +VERTEX2 1134 -77.226053 84.894878 2.056645 +VERTEX2 1135 -77.633667 85.712297 2.073238 +VERTEX2 1136 -77.878640 86.112089 1.848636 +VERTEX2 1137 -77.932302 86.043018 1.303583 +VERTEX2 1138 -77.922681 85.988794 0.794102 +VERTEX2 1139 -77.902176 85.934951 0.208011 +VERTEX2 1140 -77.871942 85.935937 -0.397860 +VERTEX2 1141 -77.919788 85.902911 -0.905355 +VERTEX2 1142 -77.165178 84.949654 -0.891659 +VERTEX2 1143 -76.610999 84.133170 -1.009487 +VERTEX2 1144 -76.195549 83.282356 -1.180583 +VERTEX2 1145 -75.690210 82.325987 -1.154935 +VERTEX2 1146 -75.227534 81.332724 -1.129768 +VERTEX2 1147 -74.801352 80.395299 -1.109178 +VERTEX2 1148 -74.464185 79.478807 -1.089582 +VERTEX2 1149 -74.001938 78.614361 -1.063598 +VERTEX2 1150 -73.555630 77.772436 -1.053786 +VERTEX2 1151 -72.941197 76.907931 -0.999808 +VERTEX2 1152 -72.389484 76.009165 -0.964804 +VERTEX2 1153 -71.709462 75.007622 -0.948763 +VERTEX2 1154 -71.079222 74.245132 -0.916780 +VERTEX2 1155 -70.487545 73.523253 -0.897133 +VERTEX2 1156 -69.814640 72.618879 -0.893509 +VERTEX2 1157 -69.203802 71.804787 -0.885769 +VERTEX2 1158 -68.431167 70.765385 -0.877034 +VERTEX2 1159 -67.896133 69.911283 -1.117065 +VERTEX2 1160 -67.344040 68.909563 -1.091951 +VERTEX2 1161 -66.881282 67.999411 -1.069604 +VERTEX2 1162 -66.439132 67.189632 -1.041393 +VERTEX2 1163 -65.904254 66.343898 -1.035375 +VERTEX2 1164 -65.223994 65.344030 -1.015966 +VERTEX2 1165 -64.729542 64.492958 -0.999959 +VERTEX2 1166 -64.184394 63.540264 -1.057690 +VERTEX2 1167 -63.735058 62.537297 -1.099630 +VERTEX2 1168 -63.282810 61.633177 -1.091796 +VERTEX2 1169 -62.935543 60.676823 -1.073878 +VERTEX2 1170 -62.400644 59.847854 -1.076156 +VERTEX2 1171 -61.796167 58.917557 -1.048551 +VERTEX2 1172 -61.362808 58.034078 -1.012541 +VERTEX2 1173 -60.923851 57.173545 -0.986639 +VERTEX2 1174 -60.406017 56.419023 -0.976803 +VERTEX2 1175 -59.890114 55.529542 -1.174151 +VERTEX2 1176 -59.538645 54.586310 -1.155309 +VERTEX2 1177 -59.187799 53.668701 -1.155074 +VERTEX2 1178 -58.707836 52.672059 -1.152603 +VERTEX2 1179 -58.232657 51.793855 -1.102238 +VERTEX2 1180 -57.768575 50.892410 -1.086823 +VERTEX2 1181 -57.230592 49.805656 -1.075582 +VERTEX2 1182 -56.714984 48.866386 -1.066700 +VERTEX2 1183 -56.141406 47.975694 -0.915022 +VERTEX2 1184 -55.433039 47.230133 -0.732731 +VERTEX2 1185 -54.804729 46.619403 -0.695779 +VERTEX2 1186 -54.092361 46.058136 -0.736036 +VERTEX2 1187 -53.295051 45.240644 -0.799436 +VERTEX2 1188 -52.555455 44.391104 -0.961885 +VERTEX2 1189 -51.895747 43.468636 -1.038905 +VERTEX2 1190 -51.428338 42.637667 -1.090263 +VERTEX2 1191 -50.817719 41.652381 -1.084503 +VERTEX2 1192 -50.322234 40.787544 -1.112403 +VERTEX2 1193 -49.827750 39.873380 -1.114946 +VERTEX2 1194 -49.387105 38.776935 -1.091368 +VERTEX2 1195 -48.860537 37.828237 -1.086499 +VERTEX2 1196 -48.354369 36.670738 -1.085412 +VERTEX2 1197 -47.823934 35.789631 -1.058629 +VERTEX2 1198 -47.317407 34.916251 -1.057711 +VERTEX2 1199 -46.738452 33.938531 -1.069306 +VERTEX2 1200 -46.323929 32.960159 -1.037538 +VERTEX2 1201 -45.848184 32.157731 -1.007036 +VERTEX2 1202 -45.265620 31.296420 -0.998428 +VERTEX2 1203 -44.738023 30.410923 -0.994090 +VERTEX2 1204 -44.202187 29.609112 -1.087880 +VERTEX2 1205 -43.825309 28.690556 -1.158778 +VERTEX2 1206 -43.529132 27.665231 -1.122035 +VERTEX2 1207 -43.049767 26.695482 -1.117584 +VERTEX2 1208 -42.697155 25.756562 -1.116587 +VERTEX2 1209 -42.263465 24.864075 -1.116785 +VERTEX2 1210 -41.736764 23.941684 -1.103969 +VERTEX2 1211 -41.385819 23.040570 -1.096252 +VERTEX2 1212 -40.942170 22.109163 -1.075091 +VERTEX2 1213 -40.380648 21.153188 -1.069990 +VERTEX2 1214 -39.865694 20.197175 -1.045685 +VERTEX2 1215 -39.400092 19.221694 -1.035953 +VERTEX2 1216 -38.930259 18.286918 -1.056762 +VERTEX2 1217 -38.577628 17.363136 -1.106438 +VERTEX2 1218 -38.055739 16.332881 -1.075786 +VERTEX2 1219 -37.592388 15.420447 -1.069700 +VERTEX2 1220 -37.143609 14.419074 -1.064591 +VERTEX2 1221 -36.668586 13.496328 -1.063171 +VERTEX2 1222 -36.223529 12.596629 -1.044696 +VERTEX2 1223 -35.745074 11.776528 -1.044185 +VERTEX2 1224 -35.239460 10.891114 -1.038954 +VERTEX2 1225 -34.813395 10.091433 -1.032470 +VERTEX2 1226 -34.339375 9.296534 -1.008972 +VERTEX2 1227 -33.767707 8.406526 -0.996268 +VERTEX2 1228 -33.283747 7.543401 -0.996150 +VERTEX2 1229 -32.680443 6.671050 -0.978514 +VERTEX2 1230 -32.112898 5.822569 -1.013635 +VERTEX2 1231 -31.721756 4.838754 -1.296047 +VERTEX2 1232 -31.621115 3.809074 -1.673717 +VERTEX2 1233 -31.929750 2.875186 -2.189049 +VERTEX2 1234 -32.692479 2.171450 -2.516951 +VERTEX2 1235 -33.626457 1.523158 -2.557060 +VERTEX2 1236 -34.548041 1.020852 -2.560727 +VERTEX2 1237 -35.529472 0.419930 -2.580511 +VERTEX2 1238 -36.421363 -0.148752 -2.609105 +VERTEX2 1239 -37.349579 -0.699631 -2.655636 +VERTEX2 1240 -38.280580 -1.112926 -2.663505 +VERTEX2 1241 -39.173834 -1.522014 -2.687389 +VERTEX2 1242 -40.097297 -1.876009 -2.741603 +VERTEX2 1243 -41.196841 -2.050123 -2.821765 +VERTEX2 1244 -41.688299 -2.321536 -2.287879 +VERTEX2 1245 -41.920476 -2.828360 -1.756028 +VERTEX2 1246 -42.010240 -3.351674 -1.208966 +VERTEX2 1247 -41.553281 -3.706106 -0.522952 +VERTEX2 1248 -41.662707 -3.797995 0.664663 +VERTEX2 1249 -40.821409 -3.226438 0.694578 +VERTEX2 1250 -39.908925 -2.572720 0.707531 +VERTEX2 1251 -39.124160 -1.965375 0.732246 +VERTEX2 1252 -38.326942 -1.275206 0.751629 +VERTEX2 1253 -37.527486 -0.670700 0.638617 +VERTEX2 1254 -36.677248 -0.045702 0.521950 +VERTEX2 1255 -35.800800 0.419110 0.427960 +VERTEX2 1256 -34.831425 0.773889 0.392515 +VERTEX2 1257 -33.872361 1.184893 0.614221 +VERTEX2 1258 -33.112423 1.836543 0.658918 +VERTEX2 1259 -32.363025 2.515438 0.662420 +VERTEX2 1260 -31.611034 3.100233 0.684526 +VERTEX2 1261 -30.855429 3.743664 0.681272 +VERTEX2 1262 -30.045831 4.172545 0.255995 +VERTEX2 1263 -29.098996 4.132149 -0.115963 +VERTEX2 1264 -27.929680 4.180320 0.356468 +VERTEX2 1265 -26.958897 4.670306 0.562214 +VERTEX2 1266 -25.996742 5.334610 0.573222 +VERTEX2 1267 -25.316329 5.836888 1.143346 +VERTEX2 1268 -25.169815 6.051737 1.680257 +VERTEX2 1269 -25.318314 6.295087 2.311728 +VERTEX2 1270 -25.611655 6.419800 2.988764 +VERTEX2 1271 -25.858258 6.382728 -2.635779 +VERTEX2 1272 -26.167113 6.181433 -2.079772 +VERTEX2 1273 -26.619932 5.562665 -2.576769 +VERTEX2 1274 -27.547800 4.992724 -2.638749 +VERTEX2 1275 -28.501829 4.569638 -2.668784 +VERTEX2 1276 -29.371075 4.183982 -2.699515 +VERTEX2 1277 -30.408544 3.685885 -2.736803 +VERTEX2 1278 -31.410899 3.257421 -2.771572 +VERTEX2 1279 -32.379072 2.907652 -2.797424 +VERTEX2 1280 -33.401708 2.686572 -2.777505 +VERTEX2 1281 -34.322862 2.316881 -2.727944 +VERTEX2 1282 -35.363799 2.009636 -2.752994 +VERTEX2 1283 -36.233045 1.696091 -2.756273 +VERTEX2 1284 -37.243733 1.185493 -2.571136 +VERTEX2 1285 -38.058777 0.564665 -2.503117 +VERTEX2 1286 -39.014421 -0.129981 -2.524062 +VERTEX2 1287 -39.772522 -0.544198 -2.602443 +VERTEX2 1288 -40.621856 -1.129070 -2.431230 +VERTEX2 1289 -41.635977 -1.756835 -2.466491 +VERTEX2 1290 -42.518640 -2.322133 -2.606100 +VERTEX2 1291 -43.430323 -2.818793 -2.653941 +VERTEX2 1292 -44.401588 -3.272261 -2.677972 +VERTEX2 1293 -45.372025 -3.675733 -2.693620 +VERTEX2 1294 -46.494836 -4.035218 -2.702834 +VERTEX2 1295 -47.537776 -4.343470 -2.713916 +VERTEX2 1296 -48.610529 -4.746151 -2.729727 +VERTEX2 1297 -49.613031 -5.137489 -2.755726 +VERTEX2 1298 -50.551696 -5.557308 -2.696260 +VERTEX2 1299 -51.419682 -5.990968 -2.620345 +VERTEX2 1300 -52.429442 -6.473561 -2.635066 +VERTEX2 1301 -53.275715 -7.019392 -2.637211 +VERTEX2 1302 -54.311388 -7.554112 -2.670133 +VERTEX2 1303 -55.407773 -8.068503 -2.591832 +VERTEX2 1304 -56.136204 -8.567367 -2.611926 +VERTEX2 1305 -57.035769 -9.150153 -2.633065 +VERTEX2 1306 -57.963805 -9.618961 -2.634361 +VERTEX2 1307 -59.015157 -10.120009 -2.622865 +VERTEX2 1308 -60.024353 -10.585172 -2.639014 +VERTEX2 1309 -60.971093 -11.100348 -2.677845 +VERTEX2 1310 -61.870077 -11.527868 -2.681376 +VERTEX2 1311 -62.811475 -11.894565 -2.690354 +VERTEX2 1312 -63.728002 -12.284698 -2.704568 +VERTEX2 1313 -64.666753 -12.646429 -2.734554 +VERTEX2 1314 -65.658349 -13.060851 -2.739075 +VERTEX2 1315 -66.602747 -13.428881 -2.746258 +VERTEX2 1316 -67.482748 -13.699328 -2.762586 +VERTEX2 1317 -68.425288 -14.038723 -2.767628 +VERTEX2 1318 -69.497506 -14.292225 -2.771424 +VERTEX2 1319 -70.403568 -14.510027 -2.788802 +VERTEX2 1320 -71.240065 -14.961084 -2.695422 +VERTEX2 1321 -72.153615 -15.357080 -2.682367 +VERTEX2 1322 -73.236697 -15.825817 -2.700876 +VERTEX2 1323 -74.077333 -16.221997 -2.740418 +VERTEX2 1324 -75.031762 -16.540815 -2.751240 +VERTEX2 1325 -76.142140 -16.958585 -2.753734 +VERTEX2 1326 -77.171249 -17.362045 -2.757262 +VERTEX2 1327 -77.945354 -17.658796 -2.772885 +VERTEX2 1328 -78.911894 -18.251858 -2.645004 +VERTEX2 1329 -79.641019 -18.781217 -2.571973 +VERTEX2 1330 -80.811444 -19.312691 -2.787855 +VERTEX2 1331 -81.926290 -19.703201 -2.797196 +VERTEX2 1332 -82.950770 -20.067464 -2.798263 +VERTEX2 1333 -83.982155 -20.355093 -2.804463 +VERTEX2 1334 -84.959965 -20.696906 -2.820245 +VERTEX2 1335 -86.004621 -21.057972 -2.832725 +VERTEX2 1336 -87.065192 -21.398542 -2.831226 +VERTEX2 1337 -88.005368 -21.670261 -2.846203 +VERTEX2 1338 -88.961451 -22.139718 -2.244199 +VERTEX2 1339 -89.188156 -22.488134 -1.707720 +VERTEX2 1340 -89.265680 -23.090763 -1.199507 +VERTEX2 1341 -88.890349 -24.001640 -1.176712 +VERTEX2 1342 -88.408124 -25.040257 -1.145467 +VERTEX2 1343 -87.936187 -26.032029 -1.138309 +VERTEX2 1344 -87.705891 -27.004042 -1.595903 +VERTEX2 1345 -88.061582 -28.019277 -2.008831 +VERTEX2 1346 -88.582720 -28.798396 -2.394467 +VERTEX2 1347 -89.384635 -29.215154 -2.645371 +VERTEX2 1348 -90.336934 -29.658079 -2.668527 +VERTEX2 1349 -91.047775 -30.180475 -1.962527 +VERTEX2 1350 -91.178201 -30.509641 -1.345667 +VERTEX2 1351 -90.942596 -31.589572 -1.339649 +VERTEX2 1352 -90.700651 -32.617789 -1.130279 +VERTEX2 1353 -90.392418 -33.447632 -1.099869 +VERTEX2 1354 -89.911606 -34.489042 -1.125453 +VERTEX2 1355 -89.409630 -35.459936 -1.100250 +VERTEX2 1356 -88.923901 -36.416667 -1.088002 +VERTEX2 1357 -88.487113 -37.229101 -1.158032 +VERTEX2 1358 -88.346072 -37.606591 -1.773282 +VERTEX2 1359 -88.445003 -37.771905 -2.303096 +VERTEX2 1360 -88.684161 -37.942437 -2.837301 +VERTEX2 1361 -88.883743 -37.980466 2.910520 +VERTEX2 1362 -89.147157 -37.765221 2.354019 +VERTEX2 1363 -89.321558 -37.504678 1.804788 +VERTEX2 1364 -89.337731 -36.534839 1.604230 +VERTEX2 1365 -89.495963 -35.630521 2.111619 +VERTEX2 1366 -89.928324 -34.679715 2.061857 +VERTEX2 1367 -90.404189 -33.734498 2.025270 +VERTEX2 1368 -90.733558 -32.781082 2.023382 +VERTEX2 1369 -91.069698 -31.801918 1.971802 +VERTEX2 1370 -91.311152 -30.846037 1.807676 +VERTEX2 1371 -91.283429 -30.391956 1.270500 +VERTEX2 1372 -91.067636 -29.931837 0.698402 +VERTEX2 1373 -90.096221 -29.272418 0.621001 +VERTEX2 1374 -89.270783 -28.798406 0.615398 +VERTEX2 1375 -88.453486 -28.196771 0.753647 +VERTEX2 1376 -88.149136 -27.793116 1.342140 +VERTEX2 1377 -88.150473 -26.800954 1.664623 +VERTEX2 1378 -88.265202 -25.820926 1.753941 +VERTEX2 1379 -88.552334 -24.833081 1.858409 +VERTEX2 1380 -88.809765 -23.898715 2.327049 +VERTEX2 1381 -89.086098 -23.687760 2.852903 +VERTEX2 1382 -90.045200 -23.526148 3.104518 +VERTEX2 1383 -91.174396 -23.644967 -2.810512 +VERTEX2 1384 -92.278208 -23.986424 -2.828961 +VERTEX2 1385 -93.211426 -24.182488 -2.842373 +VERTEX2 1386 -94.219398 -24.437207 -2.834120 +VERTEX2 1387 -95.215179 -24.744394 -2.820784 +VERTEX2 1388 -96.213775 -25.199553 -2.845697 +VERTEX2 1389 -97.198671 -25.444201 -2.752709 +VERTEX2 1390 -98.120815 -25.863870 -2.699649 +VERTEX2 1391 -99.060549 -26.168420 -2.640237 +VERTEX2 1392 -99.964357 -26.748594 -2.637686 +VERTEX2 1393 -100.855459 -27.210903 -2.637695 +VERTEX2 1394 -101.813415 -27.657341 -2.632523 +VERTEX2 1395 -102.698497 -28.052268 -2.619884 +VERTEX2 1396 -103.609779 -28.473368 -2.621384 +VERTEX2 1397 -104.560687 -28.934702 -2.618760 +VERTEX2 1398 -105.461593 -29.473037 -2.626182 +VERTEX2 1399 -106.422946 -29.941309 -2.612419 +VERTEX2 1400 -107.325482 -30.430446 -2.636123 +VERTEX2 1401 -108.192924 -30.967906 -2.651271 +VERTEX2 1402 -109.141463 -31.372170 -2.634954 +VERTEX2 1403 -110.082624 -31.858902 -2.579498 +VERTEX2 1404 -110.955513 -32.449452 -2.500915 +VERTEX2 1405 -112.029713 -33.081303 -2.657421 +VERTEX2 1406 -113.013898 -33.489230 -2.799675 +VERTEX2 1407 -114.006361 -33.747426 -2.828509 +VERTEX2 1408 -114.991637 -33.996474 -2.821547 +VERTEX2 1409 -116.176723 -34.374557 -2.755396 +VERTEX2 1410 -117.068784 -34.752931 -2.692511 +VERTEX2 1411 -118.087361 -35.207427 -2.688548 +VERTEX2 1412 -119.043234 -35.639033 -2.684200 +VERTEX2 1413 -119.965256 -36.031571 -2.669519 +VERTEX2 1414 -120.785487 -36.482693 -2.537070 +VERTEX2 1415 -121.685073 -37.173913 -2.454421 +VERTEX2 1416 -122.379844 -37.777584 -2.448951 +VERTEX2 1417 -123.226316 -38.511434 -2.448029 +VERTEX2 1418 -124.012201 -39.086838 -2.460547 +VERTEX2 1419 -124.751973 -39.688194 -2.501879 +VERTEX2 1420 -125.662584 -40.284005 -2.501366 +VERTEX2 1421 -126.733494 -41.019127 -2.498046 +VERTEX2 1422 -127.567466 -41.555608 -2.663400 +VERTEX2 1423 -128.491973 -42.024945 -2.645278 +VERTEX2 1424 -129.436083 -42.499142 -2.654654 +VERTEX2 1425 -130.463525 -43.085418 -2.668494 +VERTEX2 1426 -131.396844 -43.536033 -2.679499 +VERTEX2 1427 -132.360441 -43.935312 -2.691796 +VERTEX2 1428 -133.304235 -44.446637 -2.712669 +VERTEX2 1429 -134.329493 -44.821418 -2.704023 +VERTEX2 1430 -135.225760 -45.199058 -2.708934 +VERTEX2 1431 -136.165972 -45.612894 -2.723339 +VERTEX2 1432 -137.187095 -46.024483 -2.724730 +VERTEX2 1433 -138.047580 -46.413493 -2.723808 +VERTEX2 1434 -139.012460 -46.757779 -2.723791 +VERTEX2 1435 -140.016117 -47.166308 -2.730810 +VERTEX2 1436 -140.958662 -47.540619 -2.730371 +VERTEX2 1437 -142.030263 -47.987830 -2.697112 +VERTEX2 1438 -143.069741 -48.336253 -2.701413 +VERTEX2 1439 -144.034341 -48.841205 -2.588789 +VERTEX2 1440 -144.823309 -49.348887 -2.489560 +VERTEX2 1441 -145.737312 -49.887186 -2.781538 +VERTEX2 1442 -146.734328 -49.998284 2.874825 +VERTEX2 1443 -147.364179 -49.607997 2.394865 +VERTEX2 1444 -147.607896 -49.146156 1.792182 +VERTEX2 1445 -147.527350 -48.672372 1.196678 +VERTEX2 1446 -147.176753 -48.102322 1.768889 +VERTEX2 1447 -147.421300 -47.134051 1.745814 +VERTEX2 1448 -147.627357 -46.163451 1.971394 +VERTEX2 1449 -148.015130 -45.096146 1.933795 +VERTEX2 1450 -148.320675 -44.077436 1.905157 +VERTEX2 1451 -148.741853 -43.109464 2.173190 +VERTEX2 1452 -149.365549 -42.226121 2.239989 +VERTEX2 1453 -150.039301 -41.417270 2.208659 +VERTEX2 1454 -150.563282 -40.447836 2.083939 +VERTEX2 1455 -151.115217 -39.484294 2.049579 +VERTEX2 1456 -151.646534 -38.475362 2.033910 +VERTEX2 1457 -152.078621 -37.473784 2.016509 +VERTEX2 1458 -152.424834 -36.645424 1.990631 +VERTEX2 1459 -152.764428 -35.652296 1.976748 +VERTEX2 1460 -153.040464 -34.723010 1.951969 +VERTEX2 1461 -153.429095 -33.768700 1.946001 +VERTEX2 1462 -153.755848 -32.785578 1.921978 +VERTEX2 1463 -154.043796 -31.897661 1.917029 +VERTEX2 1464 -154.367792 -30.766924 1.986796 +VERTEX2 1465 -154.768809 -29.886424 2.010309 +VERTEX2 1466 -155.262068 -28.922092 1.988659 +VERTEX2 1467 -155.653724 -27.980603 1.963586 +VERTEX2 1468 -155.908429 -27.059875 1.937479 +VERTEX2 1469 -156.216505 -26.021442 1.920116 +VERTEX2 1470 -156.687741 -25.024792 1.982334 +VERTEX2 1471 -157.110880 -24.154991 2.079998 +VERTEX2 1472 -157.589739 -23.206399 2.063127 +VERTEX2 1473 -158.034050 -22.208583 2.044516 +VERTEX2 1474 -158.576443 -21.047695 2.015003 +VERTEX2 1475 -158.989624 -20.045475 1.993858 +VERTEX2 1476 -159.333712 -19.159840 1.971887 +VERTEX2 1477 -159.707364 -18.151032 1.949104 +VERTEX2 1478 -160.072321 -17.262737 1.928402 +VERTEX2 1479 -160.360170 -16.215204 1.912617 +VERTEX2 1480 -160.743152 -15.051215 1.908465 +VERTEX2 1481 -161.016281 -13.975144 1.900466 +VERTEX2 1482 -161.257540 -12.903094 1.902493 +VERTEX2 1483 -161.551252 -11.834037 1.962505 +VERTEX2 1484 -162.054775 -10.845348 2.093503 +VERTEX2 1485 -162.605420 -9.899221 2.112883 +VERTEX2 1486 -163.143375 -9.049752 2.106556 +VERTEX2 1487 -163.675283 -8.200518 2.093950 +VERTEX2 1488 -164.168314 -7.313009 2.087401 +VERTEX2 1489 -164.688556 -6.363052 2.057203 +VERTEX2 1490 -165.093423 -5.480854 2.039797 +VERTEX2 1491 -165.608380 -4.374167 2.040851 +VERTEX2 1492 -166.027768 -3.553389 2.038239 +VERTEX2 1493 -166.441096 -2.681316 2.020403 +VERTEX2 1494 -166.895611 -1.628453 1.996127 +VERTEX2 1495 -167.254710 -0.730416 2.006564 +VERTEX2 1496 -167.675527 0.269152 1.967948 +VERTEX2 1497 -168.034406 1.146275 1.956351 +VERTEX2 1498 -168.386292 2.189360 1.966787 +VERTEX2 1499 -168.738655 3.114637 1.947773 +VERTEX2 1500 -169.184432 4.139788 1.954365 +VERTEX2 1501 -169.559576 5.050152 2.030914 +VERTEX2 1502 -170.000987 6.005079 2.024321 +VERTEX2 1503 -170.387530 6.883169 1.986497 +VERTEX2 1504 -170.732087 7.827824 1.973469 +VERTEX2 1505 -171.102538 8.841845 1.974294 +VERTEX2 1506 -171.532975 9.706042 1.948418 +VERTEX2 1507 -171.790308 10.633357 1.925778 +VERTEX2 1508 -171.985845 11.635615 1.929668 +VERTEX2 1509 -172.386469 12.616250 1.949900 +VERTEX2 1510 -172.818822 13.581686 2.031811 +VERTEX2 1511 -173.245091 14.508196 2.035376 +VERTEX2 1512 -173.746693 15.421402 2.025845 +VERTEX2 1513 -174.146767 16.477099 2.010200 +VERTEX2 1514 -174.531317 17.384068 2.018023 +VERTEX2 1515 -174.964212 18.287525 1.999181 +VERTEX2 1516 -175.347566 19.290668 1.993053 +VERTEX2 1517 -175.799866 20.371487 1.994443 +VERTEX2 1518 -176.143637 21.265590 1.998073 +VERTEX2 1519 -176.648785 22.273476 1.980958 +VERTEX2 1520 -177.154284 23.207124 2.059245 +VERTEX2 1521 -177.649681 24.190887 2.121972 +VERTEX2 1522 -178.171864 25.070403 2.120353 +VERTEX2 1523 -178.714759 26.088509 2.125035 +VERTEX2 1524 -179.264092 26.952251 2.130779 +VERTEX2 1525 -179.757828 27.807902 1.872337 +VERTEX2 1526 -180.061139 28.750499 1.821373 +VERTEX2 1527 -180.280045 29.751581 1.811073 +VERTEX2 1528 -180.477525 30.786515 1.791535 +VERTEX2 1529 -180.782882 31.946044 1.906238 +VERTEX2 1530 -181.170774 32.960440 1.948246 +VERTEX2 1531 -181.465934 34.062144 1.959554 +VERTEX2 1532 -181.810334 34.940685 2.002175 +VERTEX2 1533 -182.299127 35.832546 2.038136 +VERTEX2 1534 -182.741547 36.723255 2.035538 +VERTEX2 1535 -183.211557 37.730175 2.033224 +VERTEX2 1536 -183.662233 38.711836 2.045454 +VERTEX2 1537 -184.069286 39.656279 2.061626 +VERTEX2 1538 -184.596765 40.537164 2.064803 +VERTEX2 1539 -185.117918 41.366482 2.600861 +VERTEX2 1540 -186.037217 41.740477 2.833729 +VERTEX2 1541 -186.978760 42.104828 2.819936 +VERTEX2 1542 -187.972408 42.395943 2.984538 +VERTEX2 1543 -188.988807 42.670724 2.892233 +VERTEX2 1544 -190.016832 42.922297 2.904861 +VERTEX2 1545 -191.060647 43.164008 2.941805 +VERTEX2 1546 -192.219588 43.384430 3.021109 +VERTEX2 1547 -193.325004 43.526302 3.091393 +VERTEX2 1548 -194.446839 43.285802 -2.796652 +VERTEX2 1549 -195.410117 42.841170 -2.582323 +VERTEX2 1550 -196.291539 42.367371 -2.608167 +VERTEX2 1551 -197.179009 41.794858 -2.601137 +VERTEX2 1552 -198.046112 41.303747 -2.571718 +VERTEX2 1553 -199.004854 40.837204 -2.557576 +VERTEX2 1554 -199.884447 40.261907 -2.528920 +VERTEX2 1555 -200.857343 39.637394 -2.498619 +VERTEX2 1556 -201.716662 38.977298 -2.459205 +VERTEX2 1557 -202.441997 38.328036 -2.425886 +VERTEX2 1558 -203.268190 37.613601 -2.433059 +VERTEX2 1559 -204.171516 36.957705 -2.493720 +VERTEX2 1560 -205.018113 36.327911 -2.455392 +VERTEX2 1561 -205.760421 35.784367 -2.440534 +VERTEX2 1562 -206.621786 35.085788 -2.408374 +VERTEX2 1563 -207.441764 34.388996 -2.447479 +VERTEX2 1564 -208.331720 33.778268 -2.487791 +VERTEX2 1565 -209.248173 33.264482 -2.708431 +VERTEX2 1566 -210.157119 32.776643 -2.581244 +VERTEX2 1567 -211.031109 32.223084 -2.581781 +VERTEX2 1568 -212.013548 31.714941 -2.563620 +VERTEX2 1569 -212.908759 31.119643 -2.524258 +VERTEX2 1570 -213.914600 30.523317 -2.496916 +VERTEX2 1571 -214.766643 29.874874 -2.419025 +VERTEX2 1572 -215.499071 29.104063 -2.152530 +VERTEX2 1573 -216.297520 28.343605 -2.424362 +VERTEX2 1574 -216.873857 27.751072 -2.411120 +VERTEX2 1575 -217.573171 27.039984 -2.376939 +VERTEX2 1576 -218.403545 26.341822 -2.351156 +VERTEX2 1577 -219.188306 25.636039 -2.556863 +VERTEX2 1578 -220.286059 24.865227 -2.552414 +VERTEX2 1579 -221.106195 24.347185 -2.531418 +VERTEX2 1580 -221.928494 23.777674 -2.539558 +VERTEX2 1581 -222.823515 23.154783 -2.540114 +VERTEX2 1582 -223.700638 22.529179 -2.532956 +VERTEX2 1583 -224.575956 21.854791 -2.517652 +VERTEX2 1584 -225.520992 21.237662 -2.508811 +VERTEX2 1585 -226.370866 20.660109 -2.494104 +VERTEX2 1586 -227.196857 19.969027 -2.456473 +VERTEX2 1587 -227.994946 19.414834 -2.417807 +VERTEX2 1588 -228.706026 18.637177 -2.411368 +VERTEX2 1589 -228.982699 18.244250 -1.871203 +VERTEX2 1590 -229.117657 17.359358 -1.285778 +VERTEX2 1591 -228.750827 16.558837 -0.901554 +VERTEX2 1592 -228.069999 15.628839 -0.887462 +VERTEX2 1593 -227.502415 14.935754 -0.878841 +VERTEX2 1594 -226.787295 14.113446 -0.753366 +VERTEX2 1595 -225.826748 13.553123 -0.513312 +VERTEX2 1596 -225.165449 12.891396 -0.835948 +VERTEX2 1597 -224.450472 12.191032 -0.808254 +VERTEX2 1598 -223.781314 11.433298 -0.788431 +VERTEX2 1599 -223.100248 10.714634 -0.768070 +VERTEX2 1600 -222.468423 9.868712 -1.061116 +VERTEX2 1601 -222.156878 8.912765 -1.376737 +VERTEX2 1602 -221.795632 7.889185 -0.908208 +VERTEX2 1603 -221.233386 7.058803 -0.855114 +VERTEX2 1604 -220.620393 6.144171 -0.844054 +VERTEX2 1605 -219.934448 5.227842 -0.821542 +VERTEX2 1606 -219.201194 4.413948 -0.810069 +VERTEX2 1607 -218.455658 3.692951 -0.794676 +VERTEX2 1608 -217.769646 2.953421 -0.767087 +VERTEX2 1609 -217.052000 2.241155 -0.792793 +VERTEX2 1610 -216.360446 1.504374 -0.791105 +VERTEX2 1611 -215.679375 0.816011 -0.778235 +VERTEX2 1612 -215.041621 0.139292 -0.777312 +VERTEX2 1613 -214.334082 -0.607898 -0.884444 +VERTEX2 1614 -213.764777 -1.436648 -0.922180 +VERTEX2 1615 -213.168008 -2.214514 -0.905922 +VERTEX2 1616 -212.457511 -3.040546 -0.907175 +VERTEX2 1617 -211.763255 -3.759924 -0.889689 +VERTEX2 1618 -211.094541 -4.572079 -0.858101 +VERTEX2 1619 -210.493060 -5.462197 -1.067104 +VERTEX2 1620 -210.014135 -6.311282 -1.034153 +VERTEX2 1621 -209.459667 -7.251210 -1.030028 +VERTEX2 1622 -208.908563 -8.168918 -0.983974 +VERTEX2 1623 -208.295240 -8.913055 -0.938477 +VERTEX2 1624 -207.727075 -9.739207 -0.910701 +VERTEX2 1625 -207.278832 -10.561615 -1.025508 +VERTEX2 1626 -206.861710 -11.444274 -1.047012 +VERTEX2 1627 -206.344056 -12.250793 -1.019438 +VERTEX2 1628 -205.728400 -13.048300 -0.752371 +VERTEX2 1629 -204.991823 -13.806496 -0.709538 +VERTEX2 1630 -204.312556 -14.492707 -0.673369 +VERTEX2 1631 -203.526584 -15.188507 -0.673799 +VERTEX2 1632 -202.758813 -15.967991 -1.019104 +VERTEX2 1633 -202.254729 -16.987646 -1.171543 +VERTEX2 1634 -201.837834 -17.885745 -0.857293 +VERTEX2 1635 -201.253022 -18.669993 -0.859701 +VERTEX2 1636 -200.821061 -19.642639 -1.254279 +VERTEX2 1637 -200.551692 -20.073240 -0.711181 +VERTEX2 1638 -199.805458 -20.645595 -0.415695 +VERTEX2 1639 -198.820304 -20.972325 -0.219184 +VERTEX2 1640 -197.969575 -21.043830 0.264808 +VERTEX2 1641 -196.967738 -20.800979 0.078120 +VERTEX2 1642 -195.985117 -21.070231 -0.460489 +VERTEX2 1643 -195.571688 -21.415847 -1.205021 +VERTEX2 1644 -195.518888 -21.607903 -1.719720 +VERTEX2 1645 -195.544582 -21.754807 -2.222485 +VERTEX2 1646 -195.788145 -21.936746 -1.691106 +VERTEX2 1647 -195.967819 -22.878991 -1.682937 +VERTEX2 1648 -196.127190 -23.938106 -1.682640 +VERTEX2 1649 -196.135781 -24.882134 -1.075201 +VERTEX2 1650 -195.591457 -25.677244 -0.967323 +VERTEX2 1651 -194.927014 -26.543039 -0.891822 +VERTEX2 1652 -194.274960 -27.466334 -0.863229 +VERTEX2 1653 -193.760101 -28.247152 -0.825404 +VERTEX2 1654 -193.094341 -28.953449 -0.807225 +VERTEX2 1655 -192.361501 -29.714976 -0.789669 +VERTEX2 1656 -191.665540 -30.400126 -0.752811 +VERTEX2 1657 -190.921913 -31.080602 -0.798740 +VERTEX2 1658 -190.280866 -31.843228 -0.897094 +VERTEX2 1659 -189.618970 -32.655571 -0.884422 +VERTEX2 1660 -188.965926 -33.366654 -0.860430 +VERTEX2 1661 -188.314719 -34.276550 -0.849113 +VERTEX2 1662 -187.687151 -35.027920 -0.888494 +VERTEX2 1663 -187.144654 -35.877179 -0.974088 +VERTEX2 1664 -186.721354 -36.769258 -0.993334 +VERTEX2 1665 -186.151812 -37.518098 -0.751325 +VERTEX2 1666 -185.452056 -38.328315 -1.003839 +VERTEX2 1667 -184.981675 -39.192102 -1.063459 +VERTEX2 1668 -184.399964 -40.077438 -0.906764 +VERTEX2 1669 -183.826020 -40.864582 -0.892909 +VERTEX2 1670 -183.224442 -41.691397 -0.859589 +VERTEX2 1671 -182.566273 -42.506227 -0.861934 +VERTEX2 1672 -181.844934 -43.254129 -0.868024 +VERTEX2 1673 -181.189734 -44.053537 -0.867237 +VERTEX2 1674 -180.411358 -44.914399 -0.691885 +VERTEX2 1675 -179.703963 -45.581785 -0.686639 +VERTEX2 1676 -178.868584 -46.245732 -0.685721 +VERTEX2 1677 -178.196764 -46.926694 -0.667153 +VERTEX2 1678 -177.434037 -47.591438 -0.658866 +VERTEX2 1679 -176.718487 -48.223625 -0.680012 +VERTEX2 1680 -175.970495 -48.972250 -0.811910 +VERTEX2 1681 -175.198548 -49.755918 -0.794240 +VERTEX2 1682 -174.506493 -50.566989 -0.777812 +VERTEX2 1683 -173.822500 -51.353007 -0.991522 +VERTEX2 1684 -173.266286 -52.271169 -0.981034 +VERTEX2 1685 -172.687239 -53.174484 -0.989512 +VERTEX2 1686 -172.185093 -54.074407 -0.998281 +VERTEX2 1687 -171.636880 -54.915480 -0.990107 +VERTEX2 1688 -171.120204 -55.800494 -0.964808 +VERTEX2 1689 -170.443902 -56.457780 -0.720098 +VERTEX2 1690 -169.688346 -57.227248 -0.940566 +VERTEX2 1691 -169.096427 -58.086124 -1.052267 +VERTEX2 1692 -168.560231 -59.051969 -0.898233 +VERTEX2 1693 -167.812897 -59.883786 -0.628137 +VERTEX2 1694 -167.058680 -60.467997 -0.834518 +VERTEX2 1695 -166.386352 -61.221034 -0.855014 +VERTEX2 1696 -165.758527 -62.112688 -1.039902 +VERTEX2 1697 -165.299707 -63.056107 -0.880379 +VERTEX2 1698 -164.481427 -63.644971 -0.529978 +VERTEX2 1699 -164.104302 -63.952417 -0.006361 +VERTEX2 1700 -163.872543 -63.859730 0.531976 +VERTEX2 1701 -162.967738 -63.152446 0.790002 +VERTEX2 1702 -162.178543 -62.414828 0.809663 +VERTEX2 1703 -161.400500 -61.637033 0.821176 +VERTEX2 1704 -160.735576 -60.947982 0.828751 +VERTEX2 1705 -159.900483 -60.102521 0.790795 +VERTEX2 1706 -159.144089 -59.372876 0.741892 +VERTEX2 1707 -158.358538 -58.756014 0.761543 +VERTEX2 1708 -157.606319 -58.062394 0.788155 +VERTEX2 1709 -156.837540 -57.374571 0.812296 +VERTEX2 1710 -156.132061 -56.633266 0.830617 +VERTEX2 1711 -155.339591 -56.057686 0.692837 +VERTEX2 1712 -154.522546 -55.425207 0.714168 +VERTEX2 1713 -153.669492 -54.749930 0.627568 +VERTEX2 1714 -152.823830 -54.202889 0.529408 +VERTEX2 1715 -151.899049 -53.621593 0.548310 +VERTEX2 1716 -151.036415 -53.085849 0.574550 +VERTEX2 1717 -150.003507 -52.557957 0.593201 +VERTEX2 1718 -149.105044 -51.905153 0.610092 +VERTEX2 1719 -148.205217 -51.164958 0.638865 +VERTEX2 1720 -147.315100 -50.551922 0.662601 +VERTEX2 1721 -146.478343 -49.980621 0.680843 +VERTEX2 1722 -145.742455 -49.429482 0.697985 +VERTEX2 1723 -144.992869 -48.733593 0.715565 +VERTEX2 1724 -144.262280 -48.091709 0.713245 +VERTEX2 1725 -143.402169 -47.454293 0.734753 +VERTEX2 1726 -142.618099 -46.775681 0.753563 +VERTEX2 1727 -141.705844 -46.009704 0.713100 +VERTEX2 1728 -140.876990 -45.371595 0.657182 +VERTEX2 1729 -140.087428 -44.772184 0.666413 +VERTEX2 1730 -139.284411 -44.149787 0.689484 +VERTEX2 1731 -138.516326 -43.487944 0.727704 +VERTEX2 1732 -137.690844 -42.892168 0.595246 +VERTEX2 1733 -136.911107 -42.385443 0.627418 +VERTEX2 1734 -136.010532 -41.784133 0.652348 +VERTEX2 1735 -135.222974 -41.136543 0.651384 +VERTEX2 1736 -134.343886 -40.547954 0.655178 +VERTEX2 1737 -133.589462 -39.975652 0.678118 +VERTEX2 1738 -132.623734 -39.346837 0.487085 +VERTEX2 1739 -131.717902 -38.825173 0.505033 +VERTEX2 1740 -130.881233 -38.385232 0.528892 +VERTEX2 1741 -130.044794 -37.871037 0.557130 +VERTEX2 1742 -129.192544 -37.310923 0.585298 +VERTEX2 1743 -128.378410 -36.722074 0.606776 +VERTEX2 1744 -127.485471 -36.225896 0.626974 +VERTEX2 1745 -126.831737 -35.515079 1.029949 +VERTEX2 1746 -126.292606 -34.661445 1.118863 +VERTEX2 1747 -125.974987 -33.752429 1.371061 +VERTEX2 1748 -125.955343 -32.964411 1.936092 +VERTEX2 1749 -126.326604 -32.028211 1.993611 +VERTEX2 1750 -126.701864 -31.653795 2.533010 +VERTEX2 1751 -127.132822 -31.536055 3.054605 +VERTEX2 1752 -127.550143 -31.523685 -2.659845 +VERTEX2 1753 -127.674479 -31.706942 -2.049790 +VERTEX2 1754 -127.727626 -32.026432 -1.379035 +VERTEX2 1755 -127.547720 -32.204261 -0.848738 +VERTEX2 1756 -127.126351 -32.400225 -0.277420 +VERTEX2 1757 -126.412509 -32.319535 -0.069399 +VERTEX2 1758 -126.053367 -32.445123 -0.572528 +VERTEX2 1759 -125.329663 -33.103730 -0.829788 +VERTEX2 1760 -124.819532 -33.390281 -0.273176 +VERTEX2 1761 -124.554164 -33.551316 0.271736 +VERTEX2 1762 -123.473762 -32.730274 0.619121 +VERTEX2 1763 -122.482156 -32.002787 0.665600 +VERTEX2 1764 -121.611948 -31.330420 0.799908 +VERTEX2 1765 -120.820398 -30.543992 0.882082 +VERTEX2 1766 -120.095877 -29.777517 0.896535 +VERTEX2 1767 -119.431346 -29.037847 0.900166 +VERTEX2 1768 -118.755559 -28.152330 0.941819 +VERTEX2 1769 -118.137112 -27.331273 0.971238 +VERTEX2 1770 -117.454700 -26.608604 0.675735 +VERTEX2 1771 -116.528963 -25.936468 0.653529 +VERTEX2 1772 -115.627295 -25.371060 0.679866 +VERTEX2 1773 -114.803756 -24.674560 0.687138 +VERTEX2 1774 -113.921836 -23.942111 0.638798 +VERTEX2 1775 -112.881404 -23.298372 0.598004 +VERTEX2 1776 -112.084167 -22.721959 0.581273 +VERTEX2 1777 -111.116102 -22.136000 0.589552 +VERTEX2 1778 -110.127665 -21.430903 0.614169 +VERTEX2 1779 -109.210227 -20.846396 0.626629 +VERTEX2 1780 -108.386336 -20.304859 0.658449 +VERTEX2 1781 -107.513738 -19.550173 0.676643 +VERTEX2 1782 -106.701784 -18.862802 0.710378 +VERTEX2 1783 -106.032436 -18.264862 0.718302 +VERTEX2 1784 -105.136377 -17.473474 0.749233 +VERTEX2 1785 -104.261811 -16.670623 0.774250 +VERTEX2 1786 -103.577159 -15.930040 0.792665 +VERTEX2 1787 -102.597503 -15.005411 0.753685 +VERTEX2 1788 -101.821958 -14.351032 0.740379 +VERTEX2 1789 -100.944209 -13.762318 0.596842 +VERTEX2 1790 -99.933686 -13.106516 0.619615 +VERTEX2 1791 -99.076435 -12.578213 0.644047 +VERTEX2 1792 -98.258393 -11.975754 0.677128 +VERTEX2 1793 -97.321018 -11.309771 0.672429 +VERTEX2 1794 -96.447165 -10.699001 0.671530 +VERTEX2 1795 -95.549748 -10.090321 0.696015 +VERTEX2 1796 -94.730870 -9.349958 0.696753 +VERTEX2 1797 -93.964293 -8.714498 0.713327 +VERTEX2 1798 -93.115209 -8.124579 0.717135 +VERTEX2 1799 -92.376548 -7.377688 0.740308 +VERTEX2 1800 -91.629073 -6.609999 0.762760 +VERTEX2 1801 -90.809716 -5.793871 0.747548 +VERTEX2 1802 -89.959670 -5.157872 0.741079 +VERTEX2 1803 -89.111975 -4.461479 0.739662 +VERTEX2 1804 -88.197423 -3.722356 0.761718 +VERTEX2 1805 -87.455867 -2.985281 0.690520 +VERTEX2 1806 -86.590880 -2.340556 0.682226 +VERTEX2 1807 -85.813375 -1.687480 0.710056 +VERTEX2 1808 -84.990393 -0.980697 0.743708 +VERTEX2 1809 -84.364370 -0.301795 0.749948 +VERTEX2 1810 -83.563976 0.403948 0.768176 +VERTEX2 1811 -82.760904 1.157948 0.780976 +VERTEX2 1812 -82.115160 1.765571 0.788204 +VERTEX2 1813 -81.328030 2.560781 0.790120 +VERTEX2 1814 -80.654727 3.204268 0.818740 +VERTEX2 1815 -79.934076 3.900908 0.747011 +VERTEX2 1816 -79.163055 4.554308 0.752587 +VERTEX2 1817 -78.360113 5.225527 0.775949 +VERTEX2 1818 -77.635780 5.986962 0.783205 +VERTEX2 1819 -76.891162 6.634930 0.789557 +VERTEX2 1820 -76.179423 7.257807 0.770790 +VERTEX2 1821 -75.246700 8.091800 0.781854 +VERTEX2 1822 -74.557375 8.785785 0.790911 +VERTEX2 1823 -73.871339 9.438419 0.752521 +VERTEX2 1824 -73.045517 10.136467 0.767444 +VERTEX2 1825 -72.345279 10.931627 0.784830 +VERTEX2 1826 -71.546478 11.533263 0.785759 +VERTEX2 1827 -70.769696 12.142186 0.800808 +VERTEX2 1828 -70.049486 12.824979 0.823639 +VERTEX2 1829 -69.510068 13.623639 0.818183 +VERTEX2 1830 -68.743664 14.451563 0.811546 +VERTEX2 1831 -67.953779 15.241271 0.850706 +VERTEX2 1832 -67.252154 15.960955 0.877595 +VERTEX2 1833 -66.547977 16.711223 0.827051 +VERTEX2 1834 -65.883144 17.415248 0.833165 +VERTEX2 1835 -65.156353 18.170255 0.829685 +VERTEX2 1836 -64.369299 18.932493 0.838634 +VERTEX2 1837 -63.614269 19.768666 0.839841 +VERTEX2 1838 -62.899492 20.485068 0.851236 +VERTEX2 1839 -62.245156 21.351583 0.845293 +VERTEX2 1840 -61.545828 22.170911 0.854284 +VERTEX2 1841 -60.872686 22.903209 0.782918 +VERTEX2 1842 -60.038800 23.709205 0.785088 +VERTEX2 1843 -59.209578 24.373047 0.792068 +VERTEX2 1844 -58.385697 25.165834 0.820690 +VERTEX2 1845 -57.770223 25.874945 0.841284 +VERTEX2 1846 -56.964031 26.745719 0.862673 +VERTEX2 1847 -56.211010 27.538170 0.874695 +VERTEX2 1848 -55.615196 28.203065 0.898239 +VERTEX2 1849 -54.978383 29.119337 0.906852 +VERTEX2 1850 -54.379090 29.926483 0.907105 +VERTEX2 1851 -53.748420 30.684453 0.916783 +VERTEX2 1852 -53.103715 31.462429 0.898570 +VERTEX2 1853 -52.329071 32.263212 0.910075 +VERTEX2 1854 -51.615385 33.065716 0.912235 +VERTEX2 1855 -51.041841 33.837462 0.909308 +VERTEX2 1856 -50.416198 34.557563 0.918009 +VERTEX2 1857 -49.775068 35.288747 0.925479 +VERTEX2 1858 -49.053546 36.264634 0.937635 +VERTEX2 1859 -48.340492 37.099336 0.922162 +VERTEX2 1860 -47.672680 37.865318 0.834424 +VERTEX2 1861 -46.957339 38.610575 0.847275 +VERTEX2 1862 -46.156671 39.330790 0.856773 +VERTEX2 1863 -45.488172 40.184206 0.884076 +VERTEX2 1864 -44.786607 40.957214 0.891523 +VERTEX2 1865 -44.141266 41.842442 0.878797 +VERTEX2 1866 -43.409001 42.652439 0.875852 +VERTEX2 1867 -42.667915 43.372089 0.877219 +VERTEX2 1868 -42.021711 44.117235 0.877228 +VERTEX2 1869 -41.345484 44.926083 0.880185 +VERTEX2 1870 -40.717802 45.791670 0.878471 +VERTEX2 1871 -40.017029 46.570772 0.878093 +VERTEX2 1872 -39.257142 47.376775 0.878098 +VERTEX2 1873 -38.465621 48.223307 0.884010 +VERTEX2 1874 -37.887580 48.919755 0.871730 +VERTEX2 1875 -37.174241 49.689579 0.864415 +VERTEX2 1876 -36.517846 50.441813 0.864835 +VERTEX2 1877 -35.778416 51.156511 0.874948 +VERTEX2 1878 -35.083386 52.002104 0.857026 +VERTEX2 1879 -34.380345 52.881161 0.997225 +VERTEX2 1880 -33.812069 53.570000 0.867943 +VERTEX2 1881 -33.065884 54.271455 0.880562 +VERTEX2 1882 -32.373445 55.178374 0.887331 +VERTEX2 1883 -31.611293 56.042796 0.881895 +VERTEX2 1884 -31.012230 56.824071 0.885776 +VERTEX2 1885 -30.220140 57.806129 0.858597 +VERTEX2 1886 -29.456041 58.620197 0.869141 +VERTEX2 1887 -28.755907 59.281547 0.862349 +VERTEX2 1888 -27.948915 60.130287 0.882078 +VERTEX2 1889 -27.390965 60.880110 0.861701 +VERTEX2 1890 -26.725720 61.711918 0.874863 +VERTEX2 1891 -26.085439 62.448906 0.868574 +VERTEX2 1892 -25.275566 63.359538 0.885337 +VERTEX2 1893 -24.607158 64.177987 0.875441 +VERTEX2 1894 -23.958571 64.841522 0.871731 +VERTEX2 1895 -23.223540 65.755959 0.851995 +VERTEX2 1896 -22.488045 66.533010 0.851226 +VERTEX2 1897 -21.805721 67.320744 0.851158 +VERTEX2 1898 -21.242896 67.995723 0.855991 +VERTEX2 1899 -20.544706 68.751668 0.851431 +VERTEX2 1900 -19.892519 69.501948 0.885628 +VERTEX2 1901 -19.332882 70.185192 0.824875 +VERTEX2 1902 -18.533119 70.978393 0.716597 +VERTEX2 1903 -17.787824 71.672129 0.881982 +VERTEX2 1904 -16.970554 72.504366 0.886753 +VERTEX2 1905 -16.185508 73.325233 0.940578 +VERTEX2 1906 -15.664086 74.138584 1.047379 +VERTEX2 1907 -15.021794 74.981652 0.995243 +VERTEX2 1908 -14.418862 75.859817 0.991668 +VERTEX2 1909 -13.808126 76.761051 0.972181 +VERTEX2 1910 -13.594527 77.015346 0.272135 +VERTEX2 1911 -12.590876 77.167821 0.078766 +VERTEX2 1912 -11.634216 76.933159 -0.397429 +VERTEX2 1913 -10.756385 76.377669 -0.653850 +VERTEX2 1914 -10.056049 75.742543 -0.640529 +VERTEX2 1915 -9.207855 75.101497 -0.669171 +VERTEX2 1916 -8.476963 74.495341 -0.714495 +VERTEX2 1917 -7.695078 73.766603 -0.744177 +VERTEX2 1918 -6.860317 72.941637 -0.745820 +VERTEX2 1919 -6.125468 72.102848 -0.762132 +VERTEX2 1920 -5.391937 71.201072 -0.791151 +VERTEX2 1921 -4.642195 70.478652 -0.644188 +VERTEX2 1922 -3.587205 69.913927 -0.306668 +VERTEX2 1923 -2.931521 69.923850 0.275238 +VERTEX2 1924 -2.453348 70.180281 0.880499 +VERTEX2 1925 -2.229814 70.724935 1.488603 +VERTEX2 1926 -2.331446 71.249965 2.070389 +VERTEX2 1927 -2.708269 71.744495 2.687066 +VERTEX2 1928 -3.140225 71.848391 -2.978301 +VERTEX2 1929 -3.464513 71.642590 -2.395755 +VERTEX2 1930 -3.667261 71.281925 -1.786109 +VERTEX2 1931 -3.585509 70.793069 -1.197585 +VERTEX2 1932 -3.367998 70.426235 -0.623926 +VERTEX2 1933 -2.847449 70.216380 -0.048966 +VERTEX2 1934 -2.559896 70.270348 0.520491 +VERTEX2 1935 -2.410522 70.466670 1.286527 +VERTEX2 1936 -2.380860 70.602682 1.924344 +VERTEX2 1937 -2.737149 71.255891 1.914662 +VERTEX2 1938 -2.917004 71.728943 2.314550 +VERTEX2 1939 -3.236598 72.093704 1.908741 +VERTEX2 1940 -3.210199 73.094617 1.548734 +EDGE2 1 0 -1.082078 -0.007851 -0.009693 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 2 1 -1.026697 0.059006 -0.007450 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 3 2 -1.083593 -0.076320 -0.012128 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 4 3 -1.081267 0.024536 -0.009858 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 5 4 -0.984019 -0.020296 0.050248 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 6 5 -1.020643 0.034546 0.010538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 7 6 -1.074638 0.001413 -0.000343 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 8 7 -1.030705 0.088901 -0.032878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 9 8 -1.005294 -0.168131 0.225786 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 10 9 -1.075867 -0.058994 0.165040 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 11 10 -1.036209 -0.097597 0.036032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 12 11 -0.647785 -0.156502 0.510727 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 13 12 -0.746326 -0.167311 0.549520 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 14 13 -0.927299 -0.018074 0.183105 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 15 14 -0.971871 -0.042307 0.034439 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 16 15 -0.945812 -0.044093 0.021013 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 17 16 -1.150371 0.044695 -0.059406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 18 17 -1.110739 0.006045 -0.073273 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 19 18 -0.967159 0.052527 0.016142 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 20 19 -1.039655 0.042429 0.029469 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 21 20 -1.082506 0.045726 -0.010186 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 22 21 -1.064889 0.074224 0.020472 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 23 22 -1.103812 0.144152 -0.156727 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 24 23 -1.061137 -0.048322 0.025695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 25 24 -1.120779 0.029446 0.011315 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 26 25 -1.146920 0.016703 0.011015 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 27 26 -1.012766 -0.004121 0.018422 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 28 27 -1.084075 -0.041412 0.008308 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 29 28 -1.148666 -0.084399 0.018031 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 30 29 -1.030555 0.123502 -0.049482 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 31 30 -1.028605 0.090859 -0.055108 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 32 31 -1.026782 0.010883 0.007784 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 33 32 -0.984453 0.033514 -0.013015 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 34 33 -1.088606 0.079160 -0.014304 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 35 34 -1.036143 0.020645 0.002170 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 36 35 -1.047487 0.010089 0.020175 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 37 36 -1.175787 -0.005670 0.034711 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 38 37 -1.082635 0.033549 0.008130 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 39 38 -0.998115 0.175274 0.002945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 40 39 -1.091057 0.059461 -0.041572 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 41 40 -0.996190 0.067166 0.010261 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 42 41 -1.161068 0.051837 -0.028397 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 43 42 -1.001881 0.119945 -0.097191 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 44 43 -1.058988 0.060228 -0.041116 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 45 44 -0.742207 -0.028485 -0.018788 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 46 45 -1.112321 0.034890 0.069615 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 47 46 -1.172048 -0.023909 0.065959 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 48 47 -1.116561 0.053728 0.010234 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 49 48 -1.084038 0.028120 0.007715 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 50 49 -1.121370 -0.042434 0.037793 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 51 50 -1.043439 -0.066723 0.015462 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 52 51 -1.032921 0.130329 -0.058361 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 53 52 -1.018523 0.110426 -0.075103 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 54 53 -0.963366 0.031893 0.005352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 55 54 -1.131259 0.079378 -0.012004 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 56 55 -0.963184 -0.052735 0.032528 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 57 56 -0.981304 0.045811 -0.002222 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 58 57 -1.027252 0.035040 0.006109 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 59 58 -0.982144 -0.420507 0.411796 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 60 59 -0.535153 -0.113965 0.550118 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 61 60 -1.003386 -0.010642 0.417221 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 62 61 -0.962938 0.065991 0.198577 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 63 62 -1.059052 0.031523 -0.014497 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 64 63 -1.112417 0.031264 -0.000382 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 65 64 -1.028063 -0.095293 0.004425 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 66 65 -0.952095 -0.029858 -0.009961 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 67 66 -1.046400 0.072759 0.037378 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 68 67 -0.921175 0.103126 -0.075970 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 69 68 -1.044313 0.296383 -0.201316 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 70 69 -0.724618 0.283810 -0.581913 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 71 70 -0.507822 0.163568 -0.533903 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 72 71 -1.041369 0.093244 -0.250323 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 73 72 -1.051183 0.020706 -0.103717 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 74 73 -1.017962 0.029772 0.017339 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 75 74 -1.028358 -0.110844 0.278877 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 76 75 -1.040762 0.081737 -0.042465 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 77 76 -1.053694 0.036364 -0.092914 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 78 77 -1.022746 0.142942 -0.221326 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 79 78 -0.968244 -0.073529 -0.003737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 80 79 -0.973972 -0.039049 0.138944 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 81 80 -1.147437 0.103292 -0.003994 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 82 81 -1.032084 -0.020833 0.000573 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 83 82 -1.005316 0.329501 -0.350488 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 84 83 -0.784397 0.348457 -0.584966 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 85 84 -0.887260 0.238843 -0.399388 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 86 85 -1.103673 0.251682 -0.464307 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 87 86 -1.166230 0.121949 -0.036845 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 88 87 -1.214790 -0.046841 0.154172 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 89 88 -0.994177 0.071653 0.050518 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 90 89 -1.105065 0.053555 0.008826 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 91 90 -1.164640 0.025919 0.002698 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 92 91 -0.935716 0.080925 -0.013911 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 93 92 -1.130034 0.034654 0.050371 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 94 93 -1.006825 0.046598 -0.017900 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 95 94 -1.097301 0.096786 0.027930 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 96 95 -1.034133 0.125857 -0.028745 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 97 96 -1.015896 0.113457 -0.023420 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 98 97 -1.122927 -0.046414 0.053399 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 99 98 -0.986276 0.203720 -0.052467 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 100 99 -1.033793 0.004810 -0.010228 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 101 100 -1.073755 0.036561 0.004568 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 102 101 -1.071510 0.031892 0.044338 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 103 102 -1.027932 -0.107639 -0.013427 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 104 103 -0.665464 0.428120 -0.519801 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 105 104 -0.323913 0.155386 -0.599929 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 106 105 -1.090588 0.005749 -0.417351 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 107 106 -1.138806 -0.046206 0.002737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 108 107 -0.924492 0.059200 -0.047594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 109 108 -1.148537 0.101139 -0.010589 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 110 109 -1.011529 0.048785 -0.013581 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 111 110 -1.034002 -0.087129 -0.001627 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 112 111 -0.997018 -0.066789 0.010612 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 113 112 -1.011100 0.041253 -0.002156 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 114 113 -1.065320 0.048219 -0.000280 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 115 114 -1.185734 0.031000 0.003305 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 116 115 -0.975768 0.085399 -0.000670 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 117 116 -0.849980 0.586651 -0.679647 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 118 117 -0.374391 0.089468 -0.542167 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 119 118 -1.060697 0.178463 -0.153340 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 120 119 -1.096742 0.170432 -0.420143 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 121 120 -1.098162 0.099226 0.058862 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 122 121 -1.094678 -0.016115 0.044472 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 123 122 -1.153829 0.058793 0.040108 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 124 123 -1.018921 0.076280 0.023666 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 125 124 -1.054997 0.003955 0.039382 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 126 125 -1.116808 0.074556 0.034261 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 126 57 2.491857 -2.640020 1.600275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 126 58 2.491144 -1.598192 1.586625 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 127 126 -1.173233 0.064973 0.010494 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 127 57 1.371619 -2.555212 1.606885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 127 58 1.364019 -1.513412 1.593235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 128 127 -0.018882 -0.057433 0.648892 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 128 56 3.143160 -2.102787 2.256595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 128 57 2.535259 -1.297058 2.253675 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 129 128 0.037701 -0.002502 0.665396 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 129 56 3.776656 0.273586 2.920985 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 129 57 2.801264 0.533112 2.918065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 130 129 -0.233999 0.308398 -0.585706 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 130 56 3.020180 -1.592628 2.345175 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 130 57 2.343384 -0.843836 2.342255 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 130 128 -0.296678 0.223857 0.088580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 130 127 -0.387978 0.130898 0.735370 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 131 130 -0.111170 -0.058474 -0.515666 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 131 56 1.756966 -2.822137 1.829645 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 131 57 1.537284 -1.837006 1.826725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 131 128 -0.233285 0.393460 -0.426950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 131 127 -0.358547 0.357594 0.219840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 132 131 0.016708 -0.005348 -0.519230 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 132 57 0.421476 -2.310364 1.317610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 132 126 -1.235149 0.919907 -0.282665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 132 128 -0.037402 0.500211 -0.936065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 132 127 -0.164260 0.529946 -0.289275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 132 58 0.711373 -1.309682 1.303960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 133 132 -0.138736 -0.066708 -0.486653 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 133 126 -0.755912 1.400220 -0.770435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 133 127 0.007332 0.553857 -0.777045 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 133 58 -0.081306 -1.481606 0.816190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 133 59 0.618345 -0.739433 0.403310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 134 133 -0.589032 0.025357 -0.264241 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 134 132 -0.608325 0.033063 -0.732570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 134 58 -0.955341 -1.416385 0.571390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 134 59 -0.096676 -0.865908 0.158510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 135 134 -0.758277 -0.093150 0.483730 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 135 60 -0.022340 -0.634615 0.086140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 135 59 -0.476150 -0.835401 0.639420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 136 135 -0.731834 -0.164441 0.489319 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 136 61 0.542253 -0.389920 0.177555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 136 60 -0.450198 -0.667617 0.591335 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 137 136 -1.034020 0.016618 0.054341 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 137 62 0.510718 -0.345767 0.012780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 137 61 -0.487352 -0.391494 0.225365 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 138 137 -1.067746 0.005432 0.042862 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 138 63 0.511476 -0.305862 0.060960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 138 62 -0.496678 -0.329516 0.052730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 139 138 -1.102987 -0.015233 0.028453 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 139 63 -0.569613 -0.281534 0.074800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 139 64 0.558979 -0.212769 0.082850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 140 139 -1.135601 0.002369 -0.033809 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 140 65 0.518940 -0.110823 0.045230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 140 64 -0.541998 -0.167521 0.046260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 141 140 -0.975531 -0.053702 0.031352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 141 66 0.531185 -0.070215 0.075620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 141 65 -0.443142 -0.114997 0.068730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 142 141 -1.046836 0.128195 -0.132972 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 142 67 0.492363 -0.135159 -0.089680 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 142 66 -0.536013 -0.036855 -0.056560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 143 142 -0.934594 -0.057442 0.029979 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 143 67 -0.475260 -0.106770 -0.056260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 143 68 0.514690 -0.151946 0.013190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 144 143 -1.032961 -0.068892 0.026034 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 144 68 -0.490705 -0.114154 0.031070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 144 69 0.564239 -0.095257 0.243815 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 145 144 -1.011270 0.067586 0.047931 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 145 69 -0.463314 -0.045761 0.275325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 145 70 0.212880 0.296631 0.846265 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 146 145 -1.083828 0.227770 -0.168751 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 147 146 -0.774119 -0.286182 0.215867 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 148 147 -0.984595 0.052080 0.147386 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 149 148 -1.183089 -0.133388 0.223388 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 150 149 -1.009490 -0.016585 0.394807 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 151 150 -1.072513 0.234504 -0.332864 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 152 151 -1.068875 0.155919 -0.394133 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 153 152 -1.108701 0.118392 -0.133785 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 154 153 -1.084421 0.019125 0.016881 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 155 154 -1.145106 0.090075 0.014842 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 156 155 -1.153275 -0.013458 -0.094919 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 157 156 -0.957801 -0.020193 0.001225 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 158 157 -1.146804 0.015838 0.026411 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 159 158 -1.034938 -0.081555 0.052528 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 160 159 -0.957732 0.093316 0.008619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 161 160 -1.033403 0.028672 0.025489 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 162 161 -1.046384 -0.144871 0.149181 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 163 162 -0.514833 -0.221956 0.591197 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 164 163 -0.447859 -0.167774 0.608629 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 165 164 -1.047928 0.012457 0.183365 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 166 165 -0.967467 -0.004441 0.012447 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 167 166 -1.118367 0.084217 0.024617 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 168 167 -0.927306 -0.038961 0.016683 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 169 168 -1.015676 0.023295 -0.031150 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 170 169 -1.032555 -0.036578 0.020758 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 171 170 -1.227354 -0.012743 0.043454 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 172 171 -0.994444 -0.014244 -0.006073 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 173 172 -1.135812 0.027402 -0.072572 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 174 173 -0.935901 0.370085 -0.431978 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 175 174 -0.589581 0.135556 -0.580004 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 176 175 -1.159647 0.272664 -0.481250 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 177 176 -0.993561 0.072412 -0.051207 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 178 177 -1.061036 -0.018412 -0.023228 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 179 178 -1.029552 0.067475 -0.000413 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 180 179 -1.182284 -0.048001 0.008534 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 181 180 -1.235547 -0.057269 0.007541 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 182 181 -1.107511 0.069240 -0.065034 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 183 182 -1.062865 0.045032 -0.022274 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 184 183 -0.979928 0.088650 0.009514 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 185 184 -1.061648 0.026241 0.009444 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 186 185 -1.204240 -0.007704 0.004655 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 187 186 -1.066427 0.047057 0.010387 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 188 187 -1.132808 0.043739 0.010666 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 189 188 -1.151613 -0.041902 0.027493 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 190 189 -1.161832 -0.016437 0.006220 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 191 190 -1.063528 0.077755 0.020825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 192 191 -1.116644 0.059756 -0.043060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 193 192 -0.990119 -0.043927 0.016062 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 194 193 -0.954717 -0.005502 0.014737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 195 194 -0.989229 -0.035357 0.014929 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 196 195 -1.044744 -0.037666 0.010992 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 197 196 -1.186367 -0.077003 0.141020 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 198 197 -1.133430 0.126303 -0.043152 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 199 198 -1.157294 -0.024475 -0.146148 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 200 199 -1.081050 0.081375 -0.026450 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 201 200 -0.976048 0.096612 -0.001978 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 202 201 -1.020117 0.051072 0.006123 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 203 202 -0.908489 -0.041316 0.008094 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 204 203 -1.203136 0.085694 0.029850 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 205 204 -1.256374 0.037610 0.002942 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 206 205 -0.929561 -0.000748 0.009032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 207 206 -1.141610 -0.026961 0.054729 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 208 207 -1.011364 0.077527 0.026249 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 209 208 -1.158420 0.025037 0.015051 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 210 209 -1.113337 -0.057036 0.015106 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 211 210 -1.143132 0.194528 -0.159745 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 212 211 -1.122768 -0.043558 0.006016 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 213 212 -1.154424 -0.036336 0.021585 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 214 213 -1.032403 0.002055 0.076102 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 215 214 -1.074054 -0.017310 -0.003595 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 216 215 -1.027597 -0.043241 0.046833 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 217 216 -1.137834 -0.035061 -0.047603 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 218 217 -1.049710 0.105042 0.022098 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 219 218 -1.082295 0.055186 -0.087039 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 220 219 -1.125646 0.124401 -0.055309 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 221 220 -1.174081 0.072093 0.010489 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 222 221 -1.105343 0.031902 0.007190 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 223 222 -1.101177 -0.122883 0.097825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 224 223 -1.060635 0.076930 0.016104 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 225 224 -0.993479 0.086812 -0.008514 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 226 225 -1.157418 0.026499 -0.013499 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 227 226 -0.970705 0.121493 -0.160870 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 228 227 -0.561596 0.183129 -0.506864 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 229 228 -0.484812 0.310068 -0.603836 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 230 229 -1.072574 0.191845 -0.301078 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 231 230 -1.032672 0.199817 -0.017037 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 232 231 -1.070459 0.008098 -0.014492 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 233 232 -0.951235 0.074318 -0.027498 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 234 233 -1.099894 0.026696 0.014749 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 235 234 -1.074833 -0.038999 -0.020104 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 236 235 -1.083473 0.092340 -0.035466 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 237 236 -1.015498 0.065650 0.015630 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 238 237 -1.027758 0.127752 -0.022822 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 239 238 -1.094662 0.054385 0.036764 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 240 239 -1.091268 0.065061 0.031448 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 241 240 -1.147656 0.068063 -0.002803 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 242 241 -1.138351 0.005521 -0.007935 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 243 242 -0.929964 0.175581 -0.130703 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 244 243 -0.594685 0.234602 -0.610027 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 245 244 -0.593931 0.233272 -0.584873 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 246 245 -1.068760 0.028150 0.013298 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 247 246 -1.051587 0.062388 -0.203685 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 248 247 -1.059630 0.075348 -0.047786 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 249 248 -1.016609 0.052161 -0.034723 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 250 249 -1.204604 -0.013130 -0.011882 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 251 250 -1.053832 0.010476 0.005032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 252 251 -1.059366 0.060539 -0.005429 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 253 252 -1.092828 0.015204 0.014216 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 254 253 -1.036509 -0.034104 0.072772 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 255 254 -1.003112 0.040896 0.034825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 256 255 -1.098431 -0.042627 0.032313 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 257 256 -1.114511 -0.054165 0.006610 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 258 257 -1.134734 0.009114 -0.004829 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 259 258 -1.131617 0.051945 -0.007821 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 260 259 -1.105276 0.028878 -0.004687 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 261 260 -1.061193 0.035603 -0.018583 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 262 261 -1.051829 -0.012778 -0.028570 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 263 262 -1.073170 0.009940 -0.012755 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 264 263 -1.135814 0.016691 0.001819 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 265 264 -1.091900 -0.046044 0.026186 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 266 265 -1.256784 -0.077640 0.019497 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 267 266 -0.957125 0.060303 -0.023853 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 268 267 -1.143763 0.055597 -0.012444 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 269 268 -1.075231 0.024455 0.030803 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 270 269 -1.123960 -0.062288 -0.007715 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 271 270 -1.085984 0.022807 -0.014364 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 272 271 -1.116131 -0.001814 -0.020804 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 273 272 -1.063514 0.083828 -0.006179 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 274 273 -1.168350 0.009051 -0.005396 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 275 274 -1.140104 -0.010344 0.105515 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 276 275 -1.143432 0.092069 -0.019067 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 277 276 -1.150701 0.021103 -0.001265 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 278 277 -1.111006 0.027101 -0.021841 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 279 278 -1.072415 0.049812 -0.046790 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 280 279 -1.054644 0.044529 -0.033981 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 281 280 -1.138883 -0.040330 -0.032758 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 282 281 -0.980773 0.052839 0.000060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 283 282 -1.089574 0.081688 -0.006619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 284 283 -1.177065 -0.009367 0.042403 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 285 284 -1.078476 0.044042 0.003342 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 286 285 -1.032134 0.065053 -0.013509 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 287 286 -1.076364 0.033468 0.032657 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 288 287 -1.105426 -0.061713 -0.023273 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 289 288 -1.180690 -0.046165 -0.026619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 290 289 -1.141519 0.039938 -0.024164 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 291 290 -1.042436 0.053064 -0.024800 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 292 291 -1.053607 -0.033748 -0.012334 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 293 292 -1.095934 -0.093093 0.116198 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 294 293 -1.041002 0.080837 -0.023518 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 295 294 -1.092729 0.060853 -0.060701 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 296 295 -1.120008 0.146760 -0.258252 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 297 296 -0.528587 0.152968 -0.619212 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 298 297 -0.504574 0.126965 -0.529737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 299 298 -1.344965 0.164284 -0.304289 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 299 161 2.583186 -2.315397 1.431380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 300 299 -1.013454 0.117701 0.012451 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 300 161 1.577413 -2.186306 1.442630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 300 160 1.366579 -3.234307 1.451610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 300 162 1.787617 -1.168927 1.290120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 301 300 -1.069046 0.033941 0.091579 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 301 163 1.032715 -0.445238 0.792000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 302 301 -1.103897 -0.015655 0.035418 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 302 164 0.324241 -0.184611 0.205770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 302 163 -0.073168 -0.406269 0.817550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 303 302 -0.991623 -0.062133 0.010004 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 303 164 -0.730645 -0.189918 0.228350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 303 165 0.276693 -0.133174 0.039680 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 304 303 -0.887458 0.076372 0.018402 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 304 166 0.324890 -0.051382 0.034560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 304 165 -0.667389 -0.083396 0.057760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 305 304 -1.135506 -0.019539 -0.003910 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 305 166 -0.788679 -0.017230 0.028740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 305 167 0.362200 -0.050674 0.011150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 306 305 -1.010164 0.024045 -0.029477 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 306 168 0.340922 -0.075702 -0.033580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 306 167 -0.663227 -0.022696 -0.014060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 307 306 -1.086942 0.059746 0.026786 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 307 168 -0.786872 -0.051489 -0.008130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 307 169 0.301426 -0.030984 0.027130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 308 307 -1.136043 -0.004112 0.025264 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 308 170 0.262050 0.019758 0.033320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 308 169 -0.824724 -0.012346 0.055700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 309 308 -1.026175 0.044818 -0.017218 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 309 170 -0.793868 0.016864 0.007790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 309 171 0.330660 -0.001992 -0.031670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 310 309 -1.099543 -0.031353 0.032840 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 310 172 0.235922 -0.044278 -0.015310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 310 171 -0.725732 -0.032952 -0.015100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 310 173 1.328440 -0.072315 0.053990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 311 310 -0.985392 -0.019496 -0.077868 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 311 172 -0.738239 0.000001 -0.078450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 311 173 0.350333 -0.096915 -0.009150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 312 311 -1.052823 0.206995 -0.202869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 312 174 0.261771 -0.106646 0.241560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 312 175 0.709385 0.109193 0.819640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 312 173 -0.731466 0.003997 -0.201220 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 313 312 -0.421189 0.201721 -0.620084 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 313 174 -0.290584 -0.040595 -0.369530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 313 175 0.199861 -0.120641 0.208550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 314 313 -0.787879 0.173121 -0.476870 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 314 175 -0.692176 -0.113497 -0.259860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 314 176 0.418910 -0.090691 0.220190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 315 314 -0.914053 0.315564 -0.330012 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 315 177 0.434119 -0.158363 -0.050640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 315 176 -0.617189 -0.037865 -0.111780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 316 315 -1.054634 -0.073418 0.032246 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 316 177 -0.615098 -0.152692 -0.030990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 316 178 0.410740 -0.160460 0.000670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 317 316 -1.046403 0.028177 0.006054 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 317 178 -0.627333 -0.157494 0.011190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 317 179 0.383754 -0.170759 0.003060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 318 317 -1.072713 0.005329 0.005249 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 318 180 0.427610 -0.156984 0.004390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 318 179 -0.666704 -0.139013 0.018670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 319 318 -1.019209 -0.010759 0.008530 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 319 181 0.512637 -0.165491 -0.005180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 319 180 -0.626655 -0.138794 0.014280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 320 319 -1.101487 0.007777 -0.005933 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 320 181 -0.572796 -0.148474 0.003300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 320 182 0.544318 -0.136663 0.052150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 321 320 -1.044889 0.096437 0.024547 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 321 183 0.596112 -0.036582 0.093925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 321 182 -0.512243 -0.110604 0.065910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 322 321 -0.918094 0.041333 0.030807 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 322 183 -0.435812 -0.011943 0.108025 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 322 184 0.630004 0.070494 0.097200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 323 322 -1.007107 -0.041558 0.014504 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 323 185 0.686384 0.190505 0.112850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 323 184 -0.329510 0.104775 0.117710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 324 323 -0.980365 0.004626 -0.062023 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 324 185 -0.383017 0.221883 0.044580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 324 186 0.826211 0.228976 0.028030 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 325 324 -1.008114 -0.079904 0.034580 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 325 187 0.797589 0.244774 0.029720 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 325 186 -0.263141 0.235013 0.045160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 326 325 -0.981180 0.011618 0.002204 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 326 187 -0.200192 0.292002 0.036280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 326 188 0.880623 0.313784 0.022860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 327 326 -1.090312 -0.033537 0.006619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 327 189 0.933978 0.347204 0.026540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 327 188 -0.191464 0.325081 0.041840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 328 327 -1.101888 0.038971 -0.044562 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 328 189 -0.094699 0.356760 -0.018540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 328 190 0.983243 0.298405 -0.027120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 329 328 -1.053339 0.063602 -0.040297 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 329 189 -1.118032 0.402433 -0.040940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 329 190 -0.041668 0.319949 -0.049520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 330 329 -1.033008 -0.030032 0.050916 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 330 191 0.029822 0.280525 -0.041860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 331 330 -1.192397 -0.054856 0.021958 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 331 193 1.039483 0.243072 0.004560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 331 191 -1.137166 0.268091 -0.026920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 331 192 0.024512 0.245493 0.017260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 332 331 -1.037898 -0.019795 0.003339 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 332 193 0.072817 0.282910 0.009180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 332 194 1.106134 0.244650 -0.001090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 332 192 -0.942154 0.280643 0.021880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 333 332 -0.923211 0.019404 -0.012564 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 333 193 -0.874086 0.298569 0.000880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 333 194 0.158878 0.251733 -0.009390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 334 333 -1.076794 0.002949 -0.001436 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 334 194 -0.946025 0.253887 -0.000740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 334 195 0.014078 0.236850 -0.010930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 335 334 -1.027883 -0.067785 0.011132 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 335 196 0.073038 0.235366 -0.009450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 335 195 -0.998087 0.265967 0.002540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 336 335 -1.034288 0.011844 0.015337 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 336 196 -0.949603 0.258121 0.000340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 336 197 0.317000 0.166891 -0.133790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 337 336 -1.048979 0.026814 -0.101474 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 337 198 0.291333 -0.103647 -0.164960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 337 197 -0.749907 0.180619 -0.224340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 338 337 -1.095233 0.036538 0.041250 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 338 198 -0.783189 -0.096479 -0.119470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 338 199 0.383951 -0.130228 0.028150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 339 338 -0.998209 0.076683 -0.001822 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 339 199 -0.665934 -0.112944 0.036260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 339 200 0.422263 -0.073547 0.069975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 340 339 -1.198749 0.145367 0.009673 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 340 201 0.367317 0.019835 0.072595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 340 200 -0.708329 -0.052942 0.084935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 341 340 -0.928017 0.080602 0.017613 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 341 201 -0.570470 0.021367 0.079345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 341 202 0.517197 0.076901 0.074100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 342 341 -1.086081 0.001637 0.005969 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 342 203 0.430349 0.170645 0.087570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 342 202 -0.574186 0.105163 0.084690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 343 342 -0.978137 0.081176 -0.030963 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 343 204 0.593307 0.232680 0.046960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 343 203 -0.543729 0.202652 0.056430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 344 343 -0.960979 0.085305 -0.007173 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 344 204 -0.410141 0.268901 0.046120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 344 205 0.790843 0.274425 0.029710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 345 344 -1.241934 0.018381 0.022980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 345 206 0.632351 0.309623 0.034740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 345 205 -0.388292 0.285409 0.046360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 346 345 -1.137450 0.010361 0.016362 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 346 206 -0.438051 0.322252 0.039670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 346 207 0.626068 0.345366 -0.017680 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 347 346 -0.909931 0.090240 -0.126309 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 347 208 0.658307 0.213533 -0.158835 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 347 207 -0.325915 0.392106 -0.135595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 348 347 -0.876373 0.067825 -0.019909 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 348 208 -0.328642 0.202871 -0.177485 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 348 209 0.777859 -0.026976 -0.187325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 349 348 -1.078784 0.018745 0.081391 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 349 209 -0.266580 0.027694 -0.106320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 349 210 0.815291 -0.103242 -0.114930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 350 349 -1.137919 -0.058688 0.010066 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 350 211 0.849633 -0.125796 0.029410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 350 210 -0.247416 -0.073518 -0.107260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 351 350 -1.074874 0.056845 0.003085 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 351 212 0.949708 -0.094468 0.030980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 351 211 -0.190608 -0.091409 0.037050 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 352 351 -0.884730 0.005303 0.045187 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 352 212 -0.040613 -0.042080 0.062030 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 352 213 1.071823 -0.000092 0.054760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 353 352 -1.101628 0.015504 0.006520 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 353 214 1.043060 0.049931 -0.004770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 353 213 -0.054261 0.024581 0.063550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 354 353 -1.014995 -0.004495 0.011341 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 354 215 1.059081 0.050045 -0.002240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 354 214 -0.035044 0.074289 0.003040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 355 354 -1.083545 0.066025 0.002756 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 355 215 0.034963 0.076525 0.006740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 355 216 1.050855 0.057417 -0.052410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 355 214 -1.059336 0.090942 0.012020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 356 355 -1.080109 -0.105845 0.019477 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 356 215 -0.992511 0.081140 0.015660 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 356 216 0.023512 0.071096 -0.043490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 357 356 -0.951138 -0.088858 0.016507 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 357 216 -1.011364 0.111591 -0.038790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 357 217 0.109813 0.071825 0.007890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 358 357 -1.012257 0.045650 -0.042521 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 358 218 0.160121 -0.001483 -0.045260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 358 217 -0.910136 0.088988 -0.035980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 359 358 -1.149097 0.056826 -0.118878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 359 219 0.188651 -0.098790 -0.065165 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 359 218 -0.924999 0.050972 -0.161915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 360 359 -1.046179 0.045268 0.087431 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 360 219 -0.816934 -0.102211 0.012200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 360 220 0.275709 -0.078347 0.058125 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 361 360 -1.006647 -0.004609 0.001060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 361 220 -0.715026 -0.063585 0.063275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 361 221 0.413105 -0.031033 0.050235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 362 361 -1.117927 0.097455 -0.000207 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 362 221 -0.659727 0.008169 0.053455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 362 222 0.398975 0.034664 0.048455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 363 362 -1.036753 0.035343 0.014519 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 363 223 0.468017 0.038586 -0.026790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 363 222 -0.643218 0.048754 0.056225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 364 363 -1.147484 0.033126 0.011121 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 364 223 -0.614743 0.072258 -0.025850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 364 224 0.487223 0.019210 -0.026780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 365 364 -1.069447 -0.023397 0.032348 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 365 224 -0.561224 0.020703 0.017140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 365 225 0.510957 0.005998 0.008180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 366 365 -1.047656 -0.051715 0.033599 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 366 225 -0.526162 0.017161 0.012440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 366 226 0.617340 0.015050 0.010480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 367 366 -1.013378 0.040296 0.001298 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 367 226 -0.415529 0.038967 0.011230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 367 227 0.599866 0.056300 0.174875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 368 367 -0.964284 0.005665 0.005474 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 368 227 -0.433242 0.063551 0.178905 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 369 368 -1.009591 0.059575 0.012522 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 370 369 -0.990375 -0.023607 0.007013 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 371 370 -1.060992 -0.073315 -0.007980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 372 371 -1.064832 -0.034110 0.006453 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 373 372 -0.915955 0.040746 -0.027814 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 374 373 -1.052661 0.033172 0.024638 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 375 374 -1.068342 0.031491 -0.044091 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 376 375 -1.147216 0.025343 0.000390 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 377 376 -0.897427 -0.066536 0.009918 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 378 377 -1.115860 -0.031081 -0.000303 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 379 378 -1.079468 0.058854 -0.000031 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 380 379 -0.970654 -0.010119 -0.000726 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 381 380 -1.081818 -0.010938 -0.022706 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 382 381 -0.988400 0.083394 -0.006023 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 383 382 -0.925736 0.053722 0.003404 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 384 383 -1.051130 -0.039854 -0.001349 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 385 384 -0.988085 0.015006 0.017480 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 386 385 -1.096087 0.061778 0.004372 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 387 386 -1.041893 -0.004053 -0.004277 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 388 387 -1.057265 0.075629 0.001321 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 389 388 -1.028470 -0.019653 0.003785 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 390 389 -1.065173 -0.001256 -0.004450 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 391 390 -1.266952 0.076944 -0.030680 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 392 391 -0.981729 0.023724 0.004295 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 393 392 -1.022708 0.003925 0.004982 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 394 393 -1.178408 -0.023913 -0.003811 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 395 394 -0.968395 0.004662 -0.017835 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 396 395 -1.020145 -0.021788 -0.006092 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 397 396 -0.977998 0.038433 -0.001962 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 398 397 -1.105340 0.080566 0.011477 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 399 398 -0.999488 0.063640 0.021919 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 400 399 -0.978195 0.097972 -0.080743 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 401 400 -0.977656 0.034393 -0.023116 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 402 401 -1.068806 0.038807 -0.081683 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 403 402 -1.069297 -0.035194 0.015879 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 404 403 -1.058320 -0.016566 0.093129 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 405 404 -1.199286 0.023331 0.064597 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 406 405 -0.871421 0.007411 -0.003954 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 407 406 -0.992538 0.086847 -0.008412 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 408 407 -1.074018 -0.002120 0.002333 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 409 408 -0.985995 0.021720 0.008829 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 410 409 -1.012175 0.020704 0.024721 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 411 410 -0.875334 0.011663 0.103176 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 412 411 -1.041459 0.131115 -0.179499 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 413 412 -1.023976 0.119943 -0.001246 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 414 413 -0.941275 0.012771 0.007205 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 415 414 -0.956047 -0.008988 0.008217 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 416 415 -1.094006 0.065601 -0.001672 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 417 416 -1.072522 0.027895 -0.006416 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 418 417 -1.090893 0.082969 0.009894 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 419 418 -0.994616 0.144060 -0.116319 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 420 419 -0.749166 0.217807 -0.516645 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 421 420 -0.405356 0.148696 -0.655163 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 422 421 -1.031807 0.055675 -0.412455 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 423 422 -1.046679 0.035476 -0.017112 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 424 423 -0.963483 0.080255 -0.009224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 425 424 -1.069407 -0.018116 0.067845 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 426 425 -1.021478 -0.075648 0.009275 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 427 426 -1.112806 -0.023046 0.064128 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 428 427 -1.057739 0.010246 -0.006082 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 429 428 -1.022461 0.037787 -0.013483 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 430 429 -1.071699 -0.049524 -0.004480 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 431 430 -1.001893 0.027916 -0.034444 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 432 431 -1.148091 -0.069913 0.039625 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 433 432 -1.024692 -0.037583 0.094510 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 434 433 -1.063484 0.081739 -0.011167 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 435 434 -1.053140 0.084086 -0.010705 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 436 435 -1.127553 0.018256 -0.022699 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 437 436 -0.382846 0.299806 -0.601704 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 438 437 -0.268903 0.052525 -0.495961 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 439 438 -0.221234 0.087794 -0.474408 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 439 437 -0.437126 0.251497 -0.998630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 440 439 -1.095587 0.070962 0.016649 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 441 440 -1.022193 0.050072 0.002793 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 442 441 -0.991712 0.091210 0.006104 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 443 442 -1.214750 0.076958 0.002390 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 444 443 -1.132572 0.020971 0.005801 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 445 444 -0.870531 -0.016685 0.005224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 446 445 -1.163546 0.100291 0.024864 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 447 446 -0.991855 0.030685 0.122038 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 448 447 -1.084090 -0.106502 0.090876 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 449 448 -1.098407 0.002987 -0.144788 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 450 449 -1.089128 -0.123236 -0.081021 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 451 450 -1.028377 0.011119 -0.024697 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 452 451 -1.081170 -0.041132 0.008174 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 453 452 -1.129676 -0.015510 0.003286 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 454 453 -0.897078 0.069028 0.003260 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 455 454 -1.059716 -0.052022 0.000114 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 456 455 -1.009516 0.105621 0.004112 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 457 456 -1.127168 0.040771 0.015769 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 458 457 -1.136039 -0.019541 -0.008863 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 459 458 -0.954649 -0.008874 0.019105 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 460 459 -1.049822 0.053643 0.010626 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 461 460 -1.029052 0.031122 -0.002271 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 462 461 -1.073319 -0.097864 0.004287 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 463 462 -0.961931 0.089773 -0.015683 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 464 463 -1.080339 0.103371 -0.029887 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 465 464 -1.070026 -0.010736 0.022254 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 466 465 -1.132878 0.118739 -0.062296 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 467 466 -1.035467 -0.044470 0.015586 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 468 467 -1.049334 0.113365 0.015367 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 469 468 -0.976846 0.039204 -0.000945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 470 469 -1.038978 0.007927 0.029695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 471 470 -1.162363 -0.026927 0.013734 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 472 471 -1.084345 -0.046388 0.021755 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 473 472 -1.170344 0.044410 0.027712 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 474 473 -1.172505 -0.028800 -0.010972 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 475 474 -1.090384 0.011031 -0.052925 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 476 475 -1.066011 -0.021438 0.015558 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 477 476 -1.067098 0.032860 -0.001805 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 478 477 -1.093555 -0.012807 0.016539 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 479 478 -0.986738 0.054419 0.006361 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 480 479 -1.054042 0.052069 0.009971 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 481 480 -1.142334 0.024784 -0.024343 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 482 481 -1.094973 0.082264 -0.084271 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 483 482 -1.091358 0.022480 0.025619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 484 483 -1.085617 -0.019236 0.029479 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 485 484 -1.103935 0.024894 0.019197 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 486 485 -1.211049 0.060322 0.009575 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 487 486 -1.025189 0.021088 0.027231 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 487 242 2.644889 2.203322 -1.496550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 488 487 -0.984100 0.166246 -0.218941 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 488 241 2.190476 2.860060 -1.726951 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 488 242 2.009541 1.798507 -1.714421 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 489 488 -0.998520 0.283348 -0.391756 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 490 489 -0.863776 0.272857 -0.495048 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 491 490 -0.916104 0.156131 -0.427996 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 491 239 3.316433 0.672241 -2.999420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 491 238 4.433968 0.850423 -2.957110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 492 491 -1.079739 0.246977 -0.388388 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 492 237 4.407989 -0.993474 2.910025 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 492 238 3.404755 -0.734101 2.926165 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 493 492 -1.060306 0.045314 0.086024 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 493 237 3.403254 -0.635306 3.004945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 493 236 4.460047 -0.783273 3.027875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 494 493 -1.152503 -0.036795 0.147164 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 494 235 4.593178 -0.172840 -3.129930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 494 236 3.442491 -0.151739 -3.114150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 495 494 -1.078320 0.019446 0.043073 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 495 235 3.522896 0.008861 -3.090520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 495 234 4.605006 0.041266 -3.111250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 496 495 -1.131964 -0.045445 0.030367 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 496 233 4.600206 0.236919 -3.069890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 496 234 3.524143 0.189403 -3.076560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 497 496 -1.124014 0.049015 -0.033892 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 497 233 3.453362 0.029901 -3.131800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 497 232 4.401850 0.013104 3.127185 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 498 497 -1.152661 0.147145 0.030083 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 498 231 4.422368 0.136338 3.129825 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 498 232 3.296659 0.142114 -3.132360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 499 498 -1.003045 -0.027963 0.041267 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 499 231 3.344009 0.323301 -3.109940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 499 230 4.428688 0.302239 -3.129140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 500 499 -1.079265 0.067710 0.033294 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 500 230 3.369011 0.428528 -3.105250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 500 229 4.463420 0.388543 2.878665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 501 500 -1.039270 -0.052037 0.015454 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 501 229 3.436594 0.453388 2.888305 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 502 501 -0.928113 0.110367 -0.008845 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 502 226 3.106578 -1.374966 1.611340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 502 367 3.133350 -0.958473 1.600110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 503 502 -1.050480 0.047888 -0.117793 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 503 366 1.838786 -2.281709 1.487940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 503 226 1.874880 -1.665242 1.498420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 503 367 1.948411 -1.254418 1.487190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 504 503 -0.971004 0.247522 -0.275783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 504 226 0.340140 -1.860242 1.231340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 505 504 -0.661784 0.104073 -0.526993 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 505 369 0.729312 -0.116969 0.678070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 505 368 -0.073636 -0.724426 0.679520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 506 505 -0.511130 0.163911 -0.529443 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 506 369 -0.004388 -0.273238 0.140480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 506 368 -1.005136 -0.383846 0.141930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 506 370 1.014179 -0.163580 0.124130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 507 506 -1.055637 0.036401 -0.029832 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 507 371 0.968003 -0.081181 0.091520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 507 370 -0.108671 -0.166769 0.092970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 508 507 -1.037899 0.217524 -0.165892 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 508 372 0.913256 -0.248983 -0.070990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 508 371 -0.113281 -0.148218 -0.069110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 509 508 -1.024408 0.010353 -0.002937 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 509 372 -0.077307 -0.261298 -0.083860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 509 373 0.909353 -0.363488 -0.055390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 510 509 -1.119759 0.104140 -0.019717 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 510 374 0.952839 -0.429061 -0.064510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 510 373 -0.147524 -0.333324 -0.070490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 511 510 -1.062970 0.129328 -0.016695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 511 375 0.965617 -0.426350 -0.002770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 511 374 -0.117885 -0.372290 -0.053740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 512 511 -1.046749 -0.047670 0.014493 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 512 375 -0.087062 -0.407157 0.005270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 512 376 0.966849 -0.428056 0.000480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 513 512 -1.054792 0.051692 -0.025877 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 513 377 0.860902 -0.444894 -0.027440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 513 376 -0.098256 -0.404005 -0.025100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 514 513 -0.952206 0.023261 -0.019730 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 514 377 -0.141055 -0.420060 -0.047100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 514 378 1.023416 -0.490986 -0.058770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 515 514 -1.156597 -0.037279 -0.022967 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 515 378 -0.080440 -0.460165 -0.069395 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 515 379 1.060051 -0.560993 -0.069275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 516 515 -0.886797 0.114886 -0.071826 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 516 378 -1.014385 -0.403339 -0.141415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 516 379 0.115895 -0.585973 -0.141295 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 517 516 -1.061993 -0.397787 0.428495 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 517 380 0.302075 -0.603657 0.281520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 517 379 -0.651320 -0.846411 0.282430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 518 517 -1.047812 0.208023 -0.144461 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 518 381 0.293974 -0.353416 0.125550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 518 380 -0.762632 -0.461621 0.127710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 519 518 -1.041704 -0.020680 0.001466 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 519 381 -0.744324 -0.332016 0.123110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 519 382 0.319944 -0.221728 0.119310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 520 519 -1.008933 0.060365 -0.002851 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 520 383 0.256349 -0.123380 0.108300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 520 382 -0.768106 -0.210801 0.119430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 521 520 -0.971500 0.016397 -0.017664 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 521 383 -0.764747 -0.103687 0.092890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 521 384 0.240508 -0.017833 0.092890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 522 521 -1.098799 0.040906 -0.009714 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 522 385 0.239595 0.089022 0.083160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 522 384 -0.771084 0.022325 0.088650 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 523 522 -1.162448 -0.053538 0.008378 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 523 386 0.244588 0.175945 0.075630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 524 523 -0.930256 -0.024361 -0.071942 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 524 386 -0.758656 0.210591 0.015700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 524 387 0.258092 0.207891 0.010260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 524 388 1.402325 0.190542 0.010250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 525 524 -0.962557 0.034655 -0.009645 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 525 387 -0.759879 0.224408 0.002160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 525 388 0.384176 0.197791 0.002150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 526 525 -0.983167 0.075088 -0.023506 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 526 388 -0.650337 0.225602 -0.005640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 526 389 0.412733 0.183615 -0.019580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 527 526 -1.033032 -0.029347 -0.034440 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 527 390 0.482598 0.152452 -0.033130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 527 389 -0.620849 0.215333 -0.029710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 528 527 -1.033821 -0.091178 0.016603 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 528 390 -0.519271 0.173490 -0.039930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 528 391 0.692493 0.106847 -0.013440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 529 528 -1.081009 0.002090 0.076686 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 529 391 -0.394643 0.157390 0.053550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 529 392 0.613426 0.184761 0.058430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 530 529 -0.989837 0.039461 -0.020943 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 530 393 0.651128 0.252738 0.044930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 530 392 -0.413377 0.206027 0.053990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 531 530 -0.957901 0.077285 0.006357 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 531 394 0.878028 0.304237 0.029680 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 531 393 -0.344558 0.274490 0.037510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 532 531 -1.062948 0.041492 -0.010684 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 532 395 0.835492 0.318317 0.035580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 532 394 -0.187659 0.324146 0.028110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 533 532 -1.037248 -0.010493 -0.010757 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 533 395 -0.213828 0.351829 0.035280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 533 396 0.797694 0.332808 0.035760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 534 533 -1.110071 0.010582 -0.007589 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 534 397 0.756329 0.360064 0.017210 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 534 396 -0.237411 0.363069 0.018060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 535 534 -1.112671 0.043986 -0.001811 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 535 398 0.791092 0.387101 0.016070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 535 397 -0.391150 0.393613 0.019910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 536 535 -1.017306 -0.031440 0.090207 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 536 398 -0.270973 0.425044 0.108250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 536 399 0.743902 0.500997 0.096640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 537 536 -0.958505 -0.135037 0.235381 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 538 537 -1.017968 -0.306817 0.341325 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 539 538 -1.049995 -0.086271 0.352783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 540 539 -1.013243 -0.242369 0.372366 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 541 540 -1.079998 -0.058067 0.091458 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 542 541 -1.184789 0.013594 0.018653 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 543 542 -1.078014 0.019144 0.023419 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 544 543 -1.147377 0.121538 0.029594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 545 544 -0.944298 0.106726 -0.119602 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 546 545 -1.053438 0.049742 0.108080 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 547 546 -0.998406 0.082152 -0.030319 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 548 547 -1.150639 0.138693 0.012083 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 549 548 -1.013646 -0.018303 0.023222 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 550 549 -0.961357 0.051859 -0.023949 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 551 550 -1.099792 -0.005697 0.021818 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 552 551 -1.040655 -0.033893 0.023279 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 553 552 -1.029082 -0.029582 0.031412 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 554 553 -1.046344 -0.129379 0.127823 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 555 554 -0.614677 -0.106488 0.526496 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 556 555 -0.946012 0.165745 0.391896 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 557 556 -0.634746 0.403041 -0.676169 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 558 557 -0.866918 0.223596 -0.465072 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 559 558 -1.014587 -0.026761 -0.041974 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 560 559 -1.027842 0.073209 0.006056 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 561 560 -1.108795 -0.024396 0.046855 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 562 561 -0.994884 0.046441 0.011855 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 563 562 -1.135756 0.073442 0.012550 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 564 563 -1.028975 0.047998 -0.009721 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 565 564 -0.990145 0.023205 -0.035482 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 566 565 -1.005629 0.030868 0.013515 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 567 566 -0.933304 0.093641 0.021107 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 568 567 -1.131195 0.012093 0.030208 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 569 568 -1.073759 0.118293 -0.058985 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 570 569 -1.086959 0.030054 0.012835 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 571 570 -1.047198 -0.026020 0.008837 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 572 571 -0.979361 -0.028454 -0.014935 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 573 572 -1.116875 -0.047060 0.013237 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 574 573 -1.021927 -0.064375 -0.003659 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 575 574 -1.044628 0.035367 0.022218 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 576 575 -1.024306 -0.015708 0.024690 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 577 576 -0.986395 0.070045 -0.009834 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 578 577 -0.985337 0.008148 -0.052558 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 579 578 -1.075317 -0.090250 0.175182 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 580 579 -0.440852 -0.096989 0.559967 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 581 580 -0.417612 -0.239965 0.645545 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 582 581 -1.159317 -0.032085 0.059799 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 583 582 -1.037060 0.074356 0.016604 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 584 583 -0.900068 -0.038381 -0.014387 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 585 584 -0.514772 0.399263 -0.662060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 586 585 -0.289428 0.078568 -0.559575 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 587 586 -0.726541 0.074200 -0.424236 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 588 587 -1.044850 -0.092327 0.119696 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 589 588 -1.108226 -0.099819 0.113352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 590 589 -0.927313 0.038228 0.031488 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 591 590 -0.773145 0.479188 -0.692767 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 592 591 -0.295250 0.110364 -0.594493 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 593 592 -0.430296 0.183236 -0.597883 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 594 593 -0.279461 0.189518 -0.557944 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 595 594 -0.939624 -0.150651 0.084473 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 595 593 -1.192532 -0.139640 -0.471310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 596 595 -0.365430 -0.178366 0.703568 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 596 593 -1.166411 -1.015083 0.216175 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 597 596 -0.445888 -0.032301 0.498619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 598 597 -0.156715 0.064137 0.523732 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 599 598 -0.189698 -0.100616 0.525046 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 599 591 0.151796 -2.398091 0.580970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 600 599 -0.206644 -0.069513 0.517016 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 600 591 1.091853 -2.129374 1.096980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 601 600 -0.108693 -0.054004 0.507000 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 601 590 1.384006 -2.170566 0.923597 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 601 589 0.721977 -2.966896 0.955517 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 602 601 -1.054198 -0.139686 0.437780 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 602 589 0.915997 -2.502388 1.393491 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 602 588 0.810066 -3.518735 1.521081 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 603 602 -1.070116 -0.079464 0.050734 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 604 603 -0.955742 -0.047301 0.034707 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 605 604 -1.136938 -0.076779 0.018551 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 606 605 -1.075651 0.070137 0.022586 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 607 606 -1.114041 0.035467 0.047017 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 608 607 -0.971171 0.004380 0.033578 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 609 608 -1.123927 0.121045 0.037920 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 610 609 -0.886558 0.047748 -0.078253 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 611 610 -0.978618 0.085048 0.030884 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 612 611 -1.119080 0.060639 -0.049980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 613 612 -1.028851 0.036608 0.010320 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 614 613 -1.023006 0.054135 0.041945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 615 614 -1.030467 0.063082 -0.095598 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 616 615 -1.139779 0.016688 0.036067 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 617 616 -0.941005 -0.003071 0.016184 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 618 617 -1.093426 0.029843 0.020827 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 619 618 -0.867478 0.011957 0.001008 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 620 619 -1.047334 0.028157 0.032988 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 621 620 -1.159493 -0.019636 0.042964 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 622 621 -1.082696 -0.032773 0.030376 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 623 622 -1.012771 0.233463 -0.430753 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 624 623 -1.031170 0.087073 0.027502 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 625 624 -1.085514 0.048956 0.004501 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 626 625 -1.065644 -0.005326 0.030108 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 627 626 -1.106177 -0.045605 0.021570 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 628 627 -1.139050 -0.003017 0.022694 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 629 628 -0.942308 0.106740 0.009436 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 630 629 -1.098333 -0.006940 0.006117 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 631 630 -1.061573 -0.045292 0.148455 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 632 631 -1.100021 0.009806 0.008737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 633 632 -1.057419 0.018833 0.029273 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 634 633 -0.928182 0.102473 -0.050631 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 635 634 -1.025823 0.017826 0.010766 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 636 635 -1.021169 0.068952 -0.082542 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 637 636 -1.088804 0.028632 -0.017742 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 638 637 -1.142070 0.032107 0.027860 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 639 638 -1.128369 0.050561 0.014175 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 640 639 -1.069313 -0.039401 0.039406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 641 640 -1.074392 0.030779 0.022204 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 642 641 -0.818394 0.042748 0.044340 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 643 642 -1.092129 0.149667 -0.080511 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 644 643 -0.976720 -0.010594 0.003983 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 645 644 -0.993611 0.090745 0.017965 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 646 645 -1.089794 0.093821 -0.057268 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 647 646 -0.922420 -0.047575 0.024238 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 648 647 -0.999296 -0.096180 0.027342 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 649 648 -1.083080 0.123862 -0.055491 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 650 649 -1.012348 0.025593 -0.040711 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 651 650 -1.008693 0.109526 0.012699 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 652 651 -1.014040 -0.096729 0.009833 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 653 652 -1.047507 0.065330 0.013315 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 654 653 -1.057554 0.028533 0.013295 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 655 654 -0.983563 0.067226 -0.064922 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 656 655 -1.060333 0.121866 0.004391 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 657 656 -0.971712 0.036126 0.026662 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 658 657 -1.245525 -0.017799 0.017243 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 659 658 -1.013091 0.014825 0.026307 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 660 659 -0.999356 0.026641 0.003034 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 661 660 -1.006480 0.041968 0.006003 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 662 661 -1.061368 0.021094 0.010946 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 663 662 -1.173062 0.092802 -0.042435 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 664 663 -0.994430 0.010661 0.066170 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 665 664 -1.007141 0.102237 -0.097556 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 666 665 -0.924375 0.108664 -0.002059 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 667 666 -1.247804 -0.088663 0.015348 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 668 667 -1.060852 0.043974 0.006508 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 669 668 -0.918338 -0.010208 0.000918 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 670 669 -0.908603 0.090694 0.018993 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 671 670 -1.086884 -0.000464 0.019685 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 672 671 -1.056983 -0.098001 0.127687 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 673 672 -0.449642 0.257988 -0.329859 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 674 673 -1.216783 0.081295 -0.070707 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 675 674 -0.948684 -0.140660 0.162783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 676 675 -1.097070 0.068570 0.020224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 677 676 -1.005228 0.140157 -0.051583 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 678 677 -1.195246 0.005124 0.004755 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 679 678 -0.948560 -0.050295 -0.036108 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 680 679 -1.024296 -0.033226 0.005971 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 681 680 -0.965404 -0.124398 0.166550 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 682 681 -1.118672 -0.031351 -0.010495 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 683 682 -1.117766 0.005009 0.013103 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 684 683 -0.915536 0.081890 -0.017980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 685 684 -1.161218 -0.013147 -0.058028 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 686 685 -1.025795 0.005472 0.001876 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 687 686 -1.069761 0.045299 0.013036 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 688 687 -1.087297 0.098142 0.013971 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 689 688 -1.020897 -0.000102 -0.071740 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 690 689 -1.064859 0.145233 0.018106 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 691 690 -0.982254 0.032839 -0.059876 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 692 691 -1.039091 -0.003169 -0.007280 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 693 692 -0.946555 0.029673 0.013489 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 694 693 -1.036973 -0.012234 0.038907 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 695 694 -1.085844 -0.142352 0.231359 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 696 695 -0.369408 -0.007369 0.505945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 697 696 -0.379897 -0.194104 0.564633 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 698 697 -1.079629 -0.033926 0.277276 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 699 698 -1.031041 -0.017134 0.042963 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 700 699 -1.041250 0.081642 0.036713 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 701 700 -1.012023 -0.024230 0.034809 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 702 701 -1.067968 0.000812 0.024740 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 703 702 -1.181196 0.118743 0.014351 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 704 703 -1.007144 0.058978 -0.085470 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 705 704 -1.023306 0.140369 -0.066129 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 706 705 -0.964732 0.188366 -0.267058 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 707 706 -1.044398 0.006426 -0.012522 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 708 707 -1.033047 0.198466 -0.290116 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 709 708 -0.980058 0.214920 -0.397855 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 710 709 -1.103896 -0.028814 -0.001782 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 711 710 -0.956369 0.307034 -0.413312 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 712 711 -0.966994 0.252813 -0.471016 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 713 712 -0.989693 0.076760 -0.142445 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 713 0 -0.077569 0.394100 -0.429558 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 714 713 -1.093631 -0.064012 0.367049 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 714 2 0.912080 0.108596 -0.029406 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 714 1 -0.108247 0.167577 -0.044713 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 715 714 -1.070106 0.086970 0.077640 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 715 3 0.970087 0.217873 0.061236 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 715 2 -0.159551 0.182810 0.046208 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 715 1 -1.181418 0.164545 0.030901 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 716 715 -1.059264 0.073145 0.009823 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 716 3 -0.100989 0.247375 0.066031 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 717 716 -0.998599 -0.013271 0.023971 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 717 4 -0.018441 0.318218 0.075134 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 717 5 0.989952 0.344391 0.017560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 718 717 -0.939816 0.058762 0.000353 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 718 4 -1.003275 0.340364 0.069024 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 718 6 0.993858 0.346035 0.014994 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 718 5 0.005259 0.360375 0.011450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 719 718 -0.968443 -0.026991 -0.007179 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 719 7 1.001268 0.370998 0.010528 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 719 6 -0.015903 0.365695 0.011712 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 719 5 -1.004450 0.383280 0.008168 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 720 719 -1.101257 0.006850 -0.008417 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 720 7 -0.000554 0.388315 -0.002870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 720 6 -1.017704 0.396640 -0.001686 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 720 8 0.993162 0.354987 0.014148 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 721 720 -1.055412 -0.006341 -0.010194 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 721 9 0.887069 0.291768 -0.202285 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 721 7 -1.108269 0.412621 0.003094 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 721 8 -0.114372 0.385220 0.020113 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 722 721 -1.055799 0.067734 -0.005601 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 722 9 -0.124695 0.317433 -0.203019 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 722 10 0.956631 -0.044658 -0.378264 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 722 8 -1.126068 0.411620 0.019379 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 723 722 -1.040512 0.060986 0.008858 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 723 9 -1.170741 0.317249 -0.190300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 723 11 0.850324 -0.392945 -0.393946 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 723 10 -0.084897 -0.031060 -0.365545 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 724 723 -0.785798 -0.358155 0.656250 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 724 12 0.706494 -0.257224 -0.268552 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 724 11 0.050668 -0.272080 0.254118 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 724 10 -0.913390 -0.548107 0.282519 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 725 724 -0.256238 -0.108062 0.601069 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 725 12 0.419855 0.093828 0.328411 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 725 13 1.131746 0.097501 -0.220520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 726 725 -0.951165 -0.033194 0.100826 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 726 12 -0.585607 0.111946 0.439891 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 726 13 0.121456 0.194794 -0.109040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 727 726 -1.070092 -0.097131 0.272396 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 727 13 -0.930347 0.159080 0.170920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 727 14 0.046474 0.135370 -0.020040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 727 15 1.064091 0.095525 -0.050880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 728 727 -0.981578 -0.083109 0.011979 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 728 14 -0.982137 0.125129 -0.003870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 728 16 1.087392 0.043258 -0.065280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 728 15 0.035992 0.101743 -0.034710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 729 728 -1.155192 0.008315 0.031510 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 729 16 0.010752 0.091335 -0.024570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 729 17 1.140454 0.022895 0.032390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 730 729 -1.020582 -0.008385 0.025478 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 730 16 -0.968790 0.121195 -0.002600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 730 17 0.162143 0.077590 0.054360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 731 730 -1.008284 0.154093 -0.038634 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 731 18 0.095548 0.223138 0.094350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 732 731 -1.031392 0.032592 -0.057538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 732 20 1.086270 0.253269 -0.019050 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 732 18 -0.937481 0.259470 0.033270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 732 19 0.042877 0.254960 0.015500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 733 732 -1.002068 0.081885 -0.053287 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 733 20 0.063545 0.275625 -0.074510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 733 19 -0.978150 0.335151 -0.039960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 734 733 -0.961630 -0.015516 -0.001191 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 734 20 -0.955155 0.278145 -0.069450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 734 21 0.106128 0.165683 -0.085810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 735 734 -0.986321 0.032797 0.025046 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 735 21 -0.884342 0.171651 -0.055110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 735 22 0.231315 0.070499 -0.076510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 736 735 -1.052006 0.149660 -0.093533 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 736 23 0.285223 -0.003294 -0.005110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 737 736 -1.174192 -0.012828 0.039605 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 737 23 -0.757024 0.005553 0.019830 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 737 24 0.305676 0.028952 -0.002610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 737 25 1.424019 -0.006369 -0.028240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 738 737 -0.984381 -0.016074 0.014992 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 738 24 -0.709206 0.034505 0.021190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 738 25 0.409661 0.025808 -0.004440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 738 26 1.551756 -0.023258 -0.026140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 739 738 -1.067878 0.007509 0.021411 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 739 25 -0.629707 0.042704 0.023790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 739 26 0.513318 0.025894 0.002090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 740 739 -1.059821 0.028382 0.026422 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 740 26 -0.593250 0.045310 0.021440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 740 27 0.414655 0.028796 0.002560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 741 740 -1.018249 0.011999 -0.007435 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 741 27 -0.627782 0.063044 -0.026800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 741 28 0.446288 -0.002556 -0.049920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 742 741 -1.029728 0.056680 0.001317 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 742 28 -0.564713 0.023848 -0.053130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 742 29 0.417537 -0.055593 -0.071970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 743 742 -1.102593 0.011111 0.027154 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 743 29 -0.604028 -0.026399 -0.052740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 743 30 0.398943 -0.128599 -0.012630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 744 743 -0.949152 0.120103 -0.051069 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 744 31 0.512241 -0.131939 -0.009440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 744 30 -0.564467 -0.104577 -0.052190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 745 744 -1.140023 0.069910 0.017157 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 745 31 -0.585195 -0.108869 0.021380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 745 32 0.439732 -0.148384 0.004170 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 746 745 -0.938830 0.012931 0.002034 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 746 33 0.525062 -0.115121 0.001240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 746 32 -0.517061 -0.099933 0.005110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 747 746 -0.995144 -0.029716 0.042152 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 747 33 -0.513461 -0.093063 0.033810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 747 34 0.527998 -0.116034 0.026610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 748 747 -1.211671 0.097405 0.010389 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 748 34 -0.659120 -0.060493 0.037020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 748 35 0.451722 -0.056688 0.016290 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 749 748 -1.080917 0.075628 0.013684 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 749 35 -0.620326 -0.012360 0.044420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 749 36 0.426617 -0.006292 0.030580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 750 749 -1.057989 0.009338 -0.025902 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 750 36 -0.555503 0.005748 0.000490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 750 37 0.614585 -0.015660 -0.030080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 751 750 -1.052025 0.133033 0.025643 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 751 38 0.592786 -0.008861 -0.028800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 751 37 -0.430650 0.003448 -0.008600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 752 751 -0.912187 -0.119168 0.015205 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 752 39 0.640248 -0.082625 -0.015010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 752 38 -0.404073 -0.011757 -0.006850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 753 752 -0.992027 0.101431 -0.023825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 753 40 0.681004 -0.039283 0.002230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 753 39 -0.358351 -0.023778 -0.037430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 753 38 -1.400821 0.070484 -0.029270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 754 753 -1.197060 -0.038546 0.033873 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 754 40 -0.473783 -0.039808 0.030450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 754 41 0.597518 -0.065179 0.013700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 755 754 -0.897288 0.010824 -0.039224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 755 42 0.694005 -0.029076 0.021470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 755 41 -0.400446 -0.009222 -0.016640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 756 755 -1.045543 0.032641 0.034768 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 756 42 -0.382204 -0.011535 0.050530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 756 43 0.605592 0.064093 0.155380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 757 756 -1.145735 -0.112395 0.023736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 757 43 -0.542367 0.093485 0.166610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 757 45 1.145965 0.449970 0.214520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 757 44 0.474880 0.293192 0.208390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 758 757 -1.095813 0.158517 -0.092531 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 758 45 0.173933 0.469827 0.122070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 758 44 -0.508760 0.375673 0.115940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 759 758 -1.012853 0.044769 -0.049481 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 759 45 -0.802459 0.473541 0.075790 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 759 46 0.321542 0.501184 0.009750 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 760 759 -1.009250 0.037777 -0.001404 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 760 46 -0.678620 0.521229 0.022930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 760 47 0.514393 0.478649 -0.071860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 761 760 -0.867761 -0.013752 0.009235 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 761 47 -0.456696 0.488280 -0.057700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 761 48 0.701194 0.385937 -0.089970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 762 761 -1.099220 0.068241 0.024511 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 762 48 -0.350470 0.402035 -0.066980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 762 49 0.755814 0.301260 -0.076190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 763 762 -1.207778 -0.054293 0.009556 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 763 49 -0.305822 0.326504 -0.077640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 763 50 0.707549 0.218985 -0.098470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 764 763 -1.012298 0.041168 0.009619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 764 50 -0.298707 0.245703 -0.087620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 765 764 -1.082127 0.151737 -0.056046 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 765 51 -0.335359 0.164341 -0.156020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 765 52 0.699857 0.031883 -0.104160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 766 765 -1.069672 -0.012212 0.047725 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 766 52 -0.361470 0.033258 -0.073270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 766 53 0.673072 -0.063752 -0.008520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 767 766 -0.992960 0.009024 0.019917 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 767 53 -0.343930 -0.028975 0.003740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 767 54 0.687223 -0.046798 -0.002710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 768 767 -1.029238 0.054175 -0.008064 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 768 54 -0.365008 -0.032433 -0.001730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 768 55 0.724534 -0.082667 0.002210 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 769 768 -1.093737 0.001505 0.011683 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 769 56 0.670721 -0.035023 -0.006490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 769 55 -0.372539 -0.037832 0.020080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 769 -0.996267 0.013855 -0.000512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 770 130 2.908218 1.034102 -2.337445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 56 -0.334031 -0.036304 0.007730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 129 3.282669 1.085059 -2.913255 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 57 0.674789 -0.068306 0.004810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 131 2.836175 0.964288 -1.821915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 770 128 3.275262 1.092485 -2.248865 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 770 -1.080928 0.091452 0.001120 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 771 130 1.827649 1.087087 -2.340645 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 129 2.202261 1.136845 -2.916455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 57 -0.409297 -0.008168 0.001610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 126 2.299135 2.409157 -1.598665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 131 1.755382 1.017504 -1.825115 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 128 2.194878 1.144296 -2.252065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 127 2.191679 1.274553 -1.605275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 132 1.720244 0.982024 -1.316000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 58 0.632146 -0.036485 -0.012040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 771 133 1.778676 0.905447 -0.828230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 772 771 -1.100723 0.004230 -0.074271 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 772 132 0.767815 0.877405 -1.394460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 772 58 -0.396766 -0.052686 -0.090500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 772 133 0.820065 0.796484 -0.906690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 772 134 1.227361 0.477442 -0.661890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 772 59 0.618905 -0.146197 -0.503380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 773 772 -0.591516 -0.377484 0.567859 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 773 133 -0.305775 0.805829 -0.345550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 773 135 0.882389 0.337081 -0.581660 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 773 60 0.515058 -0.180899 -0.495520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 773 134 0.208840 0.752456 -0.100750 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 773 59 0.025562 -0.099338 0.057760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 774 773 -0.306570 -0.081212 0.544887 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 774 136 0.980446 0.372623 -0.558805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 774 135 0.301088 0.677748 -0.053610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 774 60 0.244775 0.045241 0.032530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 774 59 -0.219142 -0.130939 0.585810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 775 774 -0.967470 -0.038022 0.396201 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 775 136 -0.241460 0.650004 -0.178905 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 775 137 0.785630 0.441741 -0.226715 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 775 61 0.222751 0.169812 -0.001350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 775 60 -0.803277 0.073157 0.412430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 776 775 -1.053578 0.022814 0.040461 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 776 137 -0.233518 0.440601 -0.190715 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 776 138 0.761254 0.211527 -0.230665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 776 62 0.202396 0.004291 -0.177935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 776 61 -0.786246 0.148589 0.034650 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 777 776 -1.020836 0.006844 0.178912 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 777 63 0.205453 -0.029323 -0.017590 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 777 139 0.798610 0.198459 -0.092390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 777 138 -0.280445 0.315731 -0.078550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 777 62 -0.801448 0.026205 -0.025820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 778 777 -1.027816 -0.098835 0.038695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 778 63 -0.832871 -0.024225 0.000890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 778 139 -0.244025 0.214478 -0.073910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 778 140 0.845588 0.108195 -0.037320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 778 64 0.297717 -0.038986 0.008940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 779 778 -1.111920 0.040927 -0.025140 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 779 140 -0.219843 0.086733 -0.059950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 779 65 0.291525 -0.054983 -0.014720 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 779 64 -0.770904 -0.048013 -0.013690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 780 779 -0.904379 0.066809 0.039808 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 780 141 -0.257080 0.058361 -0.049120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 780 66 0.270017 -0.037851 0.026500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 780 142 0.801124 0.043347 0.083060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 781 780 -1.054485 0.011894 0.029039 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 781 67 0.259673 -0.001211 0.015080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 781 66 -0.773345 -0.010982 0.048200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 781 143 0.726113 0.139163 0.071340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 781 142 -0.244124 0.081721 0.104760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 782 781 -1.064247 0.044747 0.037501 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 782 67 -0.818922 0.014445 0.048560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 782 143 -0.357441 0.170354 0.104820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 782 68 0.170322 0.073094 0.118010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 782 144 0.649261 0.229425 0.086940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 783 782 -1.045117 0.031955 -0.096404 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 783 68 -0.847885 0.132668 0.035060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 783 69 0.206975 0.155774 0.247805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 783 145 0.671372 0.188769 -0.027520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 783 144 -0.357640 0.248779 0.003990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 783 70 0.892334 0.479430 0.818745 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 784 783 -1.047650 -0.033807 0.045106 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 784 146 0.700214 0.247160 0.171845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 784 69 -0.849028 0.164594 0.286445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 784 145 -0.386252 0.215505 0.011120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 785 784 -1.018672 0.053886 -0.040614 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 785 146 -0.320263 0.253072 0.128085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 785 147 0.554101 0.403783 -0.097310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 786 785 -1.093158 -0.031665 0.060038 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 786 148 0.461033 0.198314 -0.191840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 786 147 -0.547570 0.407335 -0.054850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 787 786 -1.134798 -0.127543 0.040746 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 787 149 0.479479 -0.024298 -0.379620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 787 148 -0.607715 0.207934 -0.152850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 788 787 -0.865800 -0.408457 0.607824 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 788 150 0.504369 -0.332701 -0.173180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 788 149 -0.484921 -0.179359 0.218440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 789 788 -0.168775 -0.131489 0.562200 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 789 150 0.385547 -0.076721 0.378470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 789 149 -0.537359 -0.464606 0.770090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 790 789 -0.150482 -0.074661 0.593955 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 790 150 0.125621 0.083339 0.978670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 791 790 -0.732386 0.200122 0.011357 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 791 789 -0.960091 0.100520 0.621500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 791 150 -0.601968 0.262632 0.999970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 792 791 -0.299693 0.010134 -0.550645 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 792 789 -1.003488 0.685862 0.065810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 792 150 -0.613730 0.634662 0.444280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 792 790 -0.764283 0.626923 -0.534390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 793 792 -0.200129 0.161559 -0.812103 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 793 789 -0.508855 1.362843 -0.756170 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 793 151 0.717250 0.694806 -0.043380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 793 150 -0.281023 1.042492 -0.377700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 793 788 -0.707310 1.470694 -0.204520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 794 793 -0.307421 0.138406 -0.592779 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 794 151 0.757638 0.243546 -0.652835 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 794 150 0.138116 1.100067 -0.987155 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 794 788 0.033690 1.695192 -0.813975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 794 149 -0.429661 1.924590 -0.595535 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 795 794 -0.253179 0.067705 -0.658968 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 795 787 0.305648 2.272831 -0.880075 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 795 149 0.592392 1.887775 -1.259695 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 796 795 -0.159809 -0.022242 -0.703475 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 796 787 1.492610 1.638707 -1.577995 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 796 149 1.464861 1.159416 -1.957615 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 796 148 1.704914 2.244910 -1.730845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 796 786 1.492046 2.699407 -1.539005 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 797 796 -0.759383 0.005660 -0.436120 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 797 148 1.734402 1.334079 -2.175835 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 797 785 2.155231 2.836617 -1.941535 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 797 147 2.330828 2.173869 -2.038845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 797 786 1.737902 1.835944 -1.983995 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 798 797 -0.774003 0.026788 0.098014 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 798 147 1.349531 2.491642 -1.939125 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 798 786 0.793193 2.096366 -1.884275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 799 798 -0.179469 0.112765 -0.596369 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 799 785 2.428006 2.048903 -2.443201 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 799 146 2.836010 2.061006 -2.315116 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 799 797 -0.825755 0.598229 -0.501666 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 799 147 2.263260 1.383375 -2.540511 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 799 786 1.580889 1.372218 -2.485661 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 800 799 -0.197110 0.140666 -0.681051 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 800 785 2.969334 0.172414 -3.132264 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 800 784 4.000252 0.145249 3.107161 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 800 146 3.291944 -0.077659 -3.004179 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 800 147 2.419023 -0.236521 3.053611 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 800 -1.032702 -0.071898 -0.080274 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 801 784 2.957638 -0.242783 3.028280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 146 2.233967 -0.409184 -3.083060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 68 4.836254 -0.557976 3.101980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 69 3.782610 -0.502322 -2.968460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 783 4.000630 -0.362422 3.066920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 145 3.317045 -0.500579 3.039400 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 801 144 4.338713 -0.637189 3.070910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 801 -1.045704 -0.003147 0.121992 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 802 67 4.819314 0.010943 -3.126384 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 782 4.001329 0.052688 3.108241 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 143 4.352891 -0.129491 -3.070124 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 68 3.828664 -0.014686 -3.056934 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 783 2.975245 0.075783 -3.091994 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 802 144 3.344778 -0.154959 -3.088004 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 803 802 -1.027781 -0.074968 -0.015103 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 803 67 3.786992 -0.060043 3.130851 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 803 781 4.046546 -0.067958 3.115771 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 803 66 4.819917 -0.076946 -3.119215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 803 143 3.317082 -0.188327 -3.096075 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 803 142 4.288479 -0.155954 -3.062655 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 804 803 -0.886290 -0.313692 0.599275 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 805 804 -0.302204 -0.143218 0.605285 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 806 805 -0.396319 -0.081435 0.593424 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 806 70 -0.292460 -0.063288 -0.490080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 806 71 0.225464 -0.179354 0.037040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 807 806 -0.516825 0.229145 -0.284950 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 807 72 0.563247 -0.191639 -0.022900 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 807 805 -0.954144 0.235660 0.287200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 807 71 -0.439135 -0.012050 -0.271860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 808 807 -1.122923 0.014457 -0.111099 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 808 72 -0.543951 -0.201304 -0.134250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 808 73 0.469282 -0.256962 -0.025480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 809 808 -1.027147 0.037792 0.002177 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 809 74 0.493972 -0.312192 -0.028210 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 809 73 -0.533679 -0.264657 -0.016310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 810 809 -0.973947 -0.001924 0.049853 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 810 74 -0.491638 -0.283766 0.029090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 811 810 -1.126231 0.000407 0.007789 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 811 76 0.439751 -0.603736 -0.201640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 811 75 -0.510604 -0.364930 -0.244780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 812 811 -0.948262 0.029912 -0.017585 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 812 76 -0.545075 -0.589034 -0.217060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 812 77 0.506420 -0.814695 -0.129730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 813 812 -1.091426 0.012653 -0.000505 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 813 77 -0.584118 -0.784692 -0.121470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 813 78 0.468200 -0.899986 0.092990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 814 813 -1.073881 0.012305 0.199591 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 814 78 -0.378149 -0.848125 0.297600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 814 79 0.551683 -0.430923 0.303180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 815 814 -1.077956 0.113293 -0.134604 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 815 79 -0.522825 -0.405324 0.186940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 815 80 0.492127 -0.339962 0.049420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 816 815 -1.044675 0.034397 -0.019077 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 816 80 -0.544073 -0.309475 0.034220 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 816 81 0.627681 -0.297410 0.037740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 817 816 -0.980356 0.045918 -0.007102 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 817 81 -0.395978 -0.240456 0.021070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 817 82 0.639523 -0.244051 0.023930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 818 817 -0.944215 -0.066968 -0.033464 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 818 83 0.597394 -0.186480 0.334040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 818 82 -0.406678 -0.241979 -0.008340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 819 818 -0.576184 0.336968 -0.677920 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 819 84 0.702010 -0.277472 0.240637 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 819 83 -0.185553 -0.162905 -0.336497 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 820 819 -0.234787 0.152913 -0.697318 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 820 84 0.129759 -0.552367 -0.458511 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 820 85 1.056247 -0.762855 -0.074060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 821 820 -0.986229 0.001622 -0.006827 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 821 86 1.080110 -0.628882 0.396513 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 821 85 -0.000133 -0.755532 -0.078842 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 822 821 -1.067159 0.047465 -0.138543 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 822 87 1.060930 -0.348850 0.282275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 822 86 -0.035316 -0.637562 0.260057 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 823 822 -0.949476 0.093273 -0.030679 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 823 87 0.031885 -0.338183 0.242498 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 824 823 -0.963249 -0.005308 0.003400 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 824 87 -0.974942 -0.316764 0.249985 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 824 88 0.259709 -0.057250 0.094979 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 825 824 -1.062555 0.050922 0.068587 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 825 89 0.209722 0.022409 0.114776 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 825 88 -0.756889 -0.052264 0.170915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 826 825 -1.093251 0.004807 -0.014396 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 826 89 -0.878156 0.032830 0.100586 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 826 90 0.163293 0.126607 0.099625 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 827 826 -1.024703 0.077492 -0.117420 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 827 90 -0.893770 0.171902 -0.018094 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 827 91 0.310510 0.083964 -0.033665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 828 827 -1.019479 0.094512 -0.032713 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 828 91 -0.693026 0.136040 -0.070416 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 828 92 0.320238 0.090851 -0.051626 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 829 828 -1.146505 -0.075314 -0.037869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 829 92 -0.771061 0.117356 -0.091855 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 829 93 0.292193 -0.003490 -0.128617 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 830 829 -1.067969 -0.032922 0.143870 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 830 94 0.271076 -0.006628 0.042773 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 830 93 -0.759869 -0.034702 0.024494 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 831 830 -1.008140 0.066335 -0.028064 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 831 95 0.344427 -0.012431 -0.024384 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 831 94 -0.725437 0.036555 0.014932 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 832 831 -1.061837 0.012715 -0.003086 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 832 96 0.278026 -0.063368 -0.013227 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 832 97 1.233543 -0.086034 0.003845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 832 95 -0.741395 0.015599 -0.040634 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 833 832 -1.036782 0.033793 -0.025656 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 833 96 -0.759347 -0.022849 -0.033257 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 833 98 1.201600 -0.105947 -0.060813 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 833 97 0.195524 -0.064647 -0.016184 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 834 833 -1.058682 -0.114005 0.080088 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 834 98 0.142967 -0.080632 0.014800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 834 97 -0.863353 -0.115451 0.059429 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 834 99 1.173752 -0.097534 0.046935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 835 834 -1.073909 -0.021984 0.007511 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 835 98 -0.865229 -0.063250 0.008685 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 835 100 1.229228 -0.046224 0.052081 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 835 99 0.165433 -0.086455 0.040820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 836 835 -0.964703 -0.006878 -0.025322 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 836 101 1.164839 -0.027547 0.027359 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 836 100 0.173819 -0.042464 0.019843 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 836 99 -0.890720 -0.048386 0.008582 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 837 836 -1.004402 -0.071038 -0.007082 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 837 101 0.185088 -0.027100 0.006234 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 837 102 1.229118 -0.063578 -0.050713 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 837 100 -0.806026 -0.021080 -0.001283 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 838 837 -0.984530 -0.081670 0.017703 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 838 101 -0.828816 -0.025460 0.022450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 838 102 0.215668 -0.045004 -0.034497 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 839 838 -0.974321 0.073925 -0.015076 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 839 103 0.264150 -0.042835 -0.038232 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 840 839 -1.023158 0.383809 -0.442695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 840 104 0.050856 -0.091849 0.037853 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 840 105 0.400427 -0.018511 0.636584 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 841 840 -0.275068 0.084603 -0.648264 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 841 104 -0.212070 -0.015245 -0.614301 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 841 106 1.105984 0.156823 0.407620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 841 105 0.110270 -0.169111 -0.015570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 842 841 -0.198803 0.079394 -0.690543 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 842 106 0.778325 -0.457355 -0.275520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 842 105 -0.199685 -0.081622 -0.698710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 843 842 -1.043335 -0.146477 0.231617 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 843 106 -0.218991 -0.359614 -0.036150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 843 107 0.902026 -0.426808 -0.037340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 844 843 -1.016767 -0.030644 0.008867 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 844 107 -0.090064 -0.385760 -0.028770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 845 844 -1.163716 0.070370 0.073554 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 845 108 -0.191690 -0.332828 0.088490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 846 845 -0.960749 0.016475 0.027567 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 846 110 0.882346 -0.186956 0.111490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 846 109 -0.135215 -0.249262 0.104180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 847 846 -1.101439 0.107060 -0.004292 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 847 110 -0.237057 -0.152932 0.110460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 847 111 0.790380 -0.071064 0.107770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 848 847 -1.135435 0.117748 -0.000976 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 848 112 0.760997 0.028128 0.079870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 848 111 -0.267765 -0.044674 0.102950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 849 848 -0.929058 0.035944 0.013870 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 849 112 -0.231576 0.063685 0.088750 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 849 113 0.827635 0.133120 0.084530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 850 849 -1.081473 0.042395 -0.105832 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 850 114 0.866462 0.119599 -0.010580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 850 113 -0.174920 0.166334 -0.010470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 851 850 -1.109852 -0.044413 0.008928 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 851 114 -0.265323 0.149597 0.014090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 851 115 0.873558 0.144053 0.014310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 852 851 -1.079092 0.104308 -0.006785 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 852 116 0.839326 0.133249 -0.015140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 852 115 -0.172666 0.161794 -0.007120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 853 852 -1.031515 0.042037 0.023597 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 853 116 -0.274185 0.171458 -0.016520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 854 853 -0.496880 0.415885 -0.576149 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 854 117 0.179651 0.169706 0.097470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 854 116 -0.713759 0.636065 -0.590520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 854 118 0.561411 0.289965 0.627010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 855 854 -0.308300 0.055837 -0.654887 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 855 117 -0.058856 0.126449 -0.559960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 855 118 0.316820 -0.011646 -0.030420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 856 855 -1.037228 0.072876 -0.300838 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 856 119 0.309476 -0.428675 -0.172050 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 856 118 -0.689723 -0.064497 -0.312930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 857 856 -0.959042 0.051891 0.016511 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 857 119 -0.721633 -0.401368 -0.153010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 857 120 0.281182 -0.276145 0.267625 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 858 857 -1.139554 0.050588 -0.000253 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 858 121 0.233097 -0.065867 0.181005 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 858 120 -0.815178 -0.239164 0.256305 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 859 858 -0.954485 0.031243 0.017435 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 859 122 0.354576 0.138499 0.164015 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 859 121 -0.778375 -0.048960 0.203255 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 860 859 -0.997941 -0.036420 0.035352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 860 122 -0.680592 0.151163 0.195825 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 860 123 0.394805 0.325330 0.162435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 861 860 -1.020967 0.017560 -0.055234 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 861 123 -0.611042 0.350609 0.098685 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 861 124 0.423886 0.398645 0.076310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 862 861 -0.964105 -0.028563 0.025117 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 862 125 0.470207 0.524263 0.067590 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 862 124 -0.621632 0.458233 0.103840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 863 862 -1.042927 0.235964 -0.184597 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 863 126 0.677588 0.428637 -0.137805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 863 125 -0.420833 0.581715 -0.107885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 863 58 2.925572 -1.496612 1.448820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 863 -1.109858 -0.037146 -0.000186 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 864 772 1.842580 -1.040092 1.548780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 130 0.888531 -0.135746 -0.870325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 771 1.786809 -2.063220 1.470320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 57 1.753880 -2.471271 1.471930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 126 -0.379578 0.465975 -0.128345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 131 0.950514 -0.214628 -0.354795 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 128 0.868446 0.235370 -0.781745 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 127 0.738526 0.245253 -0.134955 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 132 0.982291 -0.253148 0.154320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 58 1.886519 -1.437921 1.458280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 864 133 1.064342 -0.202692 0.642090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 864 -0.767054 -0.317974 0.517876 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 865 130 0.012649 -0.034601 -0.368040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 56 2.257573 -2.607200 1.977135 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 770 2.094466 -2.313447 1.969405 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 57 1.895506 -1.665048 1.974215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 131 0.104953 -0.073899 0.147490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 128 -0.183621 0.281007 -0.279460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 865 127 -0.302252 0.227122 0.367330 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 865 -0.199358 0.046569 0.595393 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 866 130 -0.222812 -0.097631 0.218270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 56 3.070578 -0.998479 2.563445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 769 3.615710 -1.390812 2.569935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 770 2.772181 -0.844032 2.555715 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 131 -0.124181 -0.079296 0.733800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 866 128 -0.560926 0.056672 0.306850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 867 866 -0.293432 -0.099498 0.602021 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 867 56 2.853920 0.807253 -3.120350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 867 768 4.624250 0.789438 -3.095990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 867 55 3.896701 0.838990 -3.093780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 867 769 3.525355 0.790842 -3.113860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 868 867 -1.007727 0.095238 -0.105533 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 868 54 4.088886 0.488782 3.088485 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 868 768 3.722695 0.475137 3.090215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 868 55 3.003363 0.594903 3.092425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 868 769 2.629099 0.582938 3.072345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 869 868 -1.037871 -0.044254 0.096346 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 869 767 3.650843 0.845616 -3.089380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 869 53 3.992792 0.892501 -3.085640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 869 54 2.962114 0.856485 -3.092090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 869 768 2.599247 0.805402 -3.090360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 870 869 -0.921973 0.077098 -0.192773 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 870 767 2.778654 0.222848 3.000445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 870 765 4.836351 -0.062770 3.043595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 870 766 3.782535 0.053417 3.012705 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 870 52 4.136732 -0.026025 2.939435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 870 53 3.123240 0.203151 3.004185 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 871 870 -1.001839 -0.096521 0.060952 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 871 51 4.125060 0.039377 2.949285 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 871 765 3.795884 0.215776 3.105305 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 871 52 3.095330 0.209305 3.001145 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 872 871 -1.090327 0.032702 0.024921 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 872 51 3.056604 0.117266 2.964655 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 872 50 4.046819 -0.081485 2.969695 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 872 764 3.769855 0.188490 3.057315 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 873 872 -1.013145 0.039259 0.012409 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 873 763 3.737365 0.185748 3.096725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 873 49 4.028235 -0.154144 3.019085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 873 50 3.020706 -0.001282 2.998255 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 873 764 2.736145 0.260675 3.085875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 874 873 -1.101163 -0.058194 0.031415 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 874 762 3.706300 0.189444 3.133415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 874 48 4.053471 -0.215444 3.066435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 874 763 2.644429 0.222928 3.134865 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 874 49 2.948048 -0.105625 3.057225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 875 874 -1.101485 0.057626 -0.002324 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 875 47 4.120602 -0.183253 3.114015 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 875 762 2.606826 0.283696 -3.134460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 875 761 3.649407 0.291051 -3.111470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 875 48 2.960155 -0.115829 3.081745 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 876 875 -1.131085 -0.006254 0.018457 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 876 760 3.478202 0.403926 -3.083840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 876 46 4.185776 -0.077264 -3.060910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 876 47 2.992294 -0.103617 3.127485 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 876 761 2.514753 0.364297 -3.098000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 877 876 -1.219295 0.095207 -0.007584 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 877 759 3.245370 0.457347 -3.082280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 877 760 2.252167 0.427363 -3.095460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 877 46 2.954102 -0.062016 -3.072530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 878 877 -0.797705 0.056227 0.040465 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 878 45 3.281621 0.243033 -2.955150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 878 759 2.431777 0.625064 -3.030940 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 878 44 3.956843 0.380909 -2.961280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 878 758 3.424970 0.723076 -3.077220 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 878 46 2.167545 0.091438 -3.021190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 879 878 -1.034067 -0.059559 0.029830 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 879 43 3.984472 0.664228 -2.983450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 879 45 2.293182 0.322051 -2.935540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 879 44 2.965571 0.473141 -2.941670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 879 757 3.442916 0.762302 3.133125 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 879 758 2.427091 0.804812 -3.057610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 880 879 -0.950783 -0.034695 -0.034984 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 880 42 4.006529 0.592318 -3.123610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 880 43 3.016795 0.548875 -3.018760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 880 756 3.624152 0.593227 3.109045 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 880 757 2.479039 0.666006 3.097815 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 881 880 -0.963024 -0.062263 0.010810 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 881 42 2.964880 0.664255 -3.106160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 881 41 4.059501 0.659684 3.138915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 881 755 3.659223 0.644872 -3.127630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 881 756 2.582546 0.658492 3.126495 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 882 881 -1.098371 -0.033506 -0.036900 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 882 40 4.037444 0.511786 3.131735 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 882 754 3.562442 0.491102 3.101285 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 882 41 2.968036 0.580305 3.114985 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 882 755 2.567518 0.575075 3.131625 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 883 882 -0.977822 -0.029602 0.019601 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 883 753 3.676679 0.559176 -3.132620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 883 40 2.995349 0.592347 -3.130390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 883 754 2.520888 0.561665 3.122345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 883 752 4.673881 0.494941 3.128145 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 883 39 4.034802 0.586168 3.113135 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 884 883 -1.035512 -0.088204 0.028594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 884 752 3.597506 0.632043 -3.127390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 884 39 2.956150 0.705567 3.140785 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 884 751 4.594090 0.662109 -3.105440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 884 38 4.001372 0.649538 -3.134240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 884 37 5.024584 0.674229 -3.114040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 885 884 -1.104625 0.057601 0.022486 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 885 750 4.590289 0.793350 -3.071670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 885 36 5.144836 0.826427 -3.071180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 885 751 3.545800 0.748623 -3.093150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 885 37 3.976112 0.766033 -3.101750 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 886 885 -1.038528 -0.032952 -0.054807 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 886 749 4.550879 0.654512 -3.140420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 886 36 4.124255 0.660304 -3.109840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 887 886 -1.016657 0.072654 -0.104507 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 887 749 3.562722 0.274864 3.045975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 887 35 4.181395 0.227944 3.090395 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 887 36 3.138655 0.321857 3.076555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 888 887 -1.053324 -0.002480 0.038375 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 888 748 3.619434 0.277179 3.096425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 888 34 4.280612 0.307850 3.133445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 888 35 3.170732 0.354206 3.112715 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 889 888 -1.085450 -0.054383 0.053566 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 889 33 4.289833 0.373683 -3.109900 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 889 748 2.588599 0.346628 3.129065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 889 34 3.248425 0.398859 -3.117100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 889 747 3.776177 0.281707 3.139475 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 890 889 -1.066127 0.046331 0.013525 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 890 33 3.137617 0.453664 -3.096450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 890 32 4.179402 0.484227 -3.092580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 890 746 3.667225 0.361698 -3.097690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 890 747 2.625244 0.354788 -3.130260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 891 890 -0.979525 -0.007916 0.032832 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 891 33 2.156175 0.508187 -3.082820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 891 31 4.222931 0.573396 -3.061740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 891 745 3.645099 0.430514 -3.083120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 891 32 3.197447 0.552947 -3.078950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 891 746 2.686987 0.423448 -3.084060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 892 891 -1.207458 -0.093712 0.020359 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 892 31 3.077449 0.691984 -3.038200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 892 744 3.601289 0.618559 -3.028760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 892 745 2.503140 0.535542 -3.059580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 892 32 2.052730 0.647403 -3.055410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 893 892 -0.943761 0.131496 -0.099977 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 893 29 4.192605 0.377600 3.067235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 893 743 3.588148 0.364263 3.119975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 893 744 2.629578 0.386768 -3.123650 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 893 30 3.192078 0.501456 3.107345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 894 893 -1.103388 -0.017526 0.007710 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 894 28 4.069125 0.399397 3.098275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 894 742 3.504205 0.417702 -3.131780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 894 29 3.086143 0.469196 3.079435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 894 743 2.481894 0.448486 3.132175 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 895 894 -1.017096 -0.035446 0.008416 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 895 741 3.554958 0.455053 -3.123870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 895 27 4.183759 0.403144 3.132515 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 895 28 3.108695 0.449700 3.109395 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 895 742 2.543607 0.461722 -3.120660 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 896 895 -1.023980 0.038778 0.003180 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 896 740 3.553596 0.541886 -3.139550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 896 26 4.146937 0.497788 -3.118110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 896 741 2.509548 0.555546 -3.110190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 896 27 3.139001 0.512243 -3.136990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 896 739 4.659584 0.534656 -3.120200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 896 -1.076066 0.139506 0.043616 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 897 740 2.494197 0.701476 -3.098700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 24 5.345777 0.828395 -3.029930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 738 4.636355 0.798683 -3.051120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 25 4.230701 0.735968 -3.055560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 26 3.088844 0.681646 -3.077260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 897 739 3.599557 0.739420 -3.079350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 898 897 -1.303732 0.208295 -0.198538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 898 23 5.232868 -0.076613 3.083785 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 898 736 5.517521 -0.094926 3.088895 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 898 737 4.478555 -0.012363 3.063955 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 898 24 4.171553 -0.017519 3.061345 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 898 738 3.469488 0.088625 3.040155 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 899 898 -0.917528 0.029600 0.038448 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 899 735 5.596332 -0.010184 3.050595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 899 22 5.359567 -0.059371 2.974085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 899 23 4.259130 0.093111 3.129415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 899 736 4.544322 0.087801 3.134525 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 900 899 -0.969307 -0.178164 0.226817 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 900 735 4.460748 1.099902 -3.002040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 900 22 4.241488 0.997912 -3.078550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 901 900 -0.859581 0.090879 0.027658 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 901 899 -1.858170 -0.095436 0.267300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 901 735 3.542113 1.372892 -2.965290 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 901 22 3.326748 1.262915 -3.041800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 902 901 -0.428099 0.145551 -0.523672 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 902 900 -1.099077 0.674165 -0.485700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 902 899 -2.053341 0.986777 -0.255150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 902 735 3.359242 -0.435538 2.795445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 902 22 3.117728 -0.423376 2.718935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 903 902 -0.342235 0.104551 -0.528348 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 903 22 2.212888 -1.802102 2.206915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 904 903 -0.271057 0.123546 -0.544584 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 905 904 -0.328191 0.134151 -0.515424 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 906 905 -0.597272 0.177678 -0.517087 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 907 906 -0.271811 0.165382 -0.554345 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 907 737 -1.383589 0.556600 0.199030 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 907 898 2.928028 1.768030 -2.864925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 907 24 -1.089671 0.645418 0.196420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 907 738 -0.385311 0.735081 0.175230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 907 739 0.642748 0.881902 0.147000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 907 -0.575597 0.266293 -0.560476 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 908 737 -1.380336 1.406438 -0.346430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 898 2.934121 0.205160 2.872800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 24 -1.082989 1.329880 -0.349040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 738 -0.434321 1.041101 -0.370230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 25 -0.043079 0.916933 -0.374670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 897 4.073725 -0.304706 2.680890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 26 1.003878 0.457947 -0.396370 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 908 739 0.520727 0.633248 -0.398460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 909 908 -0.361619 0.227712 -0.537607 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 910 909 -0.320312 0.187711 -0.525605 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 911 910 -0.364485 0.135558 -0.544478 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 912 911 -0.449256 0.138898 -0.595626 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 912 901 0.366487 0.112992 0.379113 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 912 900 -0.487228 -0.108883 0.415863 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 912 899 -1.324419 -0.663370 0.646413 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 912 -0.271381 0.099958 -0.531785 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 913 901 0.117444 -0.015740 -0.148990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 900 -0.731767 0.222796 -0.112240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 899 -1.734306 0.165707 0.118310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 735 3.824107 0.816153 -3.114280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 902 0.537514 -0.003913 0.373460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 913 22 3.594803 0.739363 3.092395 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 913 -0.506512 0.067312 -0.456623 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 914 901 -0.366024 0.012069 -0.617360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 903 0.324490 -0.132830 0.417110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 735 3.316994 -0.918936 2.700535 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 902 0.014145 -0.167010 -0.094910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 22 3.077719 -0.883941 2.624025 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 914 904 0.634848 -0.011355 0.961330 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 915 914 -0.976196 -0.035309 -0.086553 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 915 734 3.035109 -1.776720 2.629105 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 915 21 2.861374 -1.869077 2.543295 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 915 902 -1.015939 -0.180304 -0.197040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 916 915 -0.915521 -0.148347 0.471879 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 916 20 3.494927 -0.697396 3.002565 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 916 733 3.576110 -0.426441 3.077075 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 916 734 2.561420 -0.353521 3.072015 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 916 21 2.444030 -0.511425 2.986205 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 917 916 -1.044558 -0.129480 0.377070 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 917 20 2.432188 0.602344 -2.880180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 917 732 3.405910 1.146397 -2.861130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 917 19 3.435282 0.889530 -2.845630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 917 733 2.401325 0.883511 -2.805670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 918 917 -1.058490 0.024280 -0.097110 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 918 732 2.461567 0.869215 -2.948620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 918 18 3.431408 0.794349 -2.915350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 918 731 3.496781 1.028115 -3.009700 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 918 19 2.468382 0.610765 -2.933120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 919 918 -1.124722 0.132755 -0.187409 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 919 730 3.645664 0.319633 3.037765 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 919 17 3.476352 0.259266 3.092125 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 919 18 2.474065 0.254267 -3.104760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 919 731 2.582282 0.471544 3.084075 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 920 919 -1.060844 0.013905 0.039441 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 920 16 3.483376 0.305871 3.078225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 920 729 3.497662 0.396720 3.102795 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 920 730 2.523734 0.485677 3.080825 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 920 17 2.357178 0.418078 3.135185 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 921 920 -0.863328 -0.026416 0.023306 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 921 728 3.665305 0.451103 -3.126670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 921 16 2.578680 0.391623 3.091235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 921 15 3.630835 0.348834 3.121805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 921 729 2.591783 0.482650 3.115805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 922 921 -0.986195 0.132028 -0.130204 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 922 728 2.651663 0.091598 3.025155 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 922 727 3.672403 -0.016745 3.041325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 922 14 3.612612 -0.146783 3.021285 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 922 15 2.604094 -0.005275 2.990445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 923 922 -1.078037 -0.025133 0.012895 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 923 726 3.602986 0.111175 -2.927340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 923 13 3.525724 -0.104988 -3.036380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 923 727 2.607829 0.114835 3.075885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 923 14 2.552568 -0.017192 3.055845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 924 923 -1.070436 0.060584 -0.210488 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 924 726 2.545774 -0.472999 -3.125980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 924 725 3.537633 -0.429500 -3.014500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 924 13 2.427373 -0.669665 3.048165 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 924 -1.097771 -0.034012 0.188328 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 925 722 2.920662 2.452413 -1.554614 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 723 2.922844 1.410396 -1.567333 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 9 3.236036 2.582229 -1.757633 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 10 2.891489 1.495185 -1.932878 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 8 3.314007 3.584994 -1.535236 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 925 721 2.930600 3.464685 -1.555348 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 926 925 -1.049753 -0.029246 0.022420 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 926 722 1.820594 2.575789 -1.513254 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 926 721 1.788668 3.587606 -1.513988 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 927 926 -0.908356 0.407574 -0.447202 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 928 927 -0.236243 0.171183 -0.593400 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 928 7 4.401174 1.599879 -2.543734 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 928 6 5.244952 2.167949 -2.542550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 928 719 5.029067 2.463550 -2.554262 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 928 720 4.181224 1.919896 -2.540864 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 928 -0.479145 0.119355 -0.482695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 929 7 4.165793 -0.571306 -3.037414 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 6 5.178010 -0.470907 -3.036230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 719 5.127979 -0.108301 -3.047942 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 720 4.123753 -0.185273 -3.034544 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 5 6.163870 -0.395972 -3.039774 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 929 718 6.136585 -0.036593 -3.051224 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 930 929 -0.995479 0.075379 0.003278 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 930 4 6.134909 -0.212554 -2.978380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 930 5 5.132727 -0.327329 -3.035954 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 930 717 6.088548 0.102808 -3.053514 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 930 718 5.104070 0.031944 -3.047404 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 931 930 -0.973267 0.009632 0.002296 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 931 4 5.136518 -0.101340 -2.958870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 931 716 6.098074 0.311268 -3.032946 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 931 717 5.084013 0.213057 -3.034004 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 932 931 -0.944847 0.061699 0.009881 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 932 3 5.249448 0.216805 -2.949044 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 932 716 5.118052 0.449460 -3.015076 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 933 932 -0.988755 -0.041910 0.014056 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 933 3 4.168388 0.299815 -2.938305 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 933 715 5.097859 0.652833 -2.999540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 933 716 4.034502 0.531045 -3.004336 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 933 2 5.281684 0.494452 -2.953332 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 934 933 -1.054058 0.128803 -0.036824 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 934 715 4.024011 0.595424 -3.024240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 934 714 5.078195 0.714043 -2.948626 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 934 2 4.203869 0.432552 -2.978032 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 934 1 5.216569 0.570335 -2.993339 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 935 934 -1.051914 0.128102 -0.134374 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 935 713 5.101192 0.302096 -2.696110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 935 714 4.065029 0.129716 -3.078392 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 935 1 4.183644 -0.030690 -3.123104 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 936 935 -0.889018 -0.056295 0.028725 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 936 0 4.416303 0.128670 -3.097553 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 936 713 4.167527 0.444014 -2.667995 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 937 936 -1.012208 0.107610 -0.042041 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 937 0 3.412329 0.058734 -3.121343 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 937 712 4.081237 0.684036 -2.813080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 938 937 -0.996947 -0.028271 -0.022502 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 938 712 3.074781 0.571548 -2.849805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 938 711 4.103345 0.664519 2.970742 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 939 938 -1.215814 -0.039616 0.014831 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 939 711 2.941151 0.723553 2.989262 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 939 710 3.800443 0.256492 2.574020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 940 939 -1.044322 0.024000 0.015653 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 940 708 4.343377 -0.922323 2.179657 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 940 709 3.610046 -0.202750 2.582961 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 940 710 2.719097 0.369924 2.582785 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 941 940 -1.076666 0.058867 -0.063805 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 941 706 3.811566 -3.086834 1.814400 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 941 707 3.565923 -2.106769 1.827900 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 941 708 3.265535 -1.113534 2.120342 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 941 709 2.576151 -0.351755 2.523646 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 942 941 -1.023788 0.024927 -0.003686 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 942 706 2.829435 -3.009250 1.822620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 942 707 2.575743 -2.031237 1.836120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 942 708 2.267201 -1.040505 2.128562 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 943 942 -1.076425 -0.053633 0.182586 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 943 705 2.478652 -3.598157 1.717225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 944 943 -0.372098 -0.132759 0.496737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 944 704 4.109212 -2.965197 2.158875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 944 705 3.557657 -2.081968 2.231085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 945 944 -1.115810 -0.102677 0.108929 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 945 704 3.372529 -2.485829 2.281205 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 946 945 -0.905815 -0.253365 0.568742 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 946 703 4.256689 -0.887245 2.773695 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 946 704 3.314089 -0.568645 2.867045 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 947 946 -1.018597 0.002916 0.158668 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 947 703 3.316743 -0.146475 2.937585 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 947 702 4.413109 -0.398079 2.961215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 948 947 -1.001195 0.025822 0.046736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 948 701 4.460212 -0.367698 3.032645 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 948 702 3.417254 -0.218046 3.000455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 949 948 -1.066295 0.051339 0.121531 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 949 701 3.429865 0.210646 -3.122180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 949 700 4.441713 0.255339 -3.076800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 950 949 -1.089978 0.015487 0.022289 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 950 699 4.399070 0.482512 -3.024040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 950 700 3.392227 0.421645 -3.044690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 951 950 -0.952424 -0.041974 -0.041977 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 951 698 4.417408 0.407049 -3.027920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 951 697 5.484429 0.533884 -2.751050 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 951 699 3.372964 0.311824 -3.076060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 952 951 -0.912280 0.022535 0.029565 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 952 698 3.397319 0.535864 -3.000890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 952 697 4.460522 0.691490 -2.724020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 953 952 -1.009589 0.013256 -0.029896 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 953 696 3.769427 0.844546 -2.223071 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 954 953 -0.994973 -0.026252 -0.001373 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 954 694 2.873169 2.311732 -1.465873 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 955 954 -1.016616 0.055633 -0.132400 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 955 693 2.128440 3.048679 -1.560724 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 956 955 -1.060799 -0.040662 0.046638 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 957 956 -0.933088 -0.047329 0.020960 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 958 957 -1.044487 0.009161 0.040250 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 959 958 -1.021250 -0.041984 0.043779 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 960 959 -1.006155 -0.037696 0.097406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 961 960 -1.038098 0.086042 -0.125456 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 962 961 -1.056280 0.070018 0.007923 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 963 962 -1.033305 -0.021609 0.040566 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 964 963 -1.079307 -0.042324 0.040172 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 965 964 -1.049713 -0.390844 0.368339 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 966 965 -1.014466 -0.041799 0.234829 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 967 966 -1.075583 0.007941 0.026676 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 968 967 -1.089801 -0.095040 0.014869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 969 968 -1.115472 -0.161877 0.113568 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 970 969 -0.985698 0.066415 0.102369 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 971 970 -1.021809 -0.372278 0.486589 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 972 971 -1.138841 -0.105538 0.325444 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 973 972 -1.052434 0.012940 0.022236 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 974 973 -1.198455 0.054078 0.012147 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 975 974 -1.034375 0.262329 -0.313288 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 976 975 -1.111810 0.056157 0.002678 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 977 976 -1.128067 0.080584 0.028789 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 978 977 -1.166005 -0.033553 0.039435 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 979 978 -1.013778 -0.024702 0.038162 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 980 979 -1.060522 -0.002056 0.010833 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 981 980 -0.964187 -0.015432 0.024238 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 982 981 -0.449558 0.205690 -0.521275 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 983 982 -0.324745 0.162539 -0.590048 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 984 983 -0.969302 0.235250 -0.486517 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 985 984 -1.014371 -0.133881 0.169980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 986 985 -1.598971 -0.092817 0.016531 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 987 986 -0.792373 0.084757 -0.295689 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 988 987 -1.024571 -0.019657 0.133452 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 989 988 -0.864609 0.059669 0.017492 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 990 989 -1.133358 0.012401 0.014810 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 991 990 -1.106085 0.010313 -0.006206 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 992 991 -1.122011 -0.023313 -0.009182 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 993 992 -1.041177 -0.004454 0.009892 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 994 993 -1.045852 0.016870 0.007741 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 995 994 -1.098164 0.048092 -0.010547 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 996 995 -1.178911 0.108304 -0.083050 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 997 996 -1.164182 -0.007012 0.025385 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 998 997 -1.023435 0.069967 -0.030139 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 999 998 -1.069126 -0.034531 0.020588 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1000 999 -0.980030 -0.017032 0.050175 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1001 1000 -1.112892 -0.072172 0.011280 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1002 1001 -0.800722 -0.027541 0.012508 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1003 1002 -1.015052 0.033169 -0.023977 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1004 1003 -1.071343 -0.026669 0.020741 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1005 1004 -1.084107 0.048928 -0.003523 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1006 1005 -0.918560 -0.016768 0.029642 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1007 1006 -1.124918 0.099625 -0.118662 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1008 1007 -1.072207 -0.013419 0.001059 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1009 1008 -0.975876 0.051683 0.015661 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1010 1009 -1.012351 -0.195564 0.303619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1011 1010 -0.977548 0.151947 -0.222363 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1012 1011 -0.992376 -0.003505 -0.045306 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1013 1012 -1.004035 -0.022623 0.017371 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1014 1013 -1.043488 0.116468 -0.002100 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1015 1014 -1.312367 0.027137 0.016060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1016 1015 -0.930139 0.061205 0.000097 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1017 1016 -0.904116 -0.033804 0.012280 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1018 1017 -1.229529 0.042580 -0.003003 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1019 1018 -1.136656 0.043828 0.014411 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1020 1019 -1.037645 0.060846 -0.020377 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1021 1020 -1.155752 0.069758 0.019092 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1022 1021 -0.939551 -0.033102 0.003752 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1023 1022 -1.111469 0.026168 0.025323 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1024 1023 -0.981698 -0.020526 0.009882 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1025 1024 -1.163647 0.050156 0.003403 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1026 1025 -1.050188 0.008485 0.095224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1027 1026 -1.166932 0.084576 -0.230867 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1028 1027 -1.008065 0.105300 -0.017397 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1029 1028 -1.053953 0.007062 0.037512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1030 1029 -1.009483 0.050460 0.012435 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1031 1030 -1.029379 0.003239 0.018569 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1032 1031 -1.018800 0.030770 0.009323 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1033 1032 -1.176992 0.081723 0.000768 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1034 1033 -0.971305 0.061647 -0.021784 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1035 1034 -1.100525 0.029920 0.018837 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1036 1035 -1.125001 0.001904 0.004494 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1037 1036 -0.964170 0.009871 0.013271 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1038 1037 -0.985236 0.010291 0.021502 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1039 1038 -1.034723 0.006586 0.002878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1040 1039 -0.957027 0.017817 0.017991 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1041 1040 -1.090177 0.014605 0.008158 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1042 1041 -1.023632 0.114649 0.004277 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1043 1042 -1.042944 0.067667 -0.000482 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1044 1043 -1.126923 0.080019 -0.002745 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1045 1044 -0.948064 -0.082659 0.050712 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1046 1045 -1.006894 0.181842 -0.197135 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1047 1046 -0.963040 -0.023076 -0.007187 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1048 1047 -0.954582 -0.039915 0.024661 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1049 1048 -1.202432 -0.033166 0.090129 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1050 1049 -1.135059 -0.075222 0.008068 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1051 1050 -1.024918 0.074622 -0.012544 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1052 1051 -0.925780 0.065189 0.005168 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1053 1052 -1.029700 0.119737 -0.051903 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1054 1053 -0.469852 0.166749 -0.666383 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1055 1054 -0.415043 0.134616 -0.745539 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1056 1055 -1.021037 -0.060914 -0.014400 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1057 1056 -1.050872 0.038316 -0.043444 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1058 1057 -0.971433 0.032810 0.013540 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1059 1058 -0.431377 -0.233489 0.543021 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1060 1059 -0.388567 -0.145710 0.691716 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1061 1060 -0.974739 -0.004190 0.135836 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1062 1061 -0.963798 0.038313 0.004401 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1063 1062 -1.002216 0.107316 -0.002574 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1064 1063 -1.098014 0.103510 -0.093377 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1065 1064 -1.002458 -0.079709 0.249594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1066 1065 -1.022085 0.106554 -0.070920 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1067 1066 -1.023366 0.040063 -0.007144 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1068 1067 -1.067001 0.041457 -0.003469 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1069 1068 -1.016218 0.068402 -0.003814 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1070 1069 -0.997380 -0.196607 0.211921 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1071 1070 -1.036905 0.023579 0.363891 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1072 1071 -1.001735 0.032399 0.018579 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1073 1072 -0.906749 0.255897 -0.424603 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1074 1073 -1.016258 0.044364 0.002289 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1075 1074 -1.146990 0.140899 -0.179629 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1076 1075 -1.298115 0.052821 -0.017359 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1077 1076 -1.038259 0.244140 -0.247407 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1078 1077 -0.624491 0.189692 -0.504302 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1079 1078 -0.574071 0.182101 -0.531000 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1080 1079 -1.230065 0.131162 -0.305278 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1081 1080 -1.172994 -0.082694 -0.012291 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1082 1081 -1.050705 0.041642 0.007687 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1083 1082 -1.079539 0.071524 -0.011406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1084 1083 -0.959853 -0.053018 -0.034830 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1085 1084 -1.068907 -0.117484 0.181202 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1086 1085 -1.131986 0.025341 -0.028850 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1087 1086 -1.012309 0.040099 0.031183 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1088 1087 -1.110696 0.054243 -0.037823 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1089 1088 -0.991220 0.108889 -0.032652 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1090 1089 -1.000703 0.028497 -0.011805 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1091 1090 -1.143689 0.123226 -0.039877 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1092 1091 -1.002121 0.045998 -0.047476 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1093 1092 -1.001498 0.032481 -0.022934 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1094 1093 -1.001431 0.081322 -0.018623 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1095 1094 -1.046140 0.038535 -0.015098 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1096 1095 -0.892618 -0.038808 0.046980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1097 1096 -1.051883 0.064044 0.002970 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1098 1097 -0.919670 0.015794 -0.021104 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1099 1098 -1.047903 -0.085481 0.037784 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1100 1099 -1.137970 0.061827 -0.010338 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1101 1100 -0.921019 -0.002987 -0.016008 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1102 1101 -1.098709 0.046236 0.051319 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1103 1102 -1.143245 -0.036088 0.011002 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1104 1103 -1.007564 -0.040328 -0.025453 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1105 1104 -1.149523 0.063460 -0.023500 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1106 1105 -1.203592 0.023433 -0.023440 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1107 1106 -1.089898 -0.001301 0.004411 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1108 1107 -1.097396 -0.052698 0.102771 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1109 1108 -0.938076 0.090998 -0.005733 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1110 1109 -0.997131 0.169569 -0.057434 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1111 1110 -1.054957 0.022949 -0.028666 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1112 1111 -1.004357 0.015999 0.000729 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1113 1112 -0.998118 0.112640 -0.015981 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1114 1113 -1.140975 0.059270 -0.013217 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1115 1114 -1.011227 0.134183 -0.237069 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1116 1115 -1.014274 0.237939 -0.199427 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1117 1116 -1.058249 0.138471 -0.245830 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1118 1117 -1.009285 0.175292 -0.194555 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1119 1118 -1.068189 0.136597 -0.118349 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1120 1119 -1.025036 -0.092600 0.094539 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1121 1120 -1.032404 -0.070023 0.032370 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1122 1121 -1.011354 0.072797 0.040446 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1123 1122 -1.035840 0.064652 -0.030731 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1124 1123 -1.083012 0.098081 -0.028985 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1125 1124 -0.882847 0.304806 -0.374190 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1126 1125 -1.051400 0.182303 -0.185147 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1127 1126 -0.644818 0.373364 -0.546702 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1128 1127 -0.578227 -0.308715 0.603140 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1129 1128 -0.926278 0.011511 -0.191660 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1130 1129 -0.234671 -0.107335 0.545836 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1130 1128 -1.019546 -0.607799 0.340600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1131 1130 -0.072274 -0.043306 0.629567 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1131 1129 -0.234350 -0.260554 1.161850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1131 1128 -0.588301 -1.147788 0.972440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1132 1131 -0.163299 -0.011852 0.677534 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1132 1126 0.623726 -2.589971 1.708065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1133 1132 -0.192647 0.033893 0.688063 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1133 1126 1.994029 -1.682692 2.390085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1133 1125 2.721878 -2.487482 2.221805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1134 1133 -0.102695 -0.111635 0.674186 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1135 1134 -0.912687 0.036404 -0.016593 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1136 1135 -0.451651 -0.125923 0.224601 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1137 1136 0.080789 -0.033520 0.545053 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1137 1135 -0.314967 -0.354128 0.756912 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1138 1137 0.031931 0.044869 0.509481 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1138 1134 -0.491593 -1.378647 1.249481 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1139 1138 -0.008944 0.056916 0.586091 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1139 1126 -0.178822 1.016576 -1.389160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1139 1127 -0.026935 0.303445 -0.846160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1140 1139 -0.027490 -0.012623 0.605870 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1140 1126 -0.720602 0.724480 -0.784780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1140 1127 -0.190383 0.223987 -0.241780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1140 1128 0.482892 -0.010171 -0.858770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1141 1140 0.003561 0.058028 0.507495 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1141 1126 -0.951474 0.273074 -0.280490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1141 1127 -0.245427 0.091077 0.262510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1141 1128 0.457179 0.211385 -0.354480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1142 1141 -1.215731 0.011589 -0.013696 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1142 1131 0.572347 -0.095426 -1.344720 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1142 1130 0.490672 0.013612 -0.712880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1142 1129 0.265892 0.074556 -0.182870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1142 1128 -0.678105 0.220622 -0.372280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1143 1142 -0.986188 -0.034535 0.117828 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1143 1130 -0.566779 0.011011 -0.591350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1143 1129 -0.797289 0.044255 -0.061340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1144 1143 -0.944888 -0.060582 0.171096 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1145 1144 -1.079002 -0.075917 -0.025647 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1146 1145 -1.095723 0.005591 -0.025168 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1147 1146 -1.029129 0.035952 -0.020590 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1148 1147 -0.968468 0.125327 -0.019595 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1149 1148 -0.980147 0.015833 -0.025984 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1150 1149 -0.952490 0.028174 -0.009812 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1151 1150 -1.059444 -0.049730 -0.053978 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1152 1151 -1.052973 0.058444 -0.035004 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1153 1152 -1.210191 0.030936 -0.016041 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1154 1153 -0.988572 -0.036305 -0.031983 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1155 1154 -0.933298 -0.012074 -0.019647 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1156 1155 -1.126453 0.042376 -0.003624 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1157 1156 -1.016908 0.042038 -0.007740 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1158 1157 -1.293190 0.070593 -0.008735 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1159 1158 -1.002199 -0.106527 0.240032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1160 1159 -1.143434 -0.028450 -0.025114 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1161 1160 -1.020555 0.031459 -0.022347 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1162 1161 -0.922221 0.027330 -0.028211 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1163 1162 -1.000273 -0.028528 -0.006018 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1164 1163 -1.208238 -0.051484 -0.019409 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1165 1164 -0.983304 0.043809 -0.016007 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1166 1165 -1.097616 -0.007282 0.057731 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1167 1166 -1.097648 0.054897 0.041940 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1168 1167 -1.010804 0.015352 -0.007835 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1169 1168 -1.006238 0.150645 -0.017917 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1170 1169 -0.983533 -0.077262 0.002278 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1171 1170 -1.107819 -0.059843 -0.027605 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1172 1171 -0.978903 0.100417 -0.036009 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1173 1172 -0.959920 0.108414 -0.025903 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1174 1173 -0.915101 -0.006849 -0.009836 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1175 1174 -1.019730 -0.132219 0.197348 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1176 1175 -1.004847 0.059156 -0.018842 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1177 1176 -0.981141 0.049615 -0.000235 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1178 1177 -1.105673 -0.033856 -0.002471 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1179 1178 -0.998143 -0.027366 -0.050364 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1180 1179 -1.013854 0.008658 -0.015415 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1181 1180 -1.211859 0.043094 -0.011241 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1182 1181 -1.071482 0.002211 -0.008882 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1183 1182 -1.055693 0.088515 -0.151678 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1184 1183 -1.025272 0.080385 -0.182291 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1185 1184 -0.873731 0.066032 -0.036952 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1186 1185 -0.904771 -0.062277 0.040257 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1187 1186 -1.141925 -0.001759 0.063399 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1188 1187 -1.119883 -0.120753 0.162449 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1189 1188 -1.129609 -0.100726 0.077021 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1190 1189 -0.952921 -0.030357 0.051358 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1191 1190 -1.156437 -0.079356 -0.005760 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1192 1191 -0.994811 -0.061636 0.027900 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1193 1192 -1.038501 -0.041552 0.002543 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1194 1193 -1.176088 0.114794 -0.023578 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1195 1194 -1.084763 -0.024314 -0.004869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1196 1195 -1.259955 0.092326 -0.001087 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1197 1196 -1.027996 -0.030570 -0.026783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1198 1197 -1.009556 -0.012590 -0.000918 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1199 1198 -1.135654 -0.037644 0.011595 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1200 1199 -1.053249 0.140378 -0.031767 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1201 1200 -0.932477 0.026668 -0.030502 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1202 1201 -1.039566 -0.023208 -0.008609 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1203 1202 -1.029960 0.040566 -0.004338 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1204 1203 -0.958943 -0.102228 0.093790 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1205 1204 -0.992610 0.022507 0.070897 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1206 1205 -1.052299 0.177985 -0.036743 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1207 1206 -1.081741 -0.006360 -0.004451 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1208 1207 -0.998430 0.095094 -0.000997 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1209 1208 -0.992278 0.001666 0.000199 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1210 1209 -1.060740 -0.055217 -0.012817 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1211 1210 -0.961900 0.099584 -0.007717 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1212 1211 -1.030320 0.052777 -0.021161 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1213 1212 -1.108183 -0.033570 -0.005101 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1214 1213 -1.085359 0.033685 -0.024306 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1215 1214 -1.076574 0.096629 -0.009732 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1216 1215 -1.044987 0.050509 0.020809 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1217 1216 -0.983888 0.098425 0.049676 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1218 1217 -1.154506 0.030169 -0.030652 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1219 1218 -1.022843 0.031937 -0.006086 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1220 1219 -1.093388 0.093030 -0.005110 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1221 1220 -1.037299 0.033427 -0.001420 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1222 1221 -1.001526 0.066924 -0.018475 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1223 1222 -0.949465 -0.001444 -0.000511 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1224 1223 -1.019523 0.013237 -0.005231 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1225 1224 -0.905024 0.044191 -0.006485 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1226 1225 -0.925236 0.022313 -0.023498 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1227 1226 -1.057783 0.003778 -0.012703 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1228 1227 -0.987544 0.062914 -0.000119 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1229 1228 -1.060560 -0.013547 -0.017636 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1229 663 4.132931 -2.300370 1.491304 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1229 662 3.987521 -3.426769 1.448582 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1230 1229 -1.020262 -0.033050 0.035121 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1230 663 3.223266 -2.162061 1.535714 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1230 662 3.128007 -3.293804 1.492992 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1231 1230 -1.053034 -0.109558 0.282412 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1231 662 2.848701 -2.487801 1.765342 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1232 1231 -1.013892 -0.205896 0.377669 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1233 1232 -0.939906 -0.289789 0.515332 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1233 660 3.843816 -1.170556 2.681415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1234 1233 -1.030255 -0.124802 0.327902 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1234 659 3.959194 -0.145402 3.009789 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1234 658 5.001982 -0.283488 3.030541 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1235 1234 -1.136644 -0.025278 0.040109 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1235 657 5.047079 -0.241763 3.080115 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1236 1235 -1.046072 0.085796 0.003667 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1236 657 4.011601 -0.148633 3.094710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1236 656 5.039338 -0.189211 3.112895 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1237 1236 -1.150710 0.013433 0.019784 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1237 655 4.950179 -0.103567 -3.139548 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1237 656 3.899065 -0.067071 3.132605 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1238 1237 -1.057114 -0.037152 0.028594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1238 654 4.916462 0.139562 3.138260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1238 655 3.904347 0.110997 -3.094068 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1239 1238 -1.078045 -0.053576 0.046531 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1239 653 4.917120 0.320467 -3.095039 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1239 654 3.858102 0.319228 -3.108215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1240 1239 -1.016764 0.061382 0.007869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1240 653 3.838503 0.459414 -3.073939 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1240 652 4.939390 0.504472 -3.060544 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1241 1240 -0.982173 0.024301 0.023884 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1241 651 4.952477 0.750674 -3.015716 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1241 652 3.904592 0.637558 -3.032074 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1242 1241 -0.988419 0.033552 0.054214 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1242 651 3.895763 1.015978 -2.954316 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1242 652 2.856793 0.838776 -2.970674 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1243 1242 -1.098528 0.180416 0.080161 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1243 650 3.620529 1.715048 -2.867670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1243 651 2.629899 1.464630 -2.883466 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1244 1243 -0.527552 0.192055 -0.533886 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1244 650 3.495140 -0.183396 2.879500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1244 649 4.514464 -0.465871 2.858419 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1245 1244 -0.540915 0.134862 -0.531851 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1245 650 2.420918 -1.767227 2.347630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1245 649 3.156176 -2.527627 2.326549 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1246 1245 -0.457655 0.269198 -0.547062 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1247 1246 -0.572904 0.078838 -0.686014 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1248 1247 0.142809 0.004835 -1.187615 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1248 1240 4.362526 0.111317 2.972669 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1248 653 0.501421 0.303777 -0.101271 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1248 1241 3.362432 0.303875 2.944199 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1248 652 -0.591371 0.444444 -0.087875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1249 1248 -1.012221 0.099341 -0.029915 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1249 1240 3.360406 0.076576 2.961073 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1249 1239 4.407434 -0.150371 2.982173 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1249 653 -0.498207 0.313795 -0.112867 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1249 654 0.547578 0.146905 -0.126042 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1250 1249 -1.118350 0.096272 -0.012952 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1250 1238 4.396007 -0.352296 3.006693 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1250 1239 3.361388 -0.173674 2.969983 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1250 654 -0.494558 0.170629 -0.138232 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1250 655 0.512204 0.062814 -0.087375 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1251 1250 -0.989645 0.072980 -0.024715 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1251 1237 4.404374 -0.527566 3.042214 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1251 1238 3.368372 -0.387145 2.996734 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1251 655 -0.511105 0.066623 -0.097334 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1251 656 0.531202 -0.073979 -0.108367 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1252 1251 -1.053697 0.040143 -0.019383 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1252 1236 4.530755 -0.678651 3.055099 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1252 1237 3.396587 -0.534830 3.035389 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1252 657 0.546990 -0.184028 -0.133377 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1252 656 -0.473399 -0.054820 -0.115192 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1253 1252 -1.002238 -0.008827 0.113012 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1253 658 0.763614 -0.233488 -0.047519 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1253 1235 4.625903 -0.239331 -3.109060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1253 1234 5.773503 -0.198829 -3.078060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1253 657 -0.426369 -0.161862 -0.028945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1254 1253 -1.048634 -0.117875 0.116667 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1254 659 0.748632 -0.177956 0.032226 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1254 658 -0.302707 -0.212006 0.052978 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1254 1234 4.678427 0.325110 -2.977563 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1255 1254 -0.990308 -0.059153 0.093990 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1255 659 -0.242832 -0.128754 0.128136 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1255 660 0.750243 -0.020636 0.116512 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1255 658 -1.286079 -0.263327 0.148888 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1255 1234 3.620727 0.748329 -2.881653 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1255 1233 4.610655 1.093968 -2.564903 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1256 1255 -1.031362 0.043001 0.035445 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1256 660 -0.299624 0.025255 0.154581 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1256 661 0.674484 0.117992 0.140519 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1256 1233 3.515570 1.285978 -2.526834 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1257 1256 -1.020640 0.216847 -0.221706 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1257 661 -0.340576 0.138786 -0.084053 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1257 662 0.688142 0.057607 -0.101704 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1258 1257 -0.999829 -0.049949 -0.044697 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1258 1230 3.231344 2.845622 -1.660529 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1258 1229 3.274397 3.853535 -1.616119 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1258 661 -1.350667 0.174044 -0.149886 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1258 662 -0.329517 0.025367 -0.167537 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1259 1258 -1.008443 -0.074413 -0.003502 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1259 663 -0.254951 -0.164559 -0.131578 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1259 664 0.726602 -0.371582 -0.198613 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1260 1259 -0.952351 0.022437 -0.022106 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1260 665 0.722943 -0.556486 -0.124533 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1260 664 -0.305492 -0.367150 -0.211430 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1261 1260 -0.992154 -0.023933 0.003254 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1261 665 -0.288525 -0.532144 -0.142913 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1261 664 -1.313307 -0.323938 -0.229810 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1261 666 0.657299 -0.710734 -0.153410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1262 1261 -0.891811 -0.209908 0.425278 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1262 667 1.115195 -0.346503 0.256792 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1262 666 -0.076449 -0.657230 0.272226 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1263 1262 -0.945150 -0.069427 0.371957 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1263 667 0.103265 -0.043550 0.615113 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1264 1263 -1.112617 0.362910 -0.472431 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1264 668 0.099986 0.392963 0.130854 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1264 667 -0.956901 0.286248 0.138773 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1265 1264 -1.082550 0.102919 -0.205746 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1265 668 -0.859133 0.415722 -0.072513 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1265 669 0.193386 0.304848 -0.079706 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1265 670 1.132993 0.186210 -0.094848 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1266 1265 -1.168642 -0.036303 -0.011008 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1266 669 -0.948782 0.330429 -0.098803 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1266 671 1.030101 0.046512 -0.124773 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1266 670 -0.011611 0.193871 -0.113945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1267 1266 -0.739152 0.410972 -0.570124 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1267 671 0.170101 -0.115971 -0.684089 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1267 670 -0.634684 0.561672 -0.673261 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1268 1267 -0.197558 0.169108 -0.536911 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1268 1266 -0.707126 0.848426 -1.105704 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1269 1268 -0.279787 0.054686 -0.631471 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1270 1269 -0.308908 0.078603 -0.677036 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1270 668 2.032249 2.234845 -2.502664 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1270 1264 1.928430 2.626813 -2.633518 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1271 1270 -0.233685 0.087054 -0.658642 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1271 668 2.720934 0.618451 3.114191 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1271 667 3.781432 0.557271 3.122110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1271 1264 2.881600 0.990746 2.983337 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1271 1263 3.838773 0.460979 2.506997 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1272 1271 -0.326280 0.171619 -0.556007 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1272 667 3.180211 -1.411605 2.568885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1272 1264 2.642365 -0.569988 2.430112 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1272 1263 3.178403 -1.523663 1.953772 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1273 1272 -0.713695 -0.280285 0.496997 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1273 1262 3.743463 -0.686158 2.814368 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1273 1271 -1.006092 -0.363566 -0.050950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1273 667 2.798813 0.000401 3.071160 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1273 666 4.027099 -0.088374 3.086595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1273 1263 2.851176 -0.098686 2.456047 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1274 1273 -1.087677 -0.052234 0.061980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1274 1261 3.645021 -0.428550 -2.983361 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1274 1262 2.636550 -0.443938 2.874188 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1274 665 3.846091 0.142409 -3.126274 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1274 664 4.890877 0.098281 3.070014 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1274 666 2.883941 0.169733 -3.136771 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1275 1274 -1.042034 0.057784 0.030035 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1275 1230 2.790670 -3.147320 1.627945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1275 1259 4.538332 -0.007463 -2.987948 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1275 663 4.765095 0.194176 -3.119526 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1275 664 3.763422 0.248542 3.096624 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 1275 -0.950672 0.023298 0.030731 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1276 1230 1.904285 -3.040353 1.646185 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 1229 2.027491 -4.041633 1.690595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 1257 5.632183 0.452387 -3.042304 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 1258 4.623935 0.297709 -2.976471 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 661 5.984839 0.348045 -3.126357 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 662 4.953140 0.326850 3.139177 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1276 1231 1.955976 -1.979936 1.373835 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1277 1276 -1.149789 -0.049261 0.037288 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1277 660 5.854267 0.515350 -3.086295 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1277 1257 4.520514 0.614763 -3.016304 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1277 661 4.875764 0.519624 -3.100357 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1278 1277 -1.089464 -0.036980 0.034769 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1278 659 5.773003 0.721877 -3.048111 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1278 660 4.776779 0.648231 -3.059735 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1278 1255 5.525856 0.601614 3.106938 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1278 1256 4.479782 0.695190 3.068869 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 1278 -1.029413 -0.002583 0.025852 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1279 1254 5.503775 0.743056 -3.054557 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 659 4.742507 0.855263 -3.022331 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 660 3.748512 0.755962 -3.033955 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 658 5.786907 0.980572 -3.001579 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 1234 0.871317 0.012500 0.251065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 1255 4.498542 0.728669 3.132718 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1279 1256 3.450404 0.795249 3.094649 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1280 1279 -1.034327 0.157570 -0.019920 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1280 1254 4.488887 0.704438 -3.075617 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1280 658 4.776958 0.935939 -3.022639 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1280 1235 0.967095 0.301701 0.199005 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1280 1253 5.568705 0.832214 -2.975120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1280 1234 -0.157928 0.071596 0.230005 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1281 1280 -0.992063 0.031749 -0.049561 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1281 1236 0.949029 0.390866 0.132150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1281 1235 -0.082947 0.273306 0.146745 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1281 1253 4.540093 0.562723 -3.027380 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1281 657 4.945237 0.772121 -3.056325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1282 1281 -1.079738 0.110065 0.025050 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1282 1236 -0.065701 0.439729 0.151830 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1282 1237 1.070362 0.567721 0.132120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1283 1282 -0.923358 0.036156 0.003279 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1283 1236 -1.102561 0.462450 0.173010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1283 1237 0.030537 0.614473 0.153300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1283 1251 4.426946 1.204746 -2.888914 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 1283 -1.126382 0.116041 -0.185137 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1284 1237 -0.898341 0.758731 -0.019075 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 1238 0.145587 0.701868 -0.064555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 1251 3.534160 0.586172 -3.061289 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 1250 4.555484 0.659423 -3.071248 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 654 5.060811 0.523976 3.073705 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1284 655 4.048962 0.560763 3.124562 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 1284 -1.024480 -0.012785 -0.068019 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1285 1238 -0.839956 0.722322 -0.128485 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 1250 3.558221 0.398230 -3.135178 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 1249 4.602280 0.374516 3.135817 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 1239 0.195788 0.550340 -0.165195 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 653 5.098666 0.057849 3.022950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 654 4.053863 0.230777 3.009775 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1285 655 3.046430 0.332132 3.060632 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 1285 -1.181364 -0.013013 0.020945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1286 1240 0.071108 0.364695 -0.158515 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 1249 3.429015 0.515190 -3.119588 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 1248 4.432239 0.521412 -3.131184 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 1239 -0.980660 0.568549 -0.137415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 653 3.934006 0.212433 3.050730 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1286 654 2.884803 0.356273 3.037555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1287 1286 -0.863223 0.033757 0.078380 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1287 1240 -0.920024 0.380666 -0.082970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1287 1248 3.416840 0.866086 -3.055639 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1287 653 2.943348 0.520384 3.126276 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1287 1241 0.092907 0.274675 -0.111440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1287 652 4.044182 0.474051 3.139672 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1288 1287 -1.025302 0.110452 -0.171212 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1288 1241 -0.879049 0.323683 -0.275275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1288 1243 1.096000 -0.602401 -0.407525 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1288 1242 0.078334 0.027981 -0.336675 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1288 651 4.091011 -0.300048 2.992194 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1288 652 3.051832 -0.124077 2.975836 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 1288 -1.184005 0.143741 0.035261 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1289 1243 -0.027335 -0.554200 -0.389065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 1242 -1.056463 0.057289 -0.318215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 650 3.973169 -0.340681 3.026450 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 651 2.961585 -0.196614 3.010654 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 1245 0.979337 -0.485257 0.678820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 1244 0.488845 -0.671026 0.146950 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1289 649 5.022867 -0.470860 3.005369 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1290 1289 -1.047557 -0.035775 0.139609 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1290 650 3.005786 0.088435 -3.136350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1290 1245 0.050985 -0.414638 0.799205 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1290 1244 -0.413647 -0.657968 0.267335 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1290 649 4.063520 0.085261 3.125754 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1291 1290 -1.038124 -0.011596 0.047840 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1291 648 4.163176 0.276342 3.138259 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1291 649 3.079324 0.314063 -3.105181 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1292 1291 -1.071524 0.028740 0.024032 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1292 646 5.096409 0.347635 -3.101090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1292 647 4.168649 0.321364 -3.119137 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1292 648 3.132539 0.325249 -3.135616 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1293 1292 -1.049441 0.056674 0.015648 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1293 646 4.034391 0.437433 -3.081100 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1293 647 3.107341 0.392623 -3.099147 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1294 1293 -1.169173 0.151553 0.009215 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1294 646 2.874861 0.522200 -3.065350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1294 645 3.880584 0.567175 -3.114902 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1295 1294 -1.076854 0.152078 0.011082 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1295 643 5.008070 0.726051 -3.069308 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1295 644 3.888278 0.680667 -3.084140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1295 645 2.814008 0.657588 -3.102232 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1296 1295 -1.144246 0.060438 0.015811 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1296 642 5.074908 0.851943 -3.122725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1296 643 3.960301 0.832637 -3.051148 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1296 644 2.841519 0.766926 -3.065980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1297 1296 -1.076075 0.014740 0.025999 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1297 642 3.942786 0.979458 -3.100315 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1297 641 4.792863 1.020566 -3.078587 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1298 1297 -1.027955 0.025465 -0.059466 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1298 640 4.908135 0.707941 -3.131607 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1298 642 2.964298 0.735259 3.105600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1298 641 3.815012 0.710624 3.127328 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1299 1298 -0.968663 0.056155 -0.075914 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1299 639 4.942510 0.180084 3.078690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1299 640 3.945103 0.295762 3.058353 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1300 1299 -1.117096 0.067881 0.014721 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1300 639 3.883685 0.275407 3.094555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1300 638 4.966802 0.211361 3.113964 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1301 1300 -1.004671 -0.068886 0.002145 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1301 637 5.047770 0.230934 3.141522 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1301 638 3.935179 0.262119 3.124164 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1302 1301 -1.165550 -0.005996 0.032922 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1302 637 3.963109 0.335792 -3.115593 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1303 1302 -1.203594 0.134248 -0.078301 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1303 636 3.868473 0.015176 3.054982 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1303 635 4.788787 -0.126632 2.978154 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1304 1303 -0.880667 -0.062472 0.020094 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1304 634 4.822900 -0.216696 3.007948 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1304 635 3.818982 -0.040215 2.993954 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1305 1304 -1.069490 -0.071051 0.021139 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1305 634 3.722743 -0.111531 3.034148 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1305 633 4.692744 -0.289622 2.971519 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1306 1305 -1.038917 0.041021 0.001296 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1306 632 4.671333 -0.443592 3.009570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1306 633 3.591557 -0.263793 2.979174 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1307 1306 -1.161454 0.086099 -0.011496 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1307 631 4.581273 -0.556839 3.028876 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1307 632 3.549252 -0.389812 3.007220 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1308 1307 -1.108465 0.078475 0.016149 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1308 630 4.585386 -0.536728 -3.091281 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1308 631 3.530288 -0.473709 3.043636 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1309 1308 -1.077187 -0.037284 0.038830 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1309 630 3.531078 -0.451294 -3.073261 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1310 1309 -0.995330 0.016237 0.003532 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1310 629 3.523153 -0.305616 -3.039968 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1310 628 4.571261 -0.216386 -3.021535 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1311 1310 -1.007081 0.080532 0.008978 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1311 627 4.609485 -0.019490 -2.986892 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1311 628 3.550681 -0.142193 -3.008425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1312 1311 -0.995510 0.034450 0.014213 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1312 627 3.565103 0.056454 -2.976882 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1312 626 4.577317 0.233226 -2.956179 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1313 1312 -1.005258 0.039467 0.029987 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1313 625 4.582786 0.483331 -2.925196 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1313 626 3.549759 0.278954 -2.948139 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1314 1313 -1.074690 0.007143 0.004521 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1314 624 4.569540 0.614912 -2.917235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1314 623 5.613341 0.806897 -2.899878 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1315 1314 -1.013289 0.024061 0.007183 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1315 623 4.573330 0.901538 -2.886948 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1315 622 5.602907 0.919060 2.971910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1316 1315 -0.917614 0.074344 0.016328 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1316 622 4.570974 0.982938 2.978780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1317 1316 -1.001382 0.028380 0.005042 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1318 1317 -1.091302 0.151569 0.003795 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1318 620 4.662226 0.819545 3.056623 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1318 621 3.530721 0.938574 3.023443 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1319 1318 -0.925514 0.108672 0.017379 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1319 619 4.592061 0.783522 3.088563 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1319 620 3.573716 0.889277 3.067543 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1320 1319 -0.949247 -0.045942 -0.093380 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1320 618 4.499854 0.276531 3.015076 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1320 619 3.614082 0.439332 2.992033 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1321 1320 -0.994429 0.049966 -0.013056 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1321 618 3.479494 0.190161 2.992371 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1321 617 4.687036 0.022112 3.030187 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1322 1321 -1.179546 0.038082 0.018509 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1322 616 4.512500 -0.031354 3.070736 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1322 617 3.515566 0.092266 3.038077 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1323 1322 -0.928600 -0.036457 0.039543 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1323 616 3.481041 0.127084 3.106581 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1323 615 4.584269 0.066538 3.136509 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1324 1323 -1.003947 0.068340 0.010822 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1324 614 4.724723 0.111827 3.055687 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1324 615 3.616711 0.185293 -3.127887 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1325 1324 -1.185904 0.033214 0.002494 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1325 614 3.555520 0.177200 3.064117 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1325 613 4.555044 0.115079 3.097615 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1326 1325 -1.105308 0.011825 0.003528 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1326 612 4.478554 0.130676 3.133898 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1326 613 3.509297 0.168759 3.110815 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1327 1326 -0.829032 0.002188 0.015622 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1327 611 4.629071 0.108463 3.099986 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1327 612 3.547611 0.168870 3.135738 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1328 1327 -1.132347 -0.060941 -0.127881 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1328 610 4.476612 -0.535489 3.007620 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1328 611 3.470239 -0.363391 2.978471 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1329 1328 -0.899490 -0.052550 -0.073031 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1329 610 3.528477 -0.774525 2.953260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1329 609 4.456868 -1.006326 2.865072 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1329 611 2.532941 -0.548001 2.924111 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1330 1329 -1.282064 -0.093125 0.215883 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1330 609 3.388248 -0.130503 3.078037 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1330 608 4.468207 -0.216957 3.114025 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1331 1330 -1.181229 0.008824 0.009341 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1331 607 4.358500 -0.198961 -3.132957 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1331 608 3.347594 -0.159742 3.122315 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1332 1331 -1.087310 0.001861 0.001066 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1332 606 4.358448 -0.145507 -3.093057 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1332 607 3.312218 -0.142988 -3.125137 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1333 1332 -1.068468 0.069724 0.006200 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1333 605 4.446896 -0.047558 -3.055324 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1333 606 3.356501 -0.101274 -3.084767 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1334 1333 -1.035717 -0.015480 0.015782 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1334 605 3.434182 0.041284 -3.043844 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1334 604 4.549793 0.122737 -3.016722 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1335 1334 -1.104978 -0.026425 0.012479 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1335 604 3.442576 0.205927 -2.999982 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1335 602 5.418246 0.556807 -2.933991 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1335 603 4.436724 0.366934 -2.968024 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1336 1335 -1.113912 -0.000392 -0.001498 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1336 589 2.897176 2.860656 -1.526570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1336 602 4.340623 0.620688 -2.920061 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1336 603 3.361840 0.417161 -2.954094 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1336 588 2.777203 3.875441 -1.398980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1337 1336 -0.978556 0.013747 0.014976 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1337 588 1.727981 3.887209 -1.395530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1338 1337 -0.963238 0.454597 -0.602003 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1338 587 3.176985 3.619356 -1.891225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1339 1338 -0.376099 0.177025 -0.536480 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1339 587 4.217799 1.628366 -2.426085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1340 1339 -0.533439 0.290886 -0.508213 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1340 586 4.735682 -0.404451 2.930210 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1340 587 3.981089 -0.486049 -2.938425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1341 1340 -0.985170 0.003181 -0.022795 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1342 1341 -1.145054 -0.010706 -0.031245 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1342 583 3.200824 -2.349714 1.648392 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1343 1342 -1.098259 -0.012801 -0.007159 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1343 582 2.201064 -3.413203 1.668970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1343 583 2.154494 -2.388257 1.638582 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1344 1343 -0.965924 -0.254625 0.457595 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1345 1344 -1.070254 -0.108514 0.412928 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1345 581 3.079250 -1.881800 2.606961 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1346 1345 -0.911768 -0.217466 0.385636 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1346 578 3.522287 0.630540 -1.892875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1347 1346 -0.903615 0.015304 0.250904 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1347 578 2.270967 1.435525 -1.655420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1347 577 2.423182 2.445121 -1.711030 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1348 1347 -1.049518 0.039603 0.023156 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1348 577 1.316729 2.487000 -1.693415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1349 1348 -0.754215 0.457550 -0.706000 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1349 577 1.841827 1.516782 -2.388385 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1349 576 2.612142 2.191786 -2.384085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1350 1349 -0.291743 0.200616 -0.616860 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1350 575 4.062415 0.511207 -2.983155 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1350 576 3.074374 0.383055 -3.001575 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1351 1350 -1.105186 0.018067 -0.006018 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1351 575 2.996507 0.512968 -2.989865 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1351 574 4.013153 0.639692 -2.979935 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1351 573 5.060434 0.824058 -2.961575 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1352 1351 -1.033221 0.219593 -0.209370 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1352 574 3.003624 -0.055217 3.091660 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1352 573 4.066268 -0.094906 3.110020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1353 1352 -0.879362 0.101830 -0.030410 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1353 573 3.069680 -0.144844 3.092230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1353 572 4.112842 -0.251947 3.100780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1354 1353 -1.146952 0.014691 0.025585 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1354 572 2.991404 -0.082993 3.135600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1354 571 3.987913 -0.100676 3.134920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1355 1354 -1.092961 -0.007244 -0.025204 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1355 570 3.967722 -0.160369 3.128120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1355 571 2.920678 -0.141211 3.117130 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1356 1355 -1.072880 0.013957 -0.012247 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1356 570 2.942782 -0.186809 3.109200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1356 569 3.886062 -0.231343 3.127330 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1357 1356 -0.919416 -0.074203 0.070030 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1357 569 2.922866 -0.063656 -3.102135 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1357 568 4.032568 -0.091594 3.129030 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1358 1357 -0.341414 -0.214075 0.615249 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1358 569 2.092256 1.490915 -2.493455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1358 568 3.018633 2.102505 -2.545475 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1359 1358 -0.189078 -0.036957 0.529815 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1359 569 0.817306 2.275041 -1.949465 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1359 1357 -0.427693 -0.370177 1.152670 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1360 1359 -0.279265 -0.091041 0.534205 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1361 1360 -0.185567 -0.082727 0.535364 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1361 1354 1.651932 -3.178786 2.246155 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1361 1355 0.994933 -2.340273 2.263945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1361 571 -0.762646 -0.003349 -0.902110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1362 1361 -0.338388 -0.034796 0.556501 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1362 572 -0.010582 -0.813689 -0.351870 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1362 1353 3.827775 -2.312344 2.830535 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1362 1354 2.775530 -1.905919 2.795715 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1363 1362 -0.293880 -0.109239 0.549231 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1363 572 0.149840 -0.791044 0.192020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1363 1353 4.209837 -0.087216 -2.908760 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1363 1354 3.099117 -0.283942 -2.943580 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1364 1363 -0.969838 0.016256 0.200557 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1364 1352 4.081169 1.275893 -2.708040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1364 573 0.351246 -0.346210 0.401980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1364 572 -0.634171 -0.704834 0.410530 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1364 1353 3.176713 0.862365 -2.690250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1365 1364 -0.856723 0.329931 -0.507389 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1365 1352 3.306161 -0.370369 3.078475 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1365 574 0.312001 -0.125807 -0.113050 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1365 573 -0.746024 -0.019170 -0.094690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1365 1351 4.348206 -0.598730 2.866885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1366 1365 -1.042337 0.067093 0.049762 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1366 575 0.258120 -0.218415 -0.078640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1366 1350 4.326966 -0.675617 2.904515 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1366 574 -0.760605 -0.109655 -0.068710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1366 1351 3.292596 -0.403207 2.911225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1367 1366 -1.058170 -0.012621 0.036588 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1367 575 -0.770537 -0.221876 -0.038980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1367 1350 3.313238 -0.517390 2.944175 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1367 576 0.223448 -0.290005 -0.057400 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1367 1349 3.610089 -0.687337 2.326685 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1368 1367 -1.001457 0.120715 0.001887 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1368 1348 2.783626 -1.500109 1.629115 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1368 577 0.224108 -0.330576 -0.064300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1368 1350 2.293991 -0.465823 2.941575 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1368 576 -0.795197 -0.230405 -0.060000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1368 1349 2.590400 -0.636541 2.324085 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1369 1368 -1.032696 0.072738 0.051580 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1369 578 0.199295 -0.344034 0.033290 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1369 1347 1.892011 -2.430355 1.688710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1369 1346 2.015969 -3.426855 1.926165 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1369 1348 1.785150 -1.372731 1.671095 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1369 577 -0.821197 -0.311645 -0.022320 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1370 1369 -0.985850 -0.010394 0.164127 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1370 579 0.280122 -0.259229 -0.004980 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1370 580 0.654365 -0.324094 -0.554867 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1370 1346 1.508915 -3.183765 2.078735 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1371 1370 -0.441960 -0.107836 0.537175 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1371 579 -0.108146 -0.201769 0.519365 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1371 580 0.248292 -0.070556 -0.030522 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1371 581 0.606566 -0.172838 -0.680234 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1372 1371 -0.461123 -0.213638 0.572099 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1372 580 -0.213889 -0.077620 0.556801 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1372 581 0.141026 0.035770 -0.092911 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1372 1345 3.729183 -0.349044 -2.699872 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1373 1372 -1.173731 0.028915 0.077400 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1373 1343 3.867418 1.526741 -1.742908 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1373 582 0.127683 -0.057246 -0.073938 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1373 581 -0.946779 0.055275 -0.020527 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1373 1344 3.462548 0.555347 -2.207578 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1374 1373 -0.947646 0.089460 0.005603 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1374 1343 2.860969 1.580278 -1.734624 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1374 582 -0.865516 -0.034635 -0.065654 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1374 583 0.153301 -0.155853 -0.096042 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1374 1342 3.020677 2.590888 -1.744434 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1375 1374 -1.007672 0.120568 -0.138249 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1375 584 0.093392 -0.282627 -0.229652 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1375 585 0.716590 -0.301289 0.445094 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1376 1375 -0.462135 0.204932 -0.588493 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1376 1340 4.361956 2.096880 -2.492453 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1376 586 0.345001 -0.443658 0.437757 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1376 585 -0.056879 -0.476924 -0.137432 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1377 1376 -0.987923 0.091625 -0.322483 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1377 588 0.789015 0.324793 0.408270 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1377 586 -0.842467 -0.446079 0.112690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1377 587 -0.153135 -0.128444 0.527240 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1378 1377 -0.984533 0.065674 -0.089318 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1378 589 0.768945 0.603981 0.203070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1378 1336 4.051854 -1.804233 1.729640 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1378 602 2.752395 2.383566 -1.190421 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1378 1337 3.893721 -0.780537 1.726190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1378 588 -0.214087 0.325003 0.330660 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1379 1378 -1.028717 0.004879 -0.104469 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1379 589 -0.196317 0.573530 0.095280 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1379 601 1.581666 3.055967 -0.860237 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1379 1336 2.808459 -2.173888 1.621850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1379 602 1.967072 2.129405 -1.298211 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1380 1379 -0.856319 0.453903 -0.468640 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1380 590 0.132652 0.572258 -0.416370 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1380 601 1.929009 2.416149 -1.339967 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1381 1380 -0.324956 0.123554 -0.525854 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1381 600 2.606547 1.484895 -1.346520 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1381 591 0.773331 -0.053187 -0.249540 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1381 601 2.617289 1.259985 -1.860087 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1382 1381 -0.964432 0.125951 -0.251615 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1382 599 1.791518 1.081803 -1.079680 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1382 591 -0.251547 -0.183017 -0.498710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1382 592 0.070925 -0.294680 0.089960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1383 1382 -1.106495 0.254696 -0.368156 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1383 598 0.894675 0.834511 -0.938890 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1383 597 0.802056 1.010864 -0.421420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1383 596 0.481722 1.046182 0.089800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1383 593 -0.588958 -0.069414 0.305975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1383 595 0.163167 0.866503 0.777285 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1384 1383 -1.155327 0.014587 0.018449 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1384 597 -0.352262 1.028228 -0.417360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1384 596 -0.672738 1.062246 0.093860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1385 1384 -0.949547 0.087736 0.013412 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1386 1385 -1.037790 0.062291 -0.008253 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1387 1386 -1.041844 0.022489 -0.013336 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1388 1387 -1.087921 -0.144191 0.024913 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1389 1388 -1.004115 0.147048 -0.092988 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1390 1389 -1.013038 0.015050 -0.053059 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1391 1390 -0.970454 0.184581 -0.059413 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1392 1391 -1.071604 -0.071657 -0.002550 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1393 1392 -1.003567 0.025415 0.000009 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1394 1393 -1.054064 0.077045 -0.005171 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1395 1394 -0.964155 0.098701 -0.012639 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1396 1395 -1.000046 0.087569 0.001499 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1397 1396 -1.054236 0.075119 -0.002624 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1398 1397 -1.049210 -0.024350 0.007422 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1399 1398 -1.066256 0.081087 -0.013763 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1400 1399 -1.026521 0.009055 0.023704 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1401 1400 -1.018336 -0.065650 0.015148 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1402 1401 -1.025550 0.106789 -0.016317 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1403 1402 -1.055763 0.089757 -0.055456 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1404 1403 -1.052782 0.048321 -0.078584 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1405 1404 -1.244843 -0.059213 0.156507 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1406 1405 -1.063990 -0.054321 0.142254 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1407 1406 -1.023741 0.060028 0.028834 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1408 1407 -1.013597 0.073576 -0.006962 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1409 1408 -1.240215 0.096148 -0.066151 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1410 1409 -0.967876 0.046421 -0.062885 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1411 1410 -1.114756 0.037190 -0.003963 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1412 1411 -1.048218 0.034882 -0.004348 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1413 1412 -0.999678 0.069670 -0.014681 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1414 1413 -0.931268 0.095023 -0.132449 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1415 1414 -1.133897 0.036312 -0.082649 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1416 1415 -0.920158 -0.020900 -0.005470 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1417 1416 -1.120050 -0.023176 -0.000922 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1418 1417 -0.972844 0.047758 0.012519 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1419 1418 -0.952484 -0.040829 0.041332 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1420 1419 -1.086198 0.066160 -0.000513 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1421 1420 -1.297799 0.054508 -0.003320 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1422 1421 -0.987298 -0.092530 0.165354 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1423 1422 -1.036452 0.027531 -0.018122 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1424 1423 -1.056263 0.022691 0.009377 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1425 1424 -1.181723 -0.053730 0.013840 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1426 1425 -1.036327 0.012741 0.011005 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1427 1426 -1.041353 0.059390 0.012297 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1428 1427 -1.070956 -0.072490 0.020873 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1429 1428 -1.087472 0.094971 -0.008646 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1430 1429 -0.972019 0.032951 0.004911 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1431 1430 -1.027251 0.003717 0.014406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1432 1431 -1.100328 0.037105 0.001390 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1433 1432 -0.944310 -0.006420 -0.000922 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1434 1433 -1.021579 0.076830 -0.000016 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1435 1434 -1.083298 0.026244 0.007019 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1436 1435 -1.013591 0.033657 -0.000439 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1437 1436 -1.159774 0.057019 -0.033259 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1438 1437 -1.088853 0.127715 0.004301 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1439 1438 -1.086067 0.076745 -0.112623 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1440 1439 -0.935175 0.075216 -0.099229 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1441 1440 -1.045051 -0.181757 0.291978 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1442 1441 -0.932463 -0.369997 0.626821 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1443 1442 -0.727356 -0.141382 0.479961 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1444 1443 -0.504085 -0.136358 0.602683 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1445 1444 -0.470448 -0.098170 0.595504 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1446 1445 -0.489905 0.455926 -0.572211 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1447 1446 -0.996060 -0.072210 0.023074 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1448 1447 -0.974112 0.188761 -0.225579 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1449 1448 -1.135446 0.016473 0.037599 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1450 1449 -1.062563 0.045681 0.028638 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1451 1450 -1.036237 0.201426 -0.268033 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1452 1451 -1.079738 0.058806 -0.066799 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1453 1452 -1.051012 -0.059618 0.031330 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1454 1453 -1.101808 0.019416 0.124720 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1455 1454 -1.109475 -0.045971 0.034360 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1456 1455 -1.140015 -0.024626 0.015670 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1457 1456 -1.090001 0.041907 0.017401 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1458 1457 -0.897542 0.021501 0.025878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1459 1458 -1.046517 0.080184 0.013883 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1460 1459 -0.965278 0.089478 0.024779 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1461 1460 -1.030340 -0.011876 0.005968 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1462 1461 -1.035525 0.031392 0.024022 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1463 1462 -0.932943 0.030461 0.004949 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1464 1463 -1.165228 0.160572 -0.069767 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1465 1464 -0.967449 0.011747 -0.023513 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1466 1465 -1.081528 -0.059485 0.021650 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1467 1466 -1.019703 -0.001458 0.025073 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1468 1467 -0.950836 0.092326 0.026108 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1469 1468 -1.081160 0.065943 0.017363 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1470 1469 -1.101939 -0.033212 -0.062218 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1471 1470 -0.965725 0.054554 -0.097664 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1472 1471 -1.062279 0.026394 0.016871 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1473 1472 -1.090629 0.059821 0.018611 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1474 1473 -1.281315 0.009127 0.029512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1475 1474 -1.083494 0.034712 0.021146 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1476 1475 -0.949687 0.028992 0.021971 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1477 1476 -1.075483 0.025371 0.022783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1478 1477 -0.959846 -0.030937 0.020702 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1479 1478 -1.083417 0.079940 0.015785 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1480 1479 -1.225136 0.024261 0.004152 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1481 1480 -1.106544 0.089936 0.007999 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1482 1481 -1.092179 0.121002 -0.002027 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1483 1482 -1.100215 0.136666 -0.060012 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1484 1483 -1.108043 0.057292 -0.130998 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1485 1484 -1.094575 0.016428 -0.019380 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1486 1485 -1.005066 -0.028928 0.006327 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1487 1486 -1.001396 -0.036474 0.012606 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1488 1487 -1.015213 0.009676 0.006550 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1489 1488 -1.082968 -0.015844 0.030198 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1490 1489 -0.969937 0.037600 0.017406 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1491 1490 -1.219902 0.042150 -0.001054 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1492 1491 -0.921706 -0.004551 0.002613 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1493 1492 -0.965042 0.006763 0.017835 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1494 1493 -1.146598 0.020416 0.024276 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1495 1494 -0.965690 0.053527 -0.010437 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1496 1495 -1.084537 -0.001437 0.038616 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1497 1496 -0.947699 -0.002672 0.011597 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1498 1497 -1.098096 0.077687 -0.010436 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1499 1498 -0.990014 0.012984 0.019014 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1500 1499 -1.117484 -0.029740 -0.006592 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1501 1500 -0.982270 0.068122 -0.076549 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1502 1501 -1.051790 0.021600 0.006594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1503 1502 -0.959404 0.000978 0.037823 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1504 1503 -1.004124 0.053192 0.013028 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1505 1504 -1.078041 0.057442 -0.000825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1506 1505 -0.962017 -0.081471 0.025877 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1507 1506 -0.958941 0.081019 0.022640 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1508 1507 -1.007084 0.168931 -0.003891 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1509 1508 -1.059273 -0.009257 -0.020231 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1510 1509 -1.056982 0.042265 -0.081912 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1511 1510 -1.019298 0.034032 -0.003565 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1512 1511 -1.040735 -0.049199 0.009531 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1513 1512 -1.125602 0.087024 0.015645 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1514 1513 -0.984074 0.045505 -0.007822 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1515 1514 -1.001644 -0.018480 0.018842 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1516 1515 -1.072139 0.061424 0.006128 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1517 1516 -1.171206 0.031996 -0.001390 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1518 1517 -0.956178 0.057645 -0.003630 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1519 1518 -1.125720 -0.061346 0.017114 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1520 1519 -1.061677 -0.008266 -0.078287 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1521 1520 -1.097511 0.093153 -0.062727 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1522 1521 -1.022754 0.014084 0.001619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1523 1522 -1.151420 0.074200 -0.004682 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1524 1523 -1.023608 -0.006635 -0.005744 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1525 1524 -0.963679 -0.217336 0.258442 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1526 1525 -0.988369 -0.060110 0.050965 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1527 1526 -1.024416 0.025611 0.010300 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1528 1527 -1.053061 0.033911 0.019538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1529 1528 -1.195423 0.093362 -0.114703 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1530 1529 -1.085948 0.013270 -0.042009 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1531 1530 -1.131373 0.144454 -0.011308 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1532 1531 -0.942060 0.054489 -0.042621 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1533 1532 -1.016433 -0.034584 -0.035961 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1534 1533 -0.994528 0.003713 0.002599 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1535 1534 -1.110847 0.028563 0.002314 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1536 1535 -1.079112 0.047800 -0.012230 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1537 1536 -1.024812 0.086173 -0.016172 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1538 1537 -1.025675 -0.046736 -0.003177 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1539 1538 -0.873704 0.442730 -0.536058 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1540 1539 -0.989405 0.077842 -0.232868 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1541 1540 -1.008440 0.048006 0.013793 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1542 1541 -1.026951 0.132117 -0.164602 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1543 1542 -1.052774 0.015452 0.092305 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1544 1543 -1.058354 0.003458 -0.012628 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1545 1544 -1.071022 0.029745 -0.036944 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1546 1545 -1.177032 0.079528 -0.079304 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1547 1546 -1.111143 0.086225 -0.070284 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1548 1547 -1.137077 0.153005 -0.395141 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1549 1548 -1.052423 0.134194 -0.214328 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1550 1549 -0.999886 0.040215 0.025844 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1551 1550 -1.055556 -0.034288 -0.007030 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1552 1551 -0.995042 0.054325 -0.029419 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1553 1552 -1.057077 0.139415 -0.014142 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1554 1553 -1.050435 0.035156 -0.028656 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1555 1554 -1.153069 0.083518 -0.030301 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1556 1555 -1.083178 0.029647 -0.039414 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1557 1556 -0.973374 -0.014023 -0.033319 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1558 1557 -1.092240 -0.004861 0.007173 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1559 1558 -1.116112 0.022157 0.060661 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1560 1559 -1.054016 0.049160 -0.038328 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1561 1560 -0.917842 0.063454 -0.014858 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1562 1561 -1.107550 0.057420 -0.032160 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1563 1562 -1.075993 -0.011025 0.039105 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1564 1563 -1.077877 0.056496 0.040313 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1565 1564 -1.047470 -0.081660 0.220640 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1566 1565 -1.029218 0.069854 -0.127187 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1567 1566 -1.034534 -0.004949 0.000537 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1568 1567 -1.100475 0.111126 -0.018161 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1569 1568 -1.074573 0.032786 -0.039362 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1570 1569 -1.162319 0.127810 -0.027342 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1571 1570 -1.067949 0.077064 -0.077891 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1572 1571 -1.046471 0.188412 -0.266495 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1573 1572 -1.101584 -0.048283 0.271832 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1574 1573 -0.824641 -0.056809 -0.013242 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1575 1574 -0.996917 -0.029011 -0.034181 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1576 1575 -1.080354 0.098933 -0.025783 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1577 1576 -1.043955 -0.155358 0.205707 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1578 1577 -1.340992 -0.030855 -0.004449 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1579 1578 -0.968984 0.045386 -0.020996 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1580 1579 -1.000253 -0.003697 0.008140 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1581 1580 -1.090415 -0.007115 0.000556 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1582 1581 -1.077306 -0.011769 -0.007158 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1583 1582 -1.104395 -0.035928 -0.015304 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1584 1583 -1.127028 0.061241 -0.008842 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1585 1584 -1.026231 0.051975 -0.014707 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1586 1585 -1.076894 -0.012476 -0.037631 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1587 1586 -0.965012 0.113256 -0.038665 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1588 1587 -1.048498 -0.105057 -0.006439 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1589 1588 -0.457200 0.148012 -0.540165 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1589 1587 -1.325312 0.685342 -0.536045 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1590 1589 -0.811246 0.378323 -0.585425 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1591 1590 -0.855420 0.208935 -0.384224 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1592 1591 -1.151050 0.059222 -0.014092 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1593 1592 -0.895819 0.005179 -0.008621 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1594 1593 -1.084141 0.110571 -0.125475 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1595 1594 -1.111909 0.016419 -0.240054 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1596 1595 -0.934341 -0.046967 0.322635 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1597 1596 -1.000299 -0.033202 -0.027694 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1598 1597 -1.009151 0.059572 -0.019822 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1599 1598 -0.989149 0.043731 -0.020361 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1600 1599 -1.046673 -0.138796 0.293045 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1601 1600 -0.998083 -0.121350 0.315622 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1602 1601 -1.029217 0.344858 -0.468529 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1603 1602 -0.995552 0.120544 -0.053094 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1604 1603 -1.090840 0.149600 -0.011060 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1605 1604 -1.138126 0.121858 -0.022512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1606 1605 -1.095073 0.030017 -0.011473 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1607 1606 -1.036789 -0.026972 -0.015394 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1608 1607 -1.007149 0.056294 -0.027589 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1609 1608 -1.011045 -0.011280 0.025706 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1610 1609 -1.010152 0.026216 -0.001688 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1611 1610 -0.968274 0.012092 -0.012869 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1612 1611 -0.929219 0.035067 -0.000923 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1613 1612 -1.026379 -0.073817 0.107132 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1614 1613 -1.004356 0.046943 0.037736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1615 1614 -0.980359 0.010260 -0.016259 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1616 1615 -1.088367 -0.050892 0.001254 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1617 1616 -0.996009 -0.086392 -0.017486 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1618 1617 -1.051734 0.025097 -0.031588 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1619 1618 -1.069883 -0.097153 0.209003 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1620 1619 -0.974581 0.022495 -0.032951 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1621 1620 -1.091250 0.008518 -0.004125 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1622 1621 -1.069336 0.049244 -0.046054 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1623 1622 -0.962749 -0.054944 -0.045497 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1624 1623 -1.000999 0.057777 -0.027776 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1625 1624 -0.935629 0.043317 0.114807 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1626 1625 -0.972951 0.080271 0.021504 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1627 1626 -0.958174 -0.018454 -0.027574 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1628 1627 -0.994467 0.161515 -0.267066 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1629 1628 -1.052765 0.095348 -0.042833 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1630 1629 -0.958937 0.112822 -0.036169 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1631 1630 -1.048354 0.053323 0.000430 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1632 1631 -1.066250 -0.245313 0.345305 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1633 1632 -1.135413 -0.068067 0.152439 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1634 1633 -0.951882 0.272590 -0.314250 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1635 1634 -0.975869 0.068769 0.002408 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1636 1635 -1.058782 -0.107760 0.394578 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1637 1636 -0.485137 0.150395 -0.543098 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1638 1637 -0.913813 0.222262 -0.295486 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1639 1638 -1.032627 0.104708 -0.196512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1640 1639 -0.802360 0.291669 -0.483991 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1641 1640 -1.017734 -0.163927 0.186687 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1642 1641 -0.999919 -0.195457 0.538609 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1643 1642 -0.470624 -0.262461 0.744533 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1644 1643 -0.182096 -0.080712 0.514699 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1645 1644 -0.132382 -0.068673 0.502765 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1646 1645 -0.209857 0.219966 -0.531379 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1646 1644 -0.366537 0.265137 -0.009200 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1646 1643 -0.550291 0.197145 0.507770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1647 1646 -0.956434 0.073102 -0.008169 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1648 1647 -1.070285 0.040166 -0.000297 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1649 1648 -0.826363 0.456495 -0.607440 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1650 1649 -0.963576 0.003050 -0.107878 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1651 1650 -1.091044 0.026634 -0.075501 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1652 1651 -1.125480 0.104604 -0.028593 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1653 1652 -0.922971 0.151268 -0.037825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1654 1653 -0.970585 0.007483 -0.018179 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1655 1654 -1.056754 0.015772 -0.017556 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1656 1655 -0.976324 0.024179 -0.036858 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1657 1656 -1.006307 -0.058085 0.045929 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1658 1657 -0.995945 -0.025199 0.098354 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1659 1658 -1.047855 0.002803 -0.012672 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1660 1659 -0.964946 -0.031382 -0.023992 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1661 1660 -1.113273 0.112264 -0.011317 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1662 1661 -0.978888 -0.013270 0.039381 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1663 1662 -1.007340 0.028470 0.085593 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1664 1663 -0.978508 0.132324 0.019246 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1665 1664 -0.927377 0.158467 -0.242008 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1666 1665 -1.059266 -0.155130 0.252514 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1667 1666 -0.983521 0.008540 0.059620 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1668 1667 -1.055721 0.087526 -0.156695 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1669 1668 -0.973055 0.046612 -0.013855 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1670 1669 -1.019054 0.083963 -0.033320 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1671 1670 -1.046988 0.030812 0.002345 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1672 1671 -1.036917 -0.067025 0.006090 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1673 1672 -1.033456 0.017547 -0.000787 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1674 1673 -1.148606 0.166306 -0.175352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1675 1674 -0.970170 0.067697 -0.005246 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1676 1675 -1.066984 -0.015118 -0.000919 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1677 1676 -0.949119 0.119264 -0.018567 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1678 1677 -1.010049 0.058648 -0.008287 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1679 1678 -0.953908 0.041627 0.021146 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1680 1679 -1.057908 -0.027605 0.131898 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1681 1680 -1.100016 -0.001439 -0.017669 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1682 1681 -1.062201 0.092218 -0.016428 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1683 1682 -1.032217 -0.142127 0.213709 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1684 1683 -1.072405 0.048393 -0.010487 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1685 1684 -1.072906 0.012065 0.008478 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1686 1685 -1.028459 0.065458 0.008769 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1687 1686 -1.003957 0.003060 -0.008174 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1688 1687 -1.021714 0.079404 -0.025298 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1689 1688 -0.941857 0.048117 -0.244710 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1690 1689 -1.066919 -0.156936 0.220468 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1691 1690 -1.039334 -0.088449 0.111701 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1692 1691 -1.089557 0.182287 -0.154034 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1693 1692 -1.093493 0.233881 -0.270096 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1694 1693 -0.939367 -0.166536 0.206381 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1695 1694 -1.009414 -0.013176 0.020496 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1696 1695 -1.086792 -0.089959 0.184888 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1697 1696 -1.019561 0.247084 -0.159523 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1698 1697 -1.003706 0.094431 -0.350401 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1699 1698 -0.379073 0.305041 -0.523617 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1700 1699 -0.246746 0.037678 -0.538337 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1701 1700 -1.139264 0.144916 -0.258027 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1702 1701 -1.078417 0.062654 -0.019661 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1703 1702 -1.099434 0.039527 -0.011512 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1704 1703 -0.957244 0.024449 -0.007575 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1705 1704 -1.188354 -0.000919 0.037956 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1706 1705 -1.050617 -0.026804 0.048903 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1707 1706 -0.994219 0.095593 -0.019652 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1708 1707 -1.022245 0.044254 -0.026611 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1709 1708 -1.028061 0.084925 -0.024141 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1710 1709 -1.023131 0.020937 -0.018321 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1711 1710 -0.977391 0.063296 0.137780 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1712 1711 -1.031657 0.057230 -0.021331 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1713 1712 -1.087019 -0.045714 0.086600 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1714 1713 -1.006165 -0.045076 0.098160 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1715 1714 -1.092212 -0.014043 -0.018902 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1716 1715 -1.015280 0.019081 -0.026240 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1717 1716 -1.151543 0.139709 -0.018650 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1718 1717 -1.110395 -0.020267 -0.016891 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1719 1718 -1.163724 -0.057655 -0.028774 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1720 1719 -1.078884 0.064258 -0.023736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1721 1720 -1.009800 0.082770 -0.018242 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1722 1721 -0.917996 0.050688 -0.017142 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1723 1722 -1.022263 -0.033443 -0.017580 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1724 1723 -0.972482 -0.007402 0.002320 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1725 1724 -1.065525 0.103661 -0.021508 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1726 1725 -1.036122 0.041611 -0.018810 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1727 1726 -1.191061 0.017444 0.040463 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1728 1727 -1.046031 0.001136 0.055918 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1729 1728 -0.991168 0.016920 -0.009231 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1730 1729 -1.015519 0.030605 -0.023071 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1731 1730 -1.013761 0.016697 -0.038220 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1732 1731 -1.017568 -0.030451 0.132458 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1733 1732 -0.928709 0.047532 -0.032172 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1734 1733 -1.080679 0.068860 -0.024930 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1735 1734 -1.018927 -0.037506 0.000964 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1736 1735 -1.055691 0.068912 -0.003794 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1737 1736 -0.946532 0.027589 -0.022940 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1738 1737 -1.147733 -0.103674 0.191033 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1739 1738 -1.045146 -0.018264 -0.017948 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1740 1739 -0.944337 0.042333 -0.023859 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1741 1740 -0.981831 0.005833 -0.028238 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1742 1741 -1.019825 0.003943 -0.028168 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1743 1742 -1.004578 -0.019498 -0.021477 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1744 1743 -1.014214 0.122077 -0.020199 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1744 1441 4.244708 0.406286 3.132671 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1744 1440 5.289894 0.633958 -2.869669 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1745 1744 -0.945947 0.194456 -0.402975 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1745 1439 5.070436 -1.364388 2.905954 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1745 1440 4.146351 -1.170562 3.021954 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1746 1745 -1.003375 0.112218 -0.088914 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1746 1438 4.818215 -2.035612 2.705862 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1747 1746 -0.953963 0.130946 -0.252198 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1748 1747 -0.729005 0.299848 -0.565031 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1748 1446 0.951724 0.122975 0.126770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1748 1444 -0.110850 0.617498 0.152420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1748 1445 0.397495 0.523607 -0.444676 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1749 1748 -1.006095 0.045584 -0.057519 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1749 1447 0.892734 0.204030 0.052910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1749 1446 -0.090699 0.142690 0.080910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1750 1749 -0.521941 0.092654 -0.539399 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1751 1750 -0.439558 0.079854 -0.521595 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1752 1751 -0.364093 0.204319 -0.568736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1753 1752 -0.219938 0.025882 -0.610055 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1753 1442 0.899997 2.928231 -1.042405 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1753 1441 0.142909 3.640885 -0.434125 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1754 1753 -0.303505 0.113064 -0.670755 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1754 1441 2.136656 2.884237 -1.106225 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1754 1744 4.433818 -0.708207 2.044289 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1755 1754 -0.252357 -0.017477 -0.530296 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1755 1439 2.806247 3.452555 -1.479250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1755 1745 3.143767 -1.787384 1.897981 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1755 1440 2.919684 2.515200 -1.363250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1756 1755 -0.458927 0.073068 -0.571318 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1756 1746 1.557243 -2.032614 1.406408 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1756 1439 3.807725 1.519653 -2.050150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1756 1438 4.353908 2.387519 -2.170915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1756 1745 1.260111 -3.071702 1.327081 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1757 1756 -0.706529 -0.129996 -0.208021 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1757 1746 0.469706 -2.311590 1.207408 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1757 1437 4.869498 2.200711 -2.373575 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1757 1438 4.084990 1.468441 -2.369915 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1758 1757 -0.369909 -0.089006 0.503129 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1758 1439 2.252951 2.183694 -1.739310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1758 1755 -1.346719 -0.431174 -0.260060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1758 1440 2.121544 1.248688 -1.623310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1759 1758 -0.974436 -0.089360 0.257261 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1760 1759 -0.568524 0.138297 -0.556612 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1760 1439 1.350803 2.059871 -2.054820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1761 1760 -0.212409 0.226352 -0.544912 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1762 1761 -1.356334 -0.041670 -0.347385 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1762 1436 4.269118 0.782680 -3.126570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1762 1435 5.303866 0.782247 -3.124370 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1763 1762 -1.229190 0.040146 -0.046479 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1763 1435 4.190605 0.652471 3.122698 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1764 1763 -1.088620 0.155706 -0.134308 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1764 1434 4.304375 0.002420 2.992004 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1765 1764 -1.110237 0.111317 -0.082174 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1765 1433 4.196394 -0.502016 2.919885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1766 1765 -1.051080 0.087448 -0.014453 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1766 1433 3.221237 -0.585856 2.902542 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1766 1432 4.175514 -0.844765 2.903002 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1767 1766 -0.992473 0.060923 -0.003631 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1767 1432 3.215763 -0.901297 2.882778 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1767 1431 4.239704 -1.218971 2.877258 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1768 1767 -1.113633 0.025496 -0.041653 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1768 1430 4.119534 -1.591140 2.859451 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1768 1431 3.132236 -1.293099 2.855481 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1769 1768 -1.026828 0.047277 -0.029419 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1769 1430 3.072112 -1.632072 2.838627 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1769 1429 3.959517 -1.933630 2.839237 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1770 1769 -0.984458 -0.137033 0.295504 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1770 1428 4.407134 -0.924721 3.130707 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1770 1429 3.312215 -0.881049 3.126847 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1771 1770 -1.143637 0.029203 0.022205 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1771 1428 3.304596 -0.799005 -3.129565 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1771 1427 4.315909 -0.803962 -3.124555 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1772 1771 -1.056654 0.127175 -0.026337 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1772 1426 4.328227 -0.889181 3.138833 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1772 1427 3.267466 -0.873467 3.134243 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1773 1772 -1.078458 -0.016046 -0.007272 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1773 1425 4.368748 -0.948797 3.125013 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1773 1426 3.324828 -0.911034 3.124283 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1774 1773 -1.144725 -0.062192 0.048340 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1774 1425 3.243562 -0.783681 -3.113905 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1774 1424 4.358380 -0.749271 -3.110775 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1775 1774 -1.222298 0.053732 0.040794 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1775 1423 4.268115 -0.519841 -3.066607 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1775 1424 3.194423 -0.569011 -3.073157 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1776 1775 -0.982804 -0.043992 0.016731 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1776 1423 3.270510 -0.560251 -3.076349 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1776 1422 4.294900 -0.527174 -3.078429 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1777 1776 -1.130432 0.051190 -0.008279 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1777 1422 3.206773 -0.580387 -3.095714 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1777 1421 4.170877 -0.441862 -2.933569 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1778 1777 -1.214136 -0.006626 -0.024617 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1778 1420 4.229393 -0.328938 -2.960494 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1778 1421 2.968298 -0.514116 -2.958344 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1779 1778 -1.085898 0.064546 -0.012460 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1779 1420 3.115271 -0.360302 -2.980069 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1779 1419 4.252986 -0.176772 -2.974359 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1780 1779 -0.983011 0.075807 -0.031820 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1780 1419 3.237749 -0.258699 -2.999997 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1780 1418 4.216824 -0.139286 -2.960167 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1781 1780 -1.152915 -0.042010 -0.018194 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1781 1417 3.939712 -0.038649 -2.983901 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1781 1418 2.952083 -0.192094 -2.980211 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1782 1781 -1.063803 0.008385 -0.033735 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1782 1417 2.935189 -0.092468 -3.008533 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1782 1416 3.969497 0.029073 -3.005163 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1783 1782 -0.897477 -0.009700 -0.007924 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1783 1415 4.011207 0.099523 -3.018123 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1783 1416 3.043347 -0.019639 -3.019943 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1784 1783 -1.195102 0.030822 -0.030931 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1784 1415 2.862311 0.088593 -3.040438 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1784 1414 3.998003 0.137686 -3.118228 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1785 1784 -1.186605 0.037484 -0.025017 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1785 1412 4.739962 -0.101134 3.022532 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1785 1413 3.744821 0.062981 3.032632 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1785 1414 2.853929 0.117804 -3.131378 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1786 1785 -1.008053 -0.032225 -0.018415 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1786 1412 3.689889 -0.194490 2.997823 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1786 1413 2.699107 -0.005839 3.007923 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1786 1411 4.701735 -0.342547 3.000503 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1787 1786 -1.347090 -0.003805 0.038980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1787 1411 3.409535 -0.181253 3.041888 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1787 1410 4.538460 -0.308207 3.044648 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1788 1787 -1.013941 0.040087 0.013306 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1788 1409 4.431667 -0.373481 3.008945 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1788 1410 3.503274 -0.228113 3.062765 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1789 1788 -1.056876 0.006390 0.143538 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1789 1409 3.437951 0.118578 3.136447 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1790 1789 -1.203508 0.052943 -0.022773 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1790 1408 3.476984 0.037787 3.067550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1790 1407 4.518302 -0.078213 3.064650 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1791 1790 -1.002733 0.092256 -0.024432 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1791 1406 4.498564 -0.165082 3.069629 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1791 1407 3.515520 -0.075475 3.057079 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1792 1791 -1.015037 0.043008 -0.033081 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1792 1406 3.437978 -0.215419 3.046518 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1792 1405 4.495001 -0.295275 -3.095508 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1793 1792 -1.148152 0.062874 0.004699 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1793 1405 3.442279 -0.269906 -3.090141 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1793 1404 4.690207 -0.085684 -2.922936 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1794 1793 -1.064125 0.065544 0.000899 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1794 1402 5.685650 0.156207 -3.071070 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1794 1403 4.644500 0.097947 -2.992964 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1794 1404 3.686745 -0.077061 -2.926964 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1795 1794 -1.078945 0.108289 -0.024485 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1795 1402 4.633663 0.087896 -3.089772 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1795 1401 5.674075 0.121937 -3.089632 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1795 1403 3.591606 0.049116 -3.011666 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1796 1795 -1.103136 -0.042307 -0.000737 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1796 1400 5.565107 0.160169 -3.085590 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1797 1796 -0.995492 0.021084 -0.016574 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1797 1400 4.529096 0.139511 -3.096186 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1797 1399 5.566860 0.191691 -3.089156 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1798 1797 -1.027659 0.113424 -0.003808 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1798 1399 4.562217 0.147026 -3.103194 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1798 1398 5.618150 0.153142 -3.103924 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1799 1798 -1.049113 -0.053162 -0.023173 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1799 1398 4.574525 0.110024 -3.108854 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1800 1799 -1.070785 -0.038542 -0.022451 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1800 1397 4.584281 0.098105 -3.125051 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1800 1398 3.548793 0.095410 -3.118741 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1801 1800 -1.155721 -0.041482 0.015212 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1801 1397 3.579079 0.082740 -3.130910 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1801 1396 4.623724 0.073718 -3.129290 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1802 1801 -1.056466 0.104652 0.006468 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1802 1396 3.556152 0.165562 -3.108918 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1802 1395 4.593329 0.165852 -3.106938 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1803 1802 -1.095586 0.056956 0.001417 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1803 1394 4.518636 0.220248 -3.113243 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1803 1395 3.511397 0.200487 -3.105963 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1804 1803 -1.171934 0.096325 -0.022056 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1804 1394 3.395974 0.195040 -3.123092 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1804 1393 4.413023 0.188044 -3.116962 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1805 1804 -1.041148 -0.095897 0.071198 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1805 1393 3.363895 0.481242 -3.049445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1805 1392 4.338644 0.563793 -3.039725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1806 1805 -1.077891 0.044977 0.008294 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1806 1391 4.365739 0.599370 -3.060435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1806 1392 3.294352 0.541734 -3.050785 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1807 1806 -1.015327 0.011593 -0.027831 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1807 1391 3.343875 0.554302 -3.077602 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1807 1390 4.354748 0.595542 -3.124442 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1808 1807 -1.084191 0.037010 -0.033651 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1808 1390 3.264892 0.552662 -3.140389 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1808 1389 4.311087 0.518032 3.097527 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1809 1808 -0.920817 -0.070072 -0.006240 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1809 1388 4.330843 0.350799 3.001838 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1809 1389 3.334948 0.502740 3.085408 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1810 1809 -1.065994 0.048578 -0.018228 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1810 1387 4.403848 0.191695 3.003360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1810 1388 3.272353 0.328659 2.992180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1811 1810 -1.101159 0.029829 -0.012800 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1811 1387 3.269925 0.179446 2.994842 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1811 1386 4.246371 -0.005709 2.994962 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1812 1811 -0.886184 0.029442 -0.007228 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1812 1386 3.349747 -0.005420 2.993578 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1812 1385 4.351856 -0.188928 2.994749 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1813 1812 -1.118899 -0.000430 -0.001917 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1813 597 4.340214 -1.443741 2.574703 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1813 1385 3.206219 -0.206859 2.985643 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1813 1384 4.145062 -0.374508 2.992063 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1813 596 4.652046 -1.525121 3.085923 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 1813 -0.929890 0.052110 -0.028620 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1814 598 3.249657 -1.282261 2.041547 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 1383 4.266644 -0.602121 2.980437 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 597 3.312778 -1.471190 2.559017 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 1384 3.134420 -0.399028 2.976377 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 596 3.623295 -1.557451 3.070237 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 593 4.859108 -0.628110 -2.996773 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1814 595 3.966553 -1.431214 -2.525463 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 1814 -1.002089 -0.021496 0.071729 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1815 1383 3.255589 -0.350072 3.048166 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 1382 4.239469 -0.695405 2.669616 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 592 4.310274 -0.400695 2.759576 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 596 2.678369 -1.346751 3.137966 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 593 3.848454 -0.335905 -2.929044 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1815 595 3.012296 -1.197573 -2.457734 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1816 1815 -1.009404 0.050084 -0.005577 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1816 591 3.518860 -0.622782 2.165959 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1816 1382 3.211370 -0.669904 2.664669 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1816 1381 4.061823 -1.176011 2.415499 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1816 592 3.283631 -0.375549 2.754629 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1817 1816 -1.043223 0.083288 -0.023362 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1817 589 2.364482 -2.550655 1.494936 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1817 601 0.221112 -0.375871 0.539419 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1817 1381 3.001744 -1.216023 2.399506 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1817 1379 2.963068 -2.454882 1.399656 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1817 1337 4.555827 0.072154 3.018056 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 1817 -1.050537 -0.028539 -0.007256 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1818 589 1.310146 -2.547636 1.496959 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 1336 4.524111 -0.048153 3.023528 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 602 0.140594 -0.153237 0.103467 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 603 1.136536 -0.066418 0.069435 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 1378 1.680903 -3.452407 1.293888 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1818 588 1.309753 -3.569488 1.624548 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1819 1818 -0.984414 0.072436 -0.006352 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1819 603 0.121369 -0.046025 0.066939 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1820 1819 -0.944533 0.049043 0.018767 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1820 1335 3.579839 -0.121779 3.034949 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1820 1334 4.677339 -0.246353 3.051689 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1820 604 0.134901 0.039897 0.034967 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1820 603 -0.870737 -0.014377 0.066925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1821 1820 -1.249497 0.065385 -0.011064 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1821 605 0.026400 0.034471 0.005925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1821 1334 3.449900 -0.239315 3.049769 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1821 1333 4.455135 -0.369830 3.061249 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1821 604 -1.091980 0.055657 0.033047 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1822 1821 -0.978151 0.002097 -0.009057 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1822 1333 3.417366 -0.402487 3.045727 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1822 606 0.085970 0.019602 -0.039040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1822 1332 4.414989 -0.506552 3.054017 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1823 1822 -0.946848 -0.007510 0.038390 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1823 606 -0.969072 0.029269 0.011967 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1823 1332 3.381142 -0.275488 3.105024 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1823 1331 4.427346 -0.335662 3.112844 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1823 607 0.076366 -0.011499 -0.020113 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1824 1823 -1.078987 0.070991 -0.014924 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1824 1330 4.474005 -0.474468 3.098813 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1824 609 1.094438 -0.199181 -0.106335 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1824 1331 3.354239 -0.397040 3.090523 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1824 607 -0.988422 0.024153 -0.042434 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1824 608 0.019164 -0.066619 -0.070347 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1825 1824 -1.057368 -0.067722 -0.017385 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1825 1330 3.408556 -0.503006 3.084531 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1825 610 0.986390 -0.264318 -0.032429 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1825 609 0.033266 -0.179481 -0.120617 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1825 608 -1.040005 -0.031576 -0.084629 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1825 1329 4.592334 -0.481573 -2.985689 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1826 1825 -0.990208 0.139774 -0.000929 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1826 1328 4.445063 -0.386609 -3.054402 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1826 610 -0.061174 -0.242978 -0.046782 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1826 609 -1.012983 -0.144471 -0.134970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1826 611 0.956363 -0.326787 -0.075931 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1826 1329 3.541281 -0.511965 -3.000042 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1827 1826 -0.977897 0.133779 -0.015050 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1827 1328 3.413661 -0.387594 -3.059038 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1827 610 -1.091862 -0.223073 -0.051418 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1827 1327 4.555059 -0.383525 3.102632 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1827 611 -0.074725 -0.311599 -0.080567 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1827 612 1.003562 -0.414084 -0.044816 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1828 1827 -0.990335 0.064368 -0.022831 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1828 1327 3.525142 -0.384752 3.092523 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1828 1326 4.453248 -0.460320 3.094363 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1828 612 -0.026482 -0.379409 -0.054925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1828 613 0.939896 -0.463209 -0.078008 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1829 1828 -0.951665 -0.152199 0.005457 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1829 1326 3.443671 -0.452634 3.096800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1829 614 0.927401 -0.557749 -0.109068 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1829 1325 4.486744 -0.492946 3.110000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1829 612 -1.036243 -0.382642 -0.052487 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1829 613 -0.069662 -0.464087 -0.075570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1830 1829 -1.128112 -0.014012 0.006637 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1830 614 -0.200612 -0.571338 -0.121097 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1830 1325 3.359254 -0.549355 3.097971 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1830 1324 4.525121 -0.625817 3.106401 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1830 615 0.904130 -0.683744 -0.021486 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1831 1830 -1.114551 0.073018 -0.039160 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1831 1324 3.374790 -0.624951 3.092656 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1831 616 0.854246 -0.726875 -0.065158 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1831 1323 4.337537 -0.704775 3.111446 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1831 615 -0.246655 -0.633103 -0.035231 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1832 1831 -1.001925 0.079813 -0.026890 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1832 616 -0.189430 -0.705599 -0.068329 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1832 1323 3.293912 -0.694545 3.108275 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1832 1322 4.323135 -0.725548 -3.139065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1832 617 0.807813 -0.826699 -0.100988 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1833 1832 -1.028913 0.010263 0.050544 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1833 1321 4.510036 -0.369003 -3.056849 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1833 616 -1.161791 -0.741046 0.005997 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1833 1322 3.339796 -0.425846 -3.064739 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1833 617 -0.158308 -0.787759 -0.026661 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1834 1833 -0.968148 0.018535 -0.006114 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1834 1321 3.489624 -0.400289 -3.067338 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1834 1320 4.513454 -0.340031 -3.090043 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1834 618 0.033826 -0.848056 -0.074967 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1834 617 -1.182856 -0.770054 -0.037151 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1835 1834 -1.047645 0.026456 0.003480 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1835 1320 3.484015 -0.276848 -3.090286 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1835 1319 4.520007 -0.325823 3.096369 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1835 619 -0.102781 -0.900946 -0.098253 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1835 620 0.909743 -1.052630 -0.119273 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1836 1835 -1.093029 0.075816 -0.008949 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1836 1317 5.420648 -0.492411 3.106817 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1836 1318 4.431267 -0.404528 3.097547 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1836 620 -0.262523 -1.017994 -0.129015 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1836 621 0.862644 -1.186729 -0.162195 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1837 1836 -1.126606 0.003934 -0.001207 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1837 1317 4.372173 -0.443081 3.113652 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1837 1316 5.415415 -0.492963 3.112412 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1837 621 -0.180978 -1.168538 -0.155360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1837 622 0.817708 -1.342119 -0.191993 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1838 1837 -1.009879 0.065433 -0.011395 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1838 1316 4.380418 -0.536633 3.099616 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1838 1315 5.403935 -0.605053 3.106486 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1838 623 0.801780 -1.345512 0.219538 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1838 622 -0.227778 -1.326886 -0.204789 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1839 1838 -1.082456 -0.085392 0.005943 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1839 624 0.801337 -1.159217 0.192231 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1839 623 -0.248093 -1.317575 0.209588 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1839 1314 5.388271 -0.691402 3.109466 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1840 1839 -1.077146 -0.010737 -0.008991 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1840 624 -0.239389 -1.124297 0.184436 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1840 625 0.795548 -0.961894 0.163025 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1841 1840 -0.993689 -0.044295 0.071366 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1841 625 -0.163222 -0.934368 0.242812 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1841 1313 4.405199 -0.330164 -3.115177 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1841 626 0.864046 -0.702777 0.219869 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1842 1841 -1.159578 0.019361 -0.002170 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1842 625 -1.208192 -0.908541 0.238199 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1842 1312 4.388454 -0.311993 -3.111751 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1842 627 0.826623 -0.474797 0.194552 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1842 1313 3.362968 -0.325419 -3.119791 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1842 626 -0.179866 -0.681692 0.215256 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1843 1842 -1.054952 0.123980 -0.006980 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1843 1312 3.336964 -0.322261 -3.117947 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1843 627 -0.225808 -0.462993 0.188356 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1843 626 -1.233560 -0.663648 0.209060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1843 628 0.828268 -0.304731 0.166823 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1843 1311 4.381723 -0.327364 -3.107937 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1844 1843 -1.141669 0.062309 -0.028622 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1844 629 0.801356 -0.165056 0.146431 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1844 628 -0.241704 -0.301144 0.164864 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1844 1311 3.311700 -0.330739 -3.109896 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1844 1310 4.334662 -0.312558 -3.096786 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1845 1844 -0.938859 -0.013791 -0.020594 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1845 629 -0.161755 -0.140211 0.141678 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1845 1309 4.395809 -0.286000 -3.090289 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1845 630 0.846234 -0.016377 0.119635 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1845 1310 3.370811 -0.304506 -3.101539 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1846 1845 -1.185781 0.046011 -0.021389 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1846 1308 4.316660 -0.283038 -3.086577 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1846 1309 3.254053 -0.319644 -3.104597 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1846 630 -0.291301 0.000737 0.105327 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1846 631 0.765665 -0.004169 -0.042941 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1847 1846 -1.090946 0.069686 -0.012022 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1847 1308 3.211032 -0.264280 -3.089120 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1847 631 -0.339242 0.023618 -0.045483 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1847 632 0.701668 -0.073698 -0.067139 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1847 1307 4.269090 -0.224182 -3.074360 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1848 1847 -0.891287 0.051842 -0.023544 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1848 1306 4.427946 -0.245159 -3.083570 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1848 632 -0.261250 -0.073203 -0.074000 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1848 1307 3.305055 -0.248159 -3.081220 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1848 633 0.827136 -0.190084 -0.104396 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1849 1848 -1.114048 -0.063099 -0.008613 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1849 634 0.640387 -0.280836 -0.042815 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1849 1306 3.261660 -0.224612 -3.084619 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1849 633 -0.339091 -0.165763 -0.105444 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1849 1305 4.362561 -0.151702 -3.076963 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1850 1849 -1.004990 -0.025148 -0.000253 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1850 634 -0.397885 -0.265656 -0.045702 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1850 1304 4.425409 -0.057921 -3.053650 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1850 1305 3.324646 -0.147266 -3.079850 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1850 635 0.617654 -0.353281 -0.059696 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1851 1850 -0.985247 0.039400 -0.009678 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1851 1303 4.423856 -0.000123 -3.043467 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1851 1304 3.455055 -0.069284 -3.059267 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1851 636 0.575479 -0.394215 0.011516 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1851 635 -0.354300 -0.343252 -0.065312 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1852 1851 -1.010193 0.019973 0.018213 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1852 1303 3.393393 -0.034158 -3.057920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1852 637 0.615180 -0.353516 0.013827 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1852 636 -0.460278 -0.372588 -0.002937 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1852 1302 4.582082 -0.065989 3.129420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1853 1852 -1.107647 0.120190 -0.011504 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1853 637 -0.534385 -0.325233 0.007490 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1853 638 0.578409 -0.348005 -0.009868 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1853 1302 3.434259 -0.062850 3.123083 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1853 1301 4.511495 -0.056142 -3.134032 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1854 1853 -1.071440 0.073320 -0.002161 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1854 639 0.650573 -0.383424 -0.029287 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1854 638 -0.433510 -0.338614 -0.009878 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1854 1301 3.499578 -0.046791 -3.134042 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1854 1300 4.528758 -0.039128 -3.123842 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1855 1854 -0.961292 -0.021506 0.002927 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1855 639 -0.313765 -0.357113 -0.030171 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1855 640 0.686893 -0.440088 -0.050508 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1855 1299 4.620204 -0.015375 -3.108860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1855 1300 3.564722 -0.016242 -3.124726 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1856 1855 -0.952062 0.059616 -0.008701 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1856 1298 4.638308 -0.041132 3.073289 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1856 640 -0.306699 -0.412438 -0.058319 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1856 1299 3.629809 -0.018460 -3.116671 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1856 641 0.783691 -0.489722 -0.082568 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1857 1856 -0.969758 0.072432 -0.007470 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1857 1298 3.673287 -0.055770 3.071842 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1857 1297 4.708687 -0.191340 2.994572 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1857 642 0.664954 -0.582649 -0.105743 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1857 641 -0.181974 -0.498784 -0.084015 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1858 1857 -1.213644 0.004235 -0.012156 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1858 1297 3.477507 -0.196170 2.980900 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1858 642 -0.571198 -0.532156 -0.119415 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1858 1296 4.572700 -0.387884 3.003310 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1858 643 0.535431 -0.666674 -0.047838 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1859 1858 -1.095937 0.063997 0.015473 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1859 1296 3.544294 -0.323435 3.019219 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1859 1295 4.568398 -0.465269 3.037379 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1859 643 -0.488029 -0.666418 -0.031929 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1859 644 0.630408 -0.737766 -0.046761 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1860 1859 -1.016029 -0.019649 0.087738 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1860 1295 3.547933 -0.127952 3.113005 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1860 1294 4.605397 -0.199500 3.125675 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1860 644 -0.358213 -0.697198 0.028865 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1860 645 0.716278 -0.704835 0.010773 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1861 1860 -1.032131 0.042753 -0.012851 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1861 1294 3.607492 -0.194370 3.122114 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1861 1293 4.759000 -0.238089 3.137864 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1861 646 0.723006 -0.660477 0.056764 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1861 645 -0.283402 -0.685856 0.007212 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1862 1861 -1.068631 0.133437 -0.009498 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1862 1293 3.739444 -0.192227 -3.141466 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1862 646 -0.294891 -0.630170 0.060619 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1862 647 0.632152 -0.585243 0.042572 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1862 1292 4.793493 -0.180090 -3.121476 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1863 1862 -1.083803 -0.024099 -0.027302 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1863 647 -0.428157 -0.574283 0.037996 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1863 648 0.607888 -0.562066 0.021517 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1863 1291 4.762912 -0.182362 -3.116742 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1863 1292 3.734994 -0.188176 -3.126052 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1864 1863 -1.042166 0.060214 -0.007448 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1864 648 -0.372833 -0.539903 0.022807 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1864 1291 3.781698 -0.154843 -3.115453 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1864 649 0.711635 -0.549283 0.062552 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1864 1290 4.755999 -0.146070 -3.063203 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 1864 -1.093378 -0.067949 0.012726 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1865 1289 4.646658 0.012987 -2.940728 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 1243 4.562871 0.561499 2.953392 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 650 0.685401 -0.445897 0.085723 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 1245 3.590195 0.293093 -2.261908 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 1244 4.033761 0.572988 -2.793778 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 649 -0.369165 -0.527768 0.064641 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1865 1290 3.674349 -0.116104 -3.061113 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 1865 -1.091050 0.043769 0.002945 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1866 1289 3.608758 0.050366 -2.933989 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 1243 3.521277 0.598300 2.960131 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 1242 4.654345 0.212060 3.030981 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 650 -0.349317 -0.435201 0.092461 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 651 0.670240 -0.367671 0.076665 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 1288 4.724414 0.256887 -2.915529 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1866 1244 2.992100 0.606225 -2.787039 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 1866 -1.027156 0.109803 -0.001367 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1867 1287 4.734547 0.396011 -3.085789 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 1241 4.657104 0.116582 3.085957 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 1242 3.658294 0.196587 3.024557 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 651 -0.329453 -0.357538 0.070241 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 1288 3.728650 0.240963 -2.921953 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1867 652 0.723100 -0.302867 0.053883 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 1867 -0.986103 0.020551 -0.000009 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1868 1287 3.701044 0.374456 -3.095340 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 1240 4.637684 0.036736 3.104875 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 1286 4.663957 0.407368 -3.019795 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 1248 0.327902 -0.648684 0.132206 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 653 0.784904 -0.281460 0.030936 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 1241 3.620935 0.095779 3.076405 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1868 652 -0.316895 -0.286074 0.044331 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1868 -1.054268 0.006030 -0.002957 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1869 1240 3.618700 0.070899 3.105462 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1286 3.644754 0.441547 -3.019208 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1249 0.304281 -0.488402 0.144389 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1248 -0.690679 -0.617051 0.132793 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1239 4.687487 -0.003031 3.126562 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 653 -0.233893 -0.249558 0.031523 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 654 0.825023 -0.264236 0.018347 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1869 1285 4.798741 0.570406 -2.991428 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1869 -1.066965 -0.069362 0.001714 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1870 1284 4.720710 0.688530 -3.058031 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1238 4.634212 -0.023040 -3.122587 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1250 0.236159 -0.348811 0.153906 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1249 -0.800049 -0.478787 0.141716 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1239 3.584439 -0.005135 3.123889 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 654 -0.278710 -0.256014 0.015674 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 655 0.732680 -0.208220 0.066531 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1870 1285 3.697225 0.568002 -2.994101 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 1870 -1.047064 0.041711 0.000378 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1871 1284 3.725834 0.695631 -3.060300 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 1237 4.682820 0.012354 -3.079375 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 1238 3.637722 -0.015742 -3.124855 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 1251 0.250945 -0.175592 0.161597 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 1250 -0.761058 -0.331535 0.151637 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 655 -0.264221 -0.192070 0.064262 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1871 656 0.787129 -0.163140 0.053230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 1871 -1.105516 0.070026 -0.000005 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1872 1236 4.776230 0.096485 -3.057047 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 1237 3.634132 0.045164 -3.076757 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 1252 0.196075 -0.005934 0.171040 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 657 0.766407 -0.094175 0.037663 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 1251 -0.797235 -0.154385 0.164215 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1872 656 -0.261086 -0.140529 0.055848 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 1872 -1.156481 0.075326 -0.005912 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1873 1280 5.657995 0.381732 3.040626 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 1281 4.595872 0.456066 3.092886 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 1235 4.665414 0.179046 -3.043554 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 1253 0.033767 0.115055 0.065506 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 1234 5.807902 0.294582 -3.012554 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1873 657 -0.381093 -0.074369 0.036561 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1873 -0.905062 -0.005708 0.012280 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1874 1254 0.109776 0.152535 -0.031253 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 659 0.852482 -0.048728 0.000973 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1279 5.662781 0.240882 3.023304 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1280 4.645845 0.417885 3.044364 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 658 -0.199408 -0.049909 0.021725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1253 -0.977347 0.130186 0.069244 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1234 4.796077 0.331296 -3.008816 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1874 1255 1.109681 0.048191 -0.127163 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 1874 -1.048635 0.042967 0.007316 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1875 1254 -0.925392 0.190900 -0.034651 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 1278 5.632226 0.152854 3.045686 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 659 -0.183374 -0.012885 -0.002425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 660 0.815324 -0.034975 -0.014049 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 1279 4.627881 0.260377 3.019906 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 1255 0.074154 0.083159 -0.130561 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1875 1256 1.106460 -0.110158 -0.168630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1875 -0.998288 0.011485 -0.000420 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1876 1278 4.626951 0.124270 3.045489 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1277 5.685940 0.044694 3.072049 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 660 -0.189987 -0.062609 -0.014246 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1279 3.622628 0.231991 3.019709 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1257 1.133634 -0.254461 0.055745 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1255 -0.931134 0.055671 -0.130758 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 661 0.785854 -0.134867 -0.028308 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1876 1256 0.101134 -0.137850 -0.168827 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1876 -1.022540 0.109373 -0.010113 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1877 1277 4.638022 0.058971 3.064266 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1257 0.083526 -0.204744 0.047962 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1258 1.098382 -0.101997 0.113795 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1276 5.731501 -0.041910 3.090266 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 661 -0.263313 -0.082447 -0.036091 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1256 -0.948035 -0.080101 -0.176610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 662 0.768115 -0.114211 -0.053742 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1877 1231 3.879680 2.035769 -1.819084 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 1877 -1.094210 -0.028228 0.017922 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1878 1230 2.903231 3.126258 -1.537579 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 1229 2.822348 4.131842 -1.493169 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 1258 0.045271 -0.094179 0.122951 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 1276 4.677646 0.008326 3.099421 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 662 -0.284870 -0.109416 -0.044587 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1878 1231 2.806880 2.068963 -1.809929 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1879 1878 -1.119875 0.113525 -0.140199 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1879 1260 0.956904 -0.098428 -0.006046 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1879 1259 -0.070266 -0.078503 -0.018863 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1879 663 -0.328275 -0.238223 -0.150442 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1879 664 0.649198 -0.463724 -0.217477 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 1879 -0.892918 -0.011669 0.129282 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1880 1260 -0.110026 0.010219 0.115273 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 1259 -1.132058 -0.094313 0.102456 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 1261 0.887166 0.106422 0.133653 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 665 0.672124 -0.459423 -0.009260 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 664 -0.371262 -0.389631 -0.096158 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1880 1274 4.520553 -0.411581 3.117014 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 1880 -1.015999 0.128752 -0.012619 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1881 1260 -1.136202 0.040180 0.110065 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 1261 -0.138522 0.131188 0.128444 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 665 -0.356507 -0.433529 -0.014469 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 664 -1.399516 -0.358304 -0.101366 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 666 0.604401 -0.489497 -0.024966 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1881 1274 3.492118 -0.405732 3.111805 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 1881 -1.140479 -0.035794 -0.006770 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1882 1262 -0.250480 0.112346 -0.298512 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 1271 4.506379 -0.073761 3.119355 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 1273 3.511139 -0.466060 -3.112880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 667 0.713490 -0.546811 -0.041720 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 666 -0.516838 -0.493335 -0.026286 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 1272 4.170625 -0.153636 -2.610605 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1882 1263 0.658304 -0.449267 -0.656833 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 1882 -1.151778 0.038835 0.005436 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1883 1270 3.647272 -0.206476 2.442784 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 1271 3.393257 -0.080368 3.109114 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 668 0.653675 -0.610135 -0.059880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 667 -0.404277 -0.514550 -0.051962 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 1264 0.481004 -0.977017 -0.190734 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 1272 3.056703 -0.156801 -2.620846 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1883 1263 -0.458461 -0.416447 -0.667074 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1884 1883 -0.984044 -0.030386 -0.003881 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1884 668 -0.442466 -0.584952 -0.062992 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1884 1265 0.420586 -0.992475 0.009521 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1884 669 0.611060 -0.685800 -0.070185 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1884 1264 -0.616278 -0.951294 -0.193846 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 1884 -1.260977 -0.042222 0.027179 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1885 1268 1.448281 -0.563468 1.182367 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 1267 1.213031 -0.779056 0.635979 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 1265 -0.751583 -0.955264 0.057566 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 669 -0.576057 -0.639795 -0.022140 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 1266 0.395245 -0.896588 0.076663 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1885 670 0.368820 -0.704176 -0.037282 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1886 1885 -1.114977 0.058132 -0.010544 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1886 1267 0.157610 -0.761887 0.636099 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1886 1266 -0.660162 -0.879518 0.076783 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1886 671 0.363337 -0.754126 -0.047990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1886 670 -0.686610 -0.687109 -0.037162 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1887 1886 -0.957757 0.101352 0.006791 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1887 671 -0.594545 -0.749631 -0.056563 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1887 673 0.880593 -1.050577 0.134341 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1887 672 0.440122 -0.832852 -0.193473 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1888 1887 -1.168162 0.083632 -0.019729 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1888 673 -0.252384 -0.996531 0.125986 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1888 672 -0.691020 -0.775134 -0.201828 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1888 674 0.916730 -0.879686 0.202371 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1889 1888 -0.932387 -0.064788 0.020377 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1889 674 -0.029215 -0.882623 0.218456 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1889 675 0.766023 -0.717592 0.064309 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1890 1889 -1.064866 -0.022727 -0.013162 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1890 674 -1.084036 -0.843328 0.205109 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1890 675 -0.286667 -0.688926 0.050962 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1890 676 0.830805 -0.661286 0.017710 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1891 1890 -0.976190 0.012763 0.006290 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1891 677 0.766245 -0.652394 0.079976 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1891 676 -0.252543 -0.661107 0.023046 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1892 1891 -1.217619 0.050490 -0.016764 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1892 678 0.747949 -0.570187 0.074434 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1892 677 -0.414693 -0.620350 0.072725 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1893 1892 -1.056648 -0.011124 0.009896 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1893 678 -0.300656 -0.558427 0.072297 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1893 679 0.680805 -0.459577 0.114847 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1894 1893 -0.925266 0.069471 0.003710 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1894 679 -0.229553 -0.403294 0.106643 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1894 680 0.852932 -0.306900 0.097985 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1895 1894 -1.172206 -0.048960 0.019736 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1895 680 -0.275875 -0.292860 0.101985 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1895 681 0.721132 -0.325610 -0.059434 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1896 1895 -1.069149 0.041033 0.000769 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1896 682 0.821397 -0.383271 -0.055749 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1896 681 -0.284383 -0.308062 -0.053534 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1897 1896 -1.042139 -0.006068 0.000068 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1897 682 -0.324055 -0.365330 -0.061090 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1897 683 0.773867 -0.441634 -0.064397 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1898 1897 -0.878673 -0.017371 -0.004833 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1898 684 0.740385 -0.528524 -0.036422 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1898 683 -0.179928 -0.417172 -0.061892 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1899 1898 -1.028682 0.027097 0.004560 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1899 684 -0.340226 -0.530157 -0.026069 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1899 685 0.791330 -0.505436 0.046733 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1900 1899 -0.993657 0.030217 -0.034197 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1900 685 -0.262314 -0.496610 0.026500 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1900 686 0.754801 -0.503076 0.025402 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1901 1900 -0.881614 -0.052650 0.060752 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1901 687 0.919692 -0.405843 0.072663 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1901 685 -1.183201 -0.538559 0.092544 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1901 686 -0.167877 -0.477885 0.091445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1902 1901 -1.124050 -0.072807 0.108278 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1902 687 -0.069049 -0.313545 0.189091 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1902 688 1.049079 -0.128310 0.181110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1903 1902 -1.009290 0.134415 -0.165385 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1903 688 -0.015554 -0.208520 0.028038 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1904 1903 -1.161463 0.107487 -0.004771 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1904 689 -0.087988 -0.117990 0.072114 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1904 690 0.923337 -0.099550 0.054219 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1905 1904 -1.125821 0.150483 -0.053825 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1905 691 0.804386 -0.069917 0.060888 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1905 689 -1.184458 -0.002176 0.022452 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1905 690 -0.173465 -0.033963 0.004557 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1906 1905 -0.965086 0.045065 -0.106801 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1906 691 -0.157497 -0.090783 -0.058418 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1906 692 0.905498 -0.168446 -0.049769 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1906 690 -1.124118 0.061303 -0.114750 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1907 1906 -1.056841 0.079932 0.052136 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1907 693 0.844173 -0.129605 -0.012434 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1907 692 -0.167873 -0.102204 -0.009163 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1908 1907 -1.064953 0.024004 0.003575 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1908 955 2.823073 -2.303864 1.554990 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1908 954 2.742760 -3.330034 1.432920 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1908 693 -0.191583 -0.127503 -0.005734 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1909 1908 -1.088673 -0.003306 0.019488 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1909 695 0.725769 -0.232779 -0.249853 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1909 694 -0.265372 -0.075331 -0.031656 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1909 954 1.633637 -3.236497 1.434217 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1910 1909 -0.274090 -0.187525 0.700046 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1910 695 0.410513 0.121697 0.455621 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1910 696 0.797383 0.185410 -0.058350 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1910 953 3.606740 -2.465882 2.164721 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1911 1910 -1.012537 -0.073029 0.193369 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1911 696 -0.215139 0.325472 0.136752 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1911 953 3.054915 -1.730879 2.359823 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1911 952 3.736761 -2.499426 2.323813 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1911 697 0.181933 0.282232 -0.400207 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1912 1911 -0.972922 -0.153903 0.476195 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1912 698 0.060286 -0.067116 -0.191410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1912 952 3.446674 -0.668200 2.809480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1912 697 -0.995560 0.132410 0.085460 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1912 951 4.395973 -1.005729 2.836510 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1912 699 1.085102 -0.290010 -0.239550 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1913 1912 -1.034651 -0.093019 0.256421 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1913 950 4.493845 -0.011044 3.041860 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1913 698 -0.979204 -0.098457 0.065960 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1913 951 3.452591 0.097443 3.093880 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1913 699 0.068593 -0.053155 0.017820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1914 1913 -0.941079 0.090697 -0.013321 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1914 950 3.460868 -0.052636 3.021630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1914 699 -0.964331 -0.005222 -0.002410 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1914 949 4.489547 -0.200647 3.053740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1914 700 0.042560 -0.065286 -0.023060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1915 1914 -1.062933 -0.023369 0.028642 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1915 949 3.456219 -0.105853 3.072930 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1915 701 0.019984 -0.080684 -0.049250 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1915 948 4.494188 -0.181622 -3.081895 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1915 702 1.070013 -0.167844 -0.081440 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1916 1915 -0.949309 -0.021002 0.045325 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1916 701 -0.994043 -0.087957 -0.005830 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1916 948 3.480325 0.005408 -3.038475 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1916 702 0.058779 -0.129458 -0.038020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1916 947 4.483724 0.102608 -2.999235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1917 1916 -1.068813 0.006470 0.029682 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1917 703 0.112408 -0.161160 -0.023190 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1917 946 4.418513 0.442213 -2.796885 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1917 947 3.401419 0.291215 -2.960775 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1918 1917 -1.172959 0.039516 0.001643 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1918 704 -0.085070 -0.124677 0.080970 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1918 705 0.954870 -0.071476 0.153180 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1918 944 4.502610 2.027346 -2.077905 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1918 945 3.909748 1.138148 -2.200235 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1919 1918 -1.110719 0.099363 0.016312 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1919 706 0.877851 0.272007 0.445390 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1919 704 -1.136412 -0.088686 0.105420 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1919 705 -0.098083 -0.010077 0.177630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1919 944 3.397286 2.174851 -2.053455 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1919 943 3.420999 2.579617 -1.539595 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1920 1919 -1.157003 0.112312 0.029019 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1920 706 -0.196181 0.323928 0.461560 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1920 705 -1.167426 0.026100 0.193800 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1920 943 2.309322 2.672357 -1.523425 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1920 942 2.158030 3.717890 -1.361060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1921 1920 -1.033333 0.127381 -0.146963 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1921 943 1.617938 2.426789 -1.676445 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1922 1921 -1.176250 0.219894 -0.337520 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1922 707 -0.981203 1.081263 -0.019480 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1922 942 1.691913 2.982532 -1.855600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 1922 -0.633702 0.168650 -0.581906 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1923 706 -1.683059 2.259040 -0.612198 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 707 -0.855031 1.680043 -0.598698 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 942 2.422773 1.808016 -2.434818 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 941 3.218812 2.427096 -2.426598 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 708 0.022969 1.126993 -0.306256 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1923 709 1.042954 1.003746 0.097048 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1924 1923 -0.502207 0.205412 -0.605261 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1924 941 3.513871 0.356430 -3.022154 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1924 940 4.529866 0.413252 -3.081470 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1924 709 0.914160 0.398719 -0.498508 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1925 1924 -0.561168 0.178063 -0.608104 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1926 1925 -0.509548 0.162315 -0.581787 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1927 1926 -0.555681 0.278880 -0.616677 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1927 703 5.139591 0.497042 2.807465 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1927 704 4.186771 0.783635 2.900815 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1927 1918 4.066641 0.692257 2.819845 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1927 946 0.855372 1.240060 0.033770 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1927 1917 5.197301 0.309218 2.830655 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 1927 -0.409320 0.172736 -0.617819 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1928 703 4.030278 -2.329752 2.197545 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 704 3.413419 -1.549056 2.290895 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 1918 3.262607 -1.555147 2.209925 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 946 0.944133 0.733304 -0.576150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 1917 3.969997 -2.516766 2.220735 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 944 -1.050364 0.849414 0.132020 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1928 945 0.016961 0.903976 0.009690 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1929 1928 -0.377850 0.068893 -0.582546 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1929 944 -0.858822 1.416860 -0.451820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1929 945 0.061779 0.874041 -0.574150 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1930 1929 -0.395655 0.121010 -0.609646 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1930 943 -0.678433 2.022152 -0.534605 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1930 1921 0.855553 -0.458555 1.141840 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1931 1930 -0.485011 0.102116 -0.588524 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1931 1920 -0.996134 -1.141870 0.395060 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1931 1921 -0.009660 -0.700936 0.548080 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1931 1922 0.816243 -0.072290 0.889600 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1932 1931 -0.390844 0.170644 -0.573659 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1932 1921 -0.786583 -0.390707 -0.028780 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1932 1922 0.248533 -0.314232 0.312740 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 1932 -0.530197 0.184125 -0.574959 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1933 707 -1.068699 1.061026 -0.299239 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 942 2.025478 2.150266 -2.135359 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 941 2.603459 2.976629 -2.127139 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 1923 0.243898 -0.292012 0.299459 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 708 -0.066622 0.791602 -0.006797 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1933 1922 -0.424206 -0.249133 -0.279759 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 1933 -0.276313 0.096180 -0.569458 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1934 706 -1.333314 2.376017 -0.869610 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 707 -0.679968 1.605299 -0.856110 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 942 2.522418 0.894596 -2.692230 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 941 3.449833 1.290624 -2.684010 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 1924 0.253188 -0.211304 0.338144 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 940 4.389445 1.681271 -2.743325 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 1923 -0.280812 -0.237063 -0.257412 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 708 0.028309 0.846951 -0.563668 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1934 709 0.983311 0.468097 -0.160364 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1935 1934 -0.230336 0.088319 -0.766036 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1935 1925 0.498354 -0.324085 0.186975 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1935 1924 -0.115284 -0.233394 -0.423299 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1936 1935 -0.117329 0.074918 -0.637817 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1936 1926 0.521017 -0.587120 0.154278 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1936 1925 -0.026736 -0.473029 -0.450024 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1937 1936 -0.735085 -0.115215 0.009682 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1937 1926 -0.227480 -0.711848 0.155698 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1938 1937 -0.469906 0.187922 -0.399888 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1938 703 4.179595 1.914407 -3.106820 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1938 1927 -0.435000 -0.402479 0.368900 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1938 1917 4.301150 1.760029 -3.083630 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1939 1938 -0.450090 -0.180581 0.405809 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1939 1926 -1.183040 -0.928966 0.163514 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1939 1937 -0.961130 -0.215361 0.007816 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 +EDGE2 1940 1939 -1.001252 0.004312 0.360007 20.000000 0.000000 20.000000 100000.000 0.000000 0.000000 +EDGE2 1940 1937 -1.811508 -0.546236 0.365244 100.000000 0.000000 100.000000 1000000.000 0.000000 0.000000 diff --git a/rtb-data/rtbdata/data/piano.png b/rtb-data/rtbdata/data/piano.png new file mode 100644 index 000000000..174d7dca4 Binary files /dev/null and b/rtb-data/rtbdata/data/piano.png differ diff --git a/rtb-data/rtbdata/data/queensland.json b/rtb-data/rtbdata/data/queensland.json new file mode 100644 index 000000000..3dc055702 --- /dev/null +++ b/rtb-data/rtbdata/data/queensland.json @@ -0,0 +1 @@ +{"places": {"Brisbane": {"lat": -27.4697707, "lon": 153.0251235, "utm": [1095.9145867144744, 6947.043658018596]}, "Camooweal": {"lat": -19.920667, "lon": 138.1213324, "utm": [-432.103885525028, 7772.551709375787]}, "Mount Isa": {"lat": -20.7255748, "lon": 139.4927085, "utm": [-283.3325926415254, 7689.989162769402]}, "Cloncurry": {"lat": -20.7063927, "lon": 140.5093258, "utm": [-176.971789275437, 7696.747610678524]}, "Hughenden": {"lat": -20.85, "lon": 144.2, "utm": [208.62239381884905, 7691.917660932694]}, "Charters Towers": {"lat": -20.0769637, "lon": 146.2601362, "utm": [422.6424087047931, 7779.8303889114595]}, "Townsville": {"lat": -19.2589635, "lon": 146.8169483, "utm": [480.763802457357, 7870.508270739987]}, "Boulia": {"lat": -22.911306, "lon": 139.910444, "utm": [-228.32608073938195, 7448.695027567425]}, "Winton": {"lat": -22.3913, "lon": 143.0381, "utm": [91.94835708401399, 7518.4805284370905]}, "Bedourie": {"lat": -24.3322136, "lon": 139.4619731, "utm": [-266.1888714830168, 7288.1442884511625]}, "Birdsville": {"lat": -25.898889, "lon": 139.351667, "utm": [-267.4995506033434, 7113.050218941856]}, "Windorah": {"lat": -25.420556, "lon": 142.654722, "utm": [62.757101391285715, 7181.357095348933]}, "Longreach": {"lat": -23.4421917, "lon": 144.2555041, "utm": [219.59058651533368, 7404.855248041811]}, "Barcaldine": {"lat": -23.5563211, "lon": 145.293339, "utm": [325.80609588169665, 7393.856437148137]}, "Blackall": {"lat": -24.416667, "lon": 145.466667, "utm": [344.536995644389, 7298.781327301602]}, "Charleville": {"lat": -26.4020616, "lon": 146.2453597, "utm": [424.73496995957424, 7079.568728198133]}, "Roma": {"lat": -26.5694477, "lon": 148.7837619, "utm": [677.6626252021844, 7060.013692193939]}, "Emerald": {"lat": -23.527291, "lon": 148.164573, "utm": [618.884226692918, 7397.6249803609235]}, "Mackay": {"lat": -21.1424956, "lon": 149.1821469, "utm": [726.6128025478556, 7660.524831465281]}, "Rockhampton": {"lat": -23.3790772, "lon": 150.510016, "utm": [858.8543105393574, 7410.150034537044]}}, "routes": [{"start": "Camooweal", "end": "Mount Isa", "distance": 188, "speed": 100}, {"start": "Mount Isa", "end": "Cloncurry", "distance": 118, "speed": 100}, {"start": "Cloncurry", "end": "Hughenden", "distance": 401, "speed": 100}, {"start": "Cloncurry", "end": "Winton", "distance": 350, "speed": 100}, {"start": "Hughenden", "end": "Charters Towers", "distance": 248, "speed": 100}, {"start": "Charters Towers", "end": "Townsville", "distance": 135, "speed": 100}, {"start": "Townsville", "end": "Mackay", "distance": 388, "speed": 100}, {"start": "Mackay", "end": "Rockhampton", "distance": 334, "speed": 100}, {"start": "Mackay", "end": "Emerald", "distance": 384, "speed": 100}, {"start": "Rockhampton", "end": "Brisbane", "distance": 682, "speed": 100}, {"start": "Rockhampton", "end": "Emerald", "distance": 270, "speed": 100}, {"start": "Brisbane", "end": "Roma", "distance": 482, "speed": 100}, {"start": "Roma", "end": "Charleville", "distance": 266, "speed": 100}, {"start": "Charleville", "end": "Blackall", "distance": 305, "speed": 100}, {"start": "Blackall", "end": "Barcaldine", "distance": 106, "speed": 100}, {"start": "Blackall", "end": "Windorah", "distance": 530, "speed": 50}, {"start": "Barcaldine", "end": "Emerald", "distance": 307, "speed": 100}, {"start": "Barcaldine", "end": "Hughenden", "distance": 500, "speed": 50}, {"start": "Barcaldine", "end": "Longreach", "distance": 106, "speed": 100}, {"start": "Longreach", "end": "Windorah", "distance": 311, "speed": 50}, {"start": "Longreach", "end": "Winton", "distance": 180, "speed": 100}, {"start": "Winton", "end": "Hughenden", "distance": 216, "speed": 100}, {"start": "Winton", "end": "Boulia", "distance": 363, "speed": 50}, {"start": "Winton", "end": "Windorah", "distance": 487, "speed": 50}, {"start": "Boulia", "end": "Mount Isa", "distance": 304, "speed": 50}, {"start": "Boulia", "end": "Bedourie", "distance": 194, "speed": 50}, {"start": "Bedourie", "end": "Birdsville", "distance": 193, "speed": 50}, {"start": "Birdsville", "end": "Windorah", "distance": 380, "speed": 50}, {"start": "Bedourie", "end": "Windorah", "distance": 411, "speed": 50}]} \ No newline at end of file diff --git a/rtb-data/rtbdata/xacro/kortex_description/robots/gen3.xacro b/rtb-data/rtbdata/xacro/kortex_description/robots/gen3.xacro index 163b8ee74..03e34ea97 100644 --- a/rtb-data/rtbdata/xacro/kortex_description/robots/gen3.xacro +++ b/rtb-data/rtbdata/xacro/kortex_description/robots/gen3.xacro @@ -5,7 +5,7 @@ - + diff --git a/setup.py b/setup.py index dd558f1a5..3de45fce7 100644 --- a/setup.py +++ b/setup.py @@ -43,7 +43,4 @@ def package_files(directory): setup( ext_modules=[frne, fknm], package_data={"roboticstoolbox": extra_files}, - # scripts=[ - # "roboticstoolbox/bin/rtbtool", - # ], ) diff --git a/tests/test_BaseRobot.py b/tests/test_BaseRobot.py new file mode 100644 index 000000000..937b69dbd --- /dev/null +++ b/tests/test_BaseRobot.py @@ -0,0 +1,631 @@ +""" +Created on Fri May 1 14:04:04 2020 +@author: Jesse Haviland +""" + +import numpy.testing as nt +import numpy as np +import roboticstoolbox as rtb +from roboticstoolbox import Link, ETS, ET, Robot +from spatialmath import SE3 +import unittest +from copy import deepcopy +from swift import Swift + +from roboticstoolbox.robot.Robot import BaseRobot + + +class TestBaseRobot(unittest.TestCase): + def test_init(self): + + links, name, urdf_string, urdf_filepath = rtb.Robot.URDF_read( + "franka_description/robots/panda_arm_hand.urdf.xacro" + ) + + robot = rtb.Robot( + links, + name=name, + manufacturer="Franka Emika", + gripper_links=links[9], + urdf_string=urdf_string, + urdf_filepath=urdf_filepath, + ) + + robot.grippers[0].tool = SE3(0, 0, 0.1034) + + def test_init2(self): + ets = rtb.ETS(rtb.ET.Rz()) + robot = rtb.Robot( + ets, name="myname", manufacturer="I made it", comment="other stuff" + ) + self.assertEqual(robot.name, "myname") + self.assertEqual(robot.manufacturer, "I made it") + self.assertEqual(robot.comment, "other stuff") + + def test_init3(self): + l0 = Link() + # l1 = Link(parent=l0) + # r = Robot([l0, l1], base=SE3.Rx(1.3)) + # r.base_link = l1 + + with self.assertRaises(TypeError): + Robot(l0, base=SE3.Rx(1.3)) # type: ignore + + with self.assertRaises(TypeError): + Robot([1, 2], base=SE3.Rx(1.3)) # type: ignore + + def test_init4(self): + ets = ETS(rtb.ET.Rz()) + robot = Robot( + ets, name="myname", manufacturer="I made it", comment="other stuff" + ) + self.assertEqual(robot.name, "myname") + self.assertEqual(robot.manufacturer, "I made it") + self.assertEqual(robot.comment, "other stuff") + + def test_init5(self): + + base = SE3.Trans(0, 0, 0.1).A + ets = ETS(rtb.ET.Rz()) + robot = Robot(ets, base=base, tool=base) + nt.assert_almost_equal(robot.base.A, base) + nt.assert_almost_equal(robot.tool.A, base) + + def test_init6(self): + + base = SE3.Trans(0, 0, 0.1) + ets = ETS(rtb.ET.Rz()) + robot = Robot(ets, base=base, tool=base) + nt.assert_almost_equal(robot.base.A, base.A) + nt.assert_almost_equal(robot.tool.A, base.A) + + def test_init7(self): + + keywords = 2 + ets = ETS(rtb.ET.Rz()) + + with self.assertRaises(TypeError): + Robot(ets, keywords=keywords) # type: ignore + + def test_init8(self): + + links = [2, 3, 4, 5] + + with self.assertRaises(TypeError): + BaseRobot(links=links) # type: ignore + + def test_init9(self): + + robot = rtb.models.Panda() + robot2 = rtb.models.PR2() + + self.assertTrue(robot2._hasdynamics) + self.assertTrue(robot._hasgeometry) + self.assertTrue(robot._hascollision) + + def test_init10(self): + + links = [Link(name="link1"), Link(name="link1"), Link(name="link1")] + + with self.assertRaises(ValueError): + Robot(links) + + def test_init11(self): + + l1 = Link(parent="l3") + l2 = Link(parent=l1) + l3 = Link(parent=l2, name="l3") + + links = [l1, l2, l3] + + with self.assertRaises(ValueError): + Robot(links) + + def test_init12(self): + + l1 = Link(jindex=1, ets=rtb.ET.Rz()) + l2 = Link(jindex=2, parent=l1, ets=rtb.ET.Rz()) + l3 = Link(parent=l2, ets=rtb.ET.Rz()) + + links = [l1, l2, l3] + + with self.assertRaises(ValueError): + Robot(links) + + def test_iter(self): + robot = rtb.models.Panda() + for link in robot: + self.assertIsInstance(link, Link) + + def test_get(self): + panda = rtb.models.ETS.Panda() + self.assertIsInstance(panda[1], Link) + self.assertIsInstance(panda["link0"], Link) + + def test_init_ets(self): + + ets = ( + rtb.ET.tx(-0.0825) + * rtb.ET.Rz() + * rtb.ET.tx(-0.0825) + * rtb.ET.tz() + * rtb.ET.tx(0.1) + ) + + robot = rtb.Robot(ets) + + self.assertEqual(robot.n, 2) + self.assertIsInstance(robot.links[0], rtb.Link) + self.assertIsInstance(robot.links[1], rtb.Link) + self.assertTrue(robot.links[0].isrevolute) + self.assertTrue(robot.links[1].isprismatic) + + self.assertIs(robot.links[0].parent, None) + self.assertIs(robot.links[1].parent, robot.links[0]) + self.assertIs(robot.links[2].parent, robot.links[1]) + + self.assertEqual(robot.links[0].children, [robot.links[1]]) + self.assertEqual(robot.links[1].children, [robot.links[2]]) + self.assertEqual(robot.links[2].children, []) + + def test_init_elink(self): + link1 = Link(ETS(ET.Rx()), name="link1") + link2 = Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2", parent=link1) + link3 = Link(ETS(ET.tx(1)), name="ee_1", parent=link2) + robot = Robot([link1, link2, link3]) + self.assertEqual(robot.n, 2) + self.assertIsInstance(robot.links[0], Link) + self.assertIsInstance(robot.links[1], Link) + self.assertIsInstance(robot.links[2], Link) + self.assertTrue(robot.links[0].isrevolute) + self.assertTrue(robot.links[1].isprismatic) + + self.assertFalse(robot.links[2].isrevolute) + self.assertFalse(robot.links[2].isprismatic) + + self.assertIs(robot.links[0].parent, None) + self.assertIs(robot.links[1].parent, robot.links[0]) + self.assertIs(robot.links[2].parent, robot.links[1]) + + self.assertEqual(robot.links[0].children, [robot.links[1]]) + self.assertEqual(robot.links[1].children, [robot.links[2]]) + self.assertEqual(robot.links[2].children, []) + + link1 = Link(ETS(ET.Rx()), name="link1") + link2 = Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2", parent="link1") + link3 = Link(ETS(ET.tx(1)), name="ee_1", parent="link2") + robot = Robot([link1, link2, link3]) + self.assertEqual(robot.n, 2) + self.assertIsInstance(robot.links[0], Link) + self.assertIsInstance(robot.links[1], Link) + self.assertIsInstance(robot.links[2], Link) + self.assertTrue(robot.links[0].isrevolute) + self.assertTrue(robot.links[1].isprismatic) + + self.assertIs(robot.links[0].parent, None) + self.assertIs(robot.links[1].parent, robot.links[0]) + self.assertIs(robot.links[2].parent, robot.links[1]) + + self.assertEqual(robot[0].children, [robot[1]]) + self.assertEqual(robot[1].children, [robot[2]]) + self.assertEqual(robot[2].children, []) + + def test_init_elink_autoparent(self): + links = [ + Link(ETS(ET.Rx()), name="link1"), + Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2"), + Link(ETS(ET.tx(1)), name="ee_1"), + ] + robot = Robot(links) + self.assertEqual(robot.n, 2) + self.assertIsInstance(robot[0], Link) + self.assertIsInstance(robot[1], Link) + self.assertIsInstance(robot[2], Link) + self.assertTrue(robot[0].isrevolute) + self.assertTrue(robot[1].isprismatic) + self.assertIs(robot[0].parent, None) + self.assertIs(robot[1].parent, robot[0]) + self.assertIs(robot[2].parent, robot[1]) + + self.assertEqual(robot[0].children, [robot[1]]) + self.assertEqual(robot[1].children, [robot[2]]) + self.assertEqual(robot[2].children, []) + + def test_init_elink_branched(self): + robot = Robot( + [ + Link(ETS(ET.Rz()), name="link1"), + Link( + ETS(ET.tx(1)) * ET.ty(-0.5) * ET.Rz(), name="link2", parent="link1" + ), + Link(ETS(ET.tx(1)), name="ee_1", parent="link2"), + Link(ET.tx(1) * ET.ty(0.5) * ET.Rz(), name="link3", parent="link1"), + Link(ETS(ET.tx(1)), name="ee_2", parent="link3"), + ] + ) + self.assertEqual(robot.n, 3) + for i in range(5): + self.assertIsInstance(robot[i], Link) + self.assertTrue(robot[0].isrevolute) + self.assertTrue(robot[1].isrevolute) + self.assertTrue(robot[3].isrevolute) + + self.assertIs(robot[0].parent, None) + self.assertIs(robot[1].parent, robot[0]) + self.assertIs(robot[2].parent, robot[1]) + self.assertIs(robot[3].parent, robot[0]) + self.assertIs(robot[4].parent, robot[3]) + + self.assertEqual(robot[0].children, [robot[1], robot[3]]) + self.assertEqual(robot[1].children, [robot[2]]) + self.assertEqual(robot[2].children, []) + self.assertEqual(robot[3].children, [robot[4]]) + self.assertEqual(robot[2].children, []) + + def test_init_bases(self): + e1 = Link() + e2 = Link() + e3 = Link(parent=e1) + e4 = Link(parent=e2) + + with self.assertRaises(ValueError): + Robot([e1, e2, e3, e4]) + + def test_jindex(self): + e1 = Link(ETS(ET.Rz()), jindex=0) + e2 = Link(ETS(ET.Rz()), jindex=1, parent=e1) + e3 = Link(ETS(ET.Rz()), jindex=2, parent=e2) + e4 = Link(ETS(ET.Rz()), jindex=0, parent=e3) + + # with self.assertRaises(ValueError): + Robot([e1, e2, e3, e4], gripper_links=e4) + + def test_jindex_fail(self): + e1 = Link(rtb.ETS(rtb.ET.Rz()), jindex=0) + e2 = Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e1) + e3 = Link(rtb.ETS(rtb.ET.Rz()), jindex=2, parent=e2) + e4 = Link(rtb.ETS(rtb.ET.Rz()), jindex=5, parent=e3) + + with self.assertRaises(ValueError): + Robot([e1, e2, e3, e4]) + + e1 = Link(rtb.ETS(rtb.ET.Rz()), jindex=0) + e2 = Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e1) + e3 = Link(rtb.ETS(rtb.ET.Rz()), jindex=2, parent=e2) + e4 = Link(rtb.ETS(rtb.ET.Rz()), parent=e3) + + with self.assertRaises(ValueError): + Robot([e1, e2, e3, e4]) + + def test_panda(self): + panda = rtb.models.ETS.Panda() + qz = np.array([0, 0, 0, 0, 0, 0, 0]) + qr = panda.qr + + nt.assert_array_almost_equal(panda.qr, qr) + nt.assert_array_almost_equal(panda.qz, qz) + nt.assert_array_almost_equal(panda.gravity, np.array([0, 0, -9.81])) + + def test_q(self): + panda = rtb.models.ETS.Panda() + + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + q3 = np.expand_dims(q1, 0) + + panda.q = q1 + nt.assert_array_almost_equal(panda.q, q1) + panda.q = q2 + nt.assert_array_almost_equal(panda.q, q2) + panda.q = q3 + nt.assert_array_almost_equal(np.expand_dims(panda.q, 0), q3) + + def test_getters(self): + panda = rtb.models.ETS.Panda() + + panda.qdd = np.ones((7, 1)) + panda.qd = np.ones((1, 7)) + panda.qdd = panda.qd + panda.qd = panda.qdd + + def test_control_mode(self): + panda = rtb.models.ETS.Panda() + panda.control_mode = "v" + self.assertEqual(panda.control_mode, "v") + + def test_base(self): + panda = rtb.models.ETS.Panda() + + pose = SE3() + + panda.base = pose.A + nt.assert_array_almost_equal(np.eye(4), panda.base.A) + + panda.base = pose + nt.assert_array_almost_equal(np.eye(4), panda.base.A) + + def test_control_mode2(self): + panda = rtb.models.ETS.Panda() + + panda.control_mode = "p" + + with self.assertRaises(ValueError): + panda.control_mode = "z" + + def test_manuf(self): + panda = rtb.models.ETS.Panda() + + self.assertIsInstance(panda.manufacturer, str) + + def test_str(self): + panda = rtb.models.Panda() + pr2 = rtb.models.PR2() + self.assertIsInstance(str(panda), str) + self.assertIsInstance(str(pr2), str) + self.assertIsInstance(repr(panda), str) + self.assertIsInstance(repr(pr2), str) + + def test_nlinks(self): + panda = rtb.models.Panda() + self.assertEqual(panda.nlinks, 12) + + def test_configs(self): + panda = rtb.models.Panda() + configs = panda.configs + + nt.assert_equal(configs["qr"], panda.qr) + nt.assert_equal(configs["qz"], panda.qz) + + def test_keywords(self): + panda = Robot( + ETS([ET.Rz(qlim=[-1, 1]), ET.tz(qlim=[-1, 1]), ET.SE3(SE3.Trans(1, 2, 3))]), + keywords=["test"], + ) + self.assertEqual(panda.keywords, ["test"]) + self.assertFalse(panda.symbolic) + self.assertFalse(panda.hasdynamics) + self.assertFalse(panda.hasgeometry) + self.assertFalse(panda.hascollision) + self.assertEqual(panda.default_backend, None) + panda.default_backend = "Swift" + + self.assertEqual(panda.qlim[0, 0], -1.0) + + def test_qlim(self): + panda = Robot(ETS([ET.Rz(qlim=[-1, 1]), ET.tz()]), keywords=["test"]) + + with self.assertRaises(ValueError): + panda.qlim + + def test_joint_types(self): + panda = Robot( + ETS([ET.Rz(qlim=[-1, 1]), ET.tz(qlim=[-1, 1]), ET.SE3(SE3.Trans(1, 2, 3))]), + ) + + self.assertTrue(panda.prismaticjoints[1]) + self.assertTrue(panda.revolutejoints[0]) + + def test_urdf_string(self): + panda = rtb.models.Panda() + self.assertIsInstance(panda.urdf_string, str) + self.assertIsInstance(panda.urdf_filepath, str) + + def test_manipulability_fail(self): + puma = rtb.models.Puma560() + puma.q = puma.qr + + with self.assertRaises(ValueError): + puma.manipulability(method="notamethod") # type: ignore + + def test_tool(self): + panda = rtb.models.Panda() + + panda.tool = SE3.Ry(0.5) + nt.assert_almost_equal(panda.tool.A, SE3.Ry(0.5).A) + + panda.tool = SE3.Ry(0.5).A + nt.assert_almost_equal(panda.tool.A, SE3.Ry(0.5).A) + + def test_get_path(self): + panda = rtb.models.Panda() + + links, n, tool = panda.get_path(end="panda_link1", start="panda_link4") + + for link in links: + print(link.name) + + self.assertEqual(links[0].name, "panda_link4") + self.assertEqual(links[1].name, "panda_link3") + self.assertEqual(links[2].name, "panda_link2") + self.assertEqual(links[3].name, "panda_link1") + + self.assertEqual(n, 4) + nt.assert_equal(tool, np.eye(4)) + + def test_get_path2(self): + panda = rtb.models.Panda() + + links, n, tool = panda.get_path(end="panda_link4", start="panda_link1") + + for link in links: + print(link.name) + + self.assertEqual(links[0].name, "panda_link1") + self.assertEqual(links[1].name, "panda_link2") + self.assertEqual(links[2].name, "panda_link3") + self.assertEqual(links[3].name, "panda_link4") + + self.assertEqual(n, 4) + nt.assert_equal(tool, np.eye(4)) + + def test_get_path_fail(self): + panda = rtb.models.Panda() + + with self.assertRaises(ValueError): + _, _, _ = panda.get_path(end="panda", start="panda") + + def test_getlink(self): + panda = rtb.models.Panda() + + l0 = panda._getlink(panda.grippers[0]) + + self.assertEqual(l0.name, "panda_hand") + + def test_getlink_fail(self): + panda = rtb.models.Panda() + + with self.assertRaises(ValueError): + panda._getlink(rtb.Link(rtb.ETS())) + + def test_getlink_fail2(self): + panda = rtb.models.Panda() + + with self.assertRaises(ValueError): + panda._getlink(rtb.Gripper([rtb.Link(rtb.ETS())])) + + def test_getlink_fail3(self): + panda = rtb.models.Panda() + + with self.assertRaises(TypeError): + panda._getlink(2.0) + + def test_limits(self): + r = rtb.models.YuMi() + + end, _, _ = r._get_limit_links() + + self.assertEqual(end.name, "r_gripper") + + def test_limits2(self): + r = rtb.models.Panda() + + end, _, _ = r._get_limit_links() + + self.assertEqual(end.name, "panda_hand") + + def test_limits3(self): + l1 = rtb.Link(rtb.ETS(), name="l1") + l2 = rtb.Link(rtb.ETS(), parent=l1, name="l2") + l3 = rtb.Link(rtb.ETS(), parent=l1, name="l3") + + r = rtb.Robot([l1, l2, l3]) + + end, _, _ = r._get_limit_links() + + self.assertEqual(end.name, "l2") + + def test_limits4(self): + r = rtb.models.Panda() + + end, _, _ = r._get_limit_links(end=r.grippers[0]) + + self.assertEqual(end.name, "panda_hand") + + def test_ets_gripper(self): + r = rtb.models.Panda() + + ets = r.ets(start=r.grippers[0]) + + self.assertEqual(ets.n, 0) + + def test_ets_gripper2(self): + r = rtb.models.YuMi() + + ets = r.ets() + + self.assertEqual(ets.n, 7) + + def test_ets_gripper3(self): + l1 = rtb.Link(rtb.ETS(), name="l1") + l2 = rtb.Link(rtb.ETS(), parent=l1, name="l2") + l3 = rtb.Link(rtb.ETS(), parent=l1, name="l3") + + r = rtb.Robot([l1, l2, l3]) + + ets = r.ets() + + self.assertEqual(ets.n, 0) + + def test_ets_gripper4(self): + r = rtb.models.Panda() + + ets = r.ets(end=r.grippers[0]) + + self.assertEqual(ets.n, 7) + + def test_ets(self): + r = rtb.models.Panda() + + ets = r.ets(end=r.links[4], start=r.links[4]) + + self.assertEqual(ets.n, 1) + + def test_copy(self): + r = rtb.models.Panda() + + r2 = deepcopy(r) + + self.assertTrue(r2.links[2] != r.links[2]) + + def test_copy2(self): + r = rtb.models.DH.Panda() + + r2 = deepcopy(r) + + self.assertTrue(r2.links[2] != r.links[2]) + + def test_copy3(self): + r = rtb.Robot2(rtb.ETS2(rtb.ET2.R())) + + r2 = deepcopy(r) + + self.assertTrue(r2.links[0] != r.links[0]) + + def test_toradians(self): + r = rtb.models.Panda() + + q = np.ones((2, 7)) + + qn = r.toradians(q) + + nt.assert_equal(qn, q * np.pi / 180.0) + + def test_todegrees(self): + r = rtb.models.Panda() + + q = np.ones((2, 7)) + + qn = r.todegrees(q) + + nt.assert_equal(qn, q * 180.0 / np.pi) + + def test_random_q(self): + r = rtb.models.Panda() + + q = r.random_q() + + self.assertEqual(q.shape[0], 7) + + def test_heirarchy(self): + r = rtb.models.Panda() + + r.hierarchy() + + def test_segments2(self): + r = rtb.models.YuMi() + + segs = r.segments() + + self.assertEqual(len(segs), 7) + + def test_get_backend(self): + r = rtb.models.Panda() + + be = r._get_graphical_backend() + + self.assertTrue(isinstance(be, Swift)) + + def test_teach(self): + robot = rtb.models.ETS.Panda() + e = robot.teach(q=None, block=False, vellipse=True, fellipse=True) + e.close() diff --git a/tests/test_CustomXacro.py b/tests/test_CustomXacro.py index 3f21344a0..61aee8189 100755 --- a/tests/test_CustomXacro.py +++ b/tests/test_CustomXacro.py @@ -6,7 +6,7 @@ import numpy.testing as nt import numpy as np import unittest -from roboticstoolbox import ERobot +from roboticstoolbox import Robot from spatialmath import SE3 from roboticstoolbox.tools.data import rtb_path_to_datafile from distutils.dir_util import copy_tree @@ -16,7 +16,7 @@ class TestCustomXacro(unittest.TestCase): def test_custom(self): - class CustomPanda(ERobot): + class CustomPanda(Robot): def __init__(self, xacro_path): links, name, urdf_string, urdf_filepath = self.URDF_read( diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index b954dd161..8144f00ec 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -629,7 +629,7 @@ def test_friction(self): l0 = rp.RevoluteDH(d=2, B=3, G=2, Tc=[2, -1]) qd = [1, 2, 3, 4] - r0 = rp.DHRobot([l0, l0, l0, l0]) + r0 = rp.DHRobot([l0, l0.copy(), l0.copy(), l0.copy()]) tau = np.array([-16, -28, -40, -52]) @@ -969,16 +969,16 @@ def test_ikine_LM(self): sol = puma.ikine_LM(T) self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=6) + self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=4) - def test_ikine_LMS(self): - puma = rp.models.DH.Puma560() + # def test_ikine_LMS(self): + # puma = rp.models.DH.Puma560() - T = puma.fkine(puma.qn) + # T = puma.fkine(puma.qn) - sol = puma.ikine_LM(T) - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=6) + # sol = puma.ikine_LM(T) + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=6) # def test_ikine_unc(self): # puma = rp.models.DH.Puma560() @@ -1403,7 +1403,7 @@ def test_perturb(self): def test_teach(self): panda = rp.models.DH.Panda() - e = panda.teach(block=False) + e = panda.teach(panda.q, block=False) e.close() def test_teach_withq(self): @@ -1418,8 +1418,8 @@ def test_plot(self): def test_teach_basic(self): l0 = rp.DHLink(d=2) - r0 = rp.DHRobot([l0, l0]) - e = r0.teach(block=False) + r0 = rp.DHRobot([l0, l0.copy()]) + e = r0.teach(r0.q, block=False) e.step() e.close() @@ -1440,35 +1440,23 @@ def test_control_type(self): def test_plot_vellipse(self): panda = rp.models.DH.Panda() - e = panda.plot_vellipse(block=False, limits=[1, 2, 1, 2, 1, 2]) + e = panda.plot_vellipse(panda.q, block=False, limits=[1, 2, 1, 2, 1, 2]) e.close() - e = panda.plot_vellipse(block=False, q=panda.qr, centre="ee", opt="rot") + e = panda.plot_vellipse(panda.q, block=False, centre="ee", opt="rot") e.step() e.close() - with self.assertRaises(TypeError): - panda.plot_vellipse(q=panda.qr, vellipse=10) - - with self.assertRaises(ValueError): - panda.plot_vellipse(q=panda.qr, centre="ff") - def test_plot_fellipse(self): panda = rp.models.DH.Panda() e = panda.plot_fellipse(q=panda.qr, block=False, limits=[1, 2, 1, 2, 1, 2]) e.close() - e = panda.plot_fellipse(block=False, q=panda.qr, centre="ee", opt="rot") + e = panda.plot_fellipse(panda.qr, block=False, centre="ee", opt="rot") e.step() e.close() - with self.assertRaises(TypeError): - panda.plot_fellipse(q=panda.qr, fellipse=10) - - with self.assertRaises(ValueError): - panda.plot_fellipse(q=panda.qr, centre="ff") - def test_plot_with_vellipse(self): panda = rp.models.DH.Panda() e = panda.plot(panda.qr, block=False, vellipse=True, backend="pyplot") diff --git a/tests/test_ELink.py b/tests/test_ELink.py index 2415b136f..4710bd9ac 100644 --- a/tests/test_ELink.py +++ b/tests/test_ELink.py @@ -10,6 +10,7 @@ import spatialgeometry as gm import unittest import spatialmath as sm +from roboticstoolbox.robot.Link import BaseLink class TestLink(unittest.TestCase): @@ -86,6 +87,13 @@ def test_Tc(self): nt.assert_array_almost_equal(l1.Tc, Tc1) nt.assert_array_almost_equal(l2.Tc, Tc2) + def test_B(self): + l0 = rtb.Link(B=1.0) + l1 = rtb.Link(B=None) + + nt.assert_array_almost_equal(l0.B, 1.0) + nt.assert_array_almost_equal(l1.B, 0.0) + def test_I(self): l0 = rtb.Link(I=[1, 2, 3]) l1 = rtb.Link(I=[0, 1, 2, 3, 4, 5]) @@ -147,7 +155,7 @@ def test_dyn(self): B = 0 Tc = 0.4(+) -0.43(-) G = -63 -qlim = -2.8 to 2.8""", +qlim = -2.8 to 2.8""", # noqa ) def test_properties(self): @@ -158,7 +166,6 @@ def test_properties(self): self.assertEqual(l0.Jm, 0.0) def test_fail_parent(self): - with self.assertRaises(TypeError): rtb.Link(parent=1) @@ -231,7 +238,116 @@ def test_collided(self): self.assertTrue(c0) self.assertFalse(c1) + def test_init_ets2(self): + e1 = rtb.ET2.R() + link = BaseLink(e1) -if __name__ == "__main__": + self.assertEqual(link.Ts, None) + + def test_get_ets(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + self.assertEqual(link.ets, e1) + + def test_set_ets_fail(self): + e1 = rtb.ETS(rtb.ET.Ry()) + e2 = rtb.ET.Ry() * rtb.ET.Rx(1.0) + link = rtb.Link(e1) + + with self.assertRaises(ValueError): + link.ets = e2 + + def test_set_robot(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + robot = rtb.models.Panda() + + link.robot = robot + + self.assertEqual(link.robot, robot) + + def test_set_qlim_fail(self): + e1 = rtb.ETS(rtb.ET.Ry(1.0)) + link = rtb.Link(e1) + + with self.assertRaises(ValueError): + link.qlim = [1.0, 2.0] + + def test_set_collision(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + s1 = gm.Cuboid([1.0, 1.0, 1.0]) + + link.collision = s1 + + self.assertEqual(link.collision[0], s1) + + def test_set_collision2(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + s1 = gm.Cuboid([1.0, 1.0, 1.0]) + + sg = gm.SceneGroup() + sg.append(s1) + + link.collision = sg + self.assertEqual(link.collision[0], sg[0]) + + def test_set_geometry(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + s1 = gm.Cuboid([1.0, 1.0, 1.0]) + + link.geometry = s1 + + self.assertEqual(link.geometry[0], s1) + + def test_set_geometry2(self): + e1 = rtb.ETS(rtb.ET.Ry()) + link = rtb.Link(e1) + + s1 = gm.Cuboid([1.0, 1.0, 1.0]) + + sg = gm.SceneGroup() + sg.append(s1) + + link.geometry = sg + + self.assertEqual(link.geometry[0], sg[0]) + + def test_dyn2list(self): + l1 = rtb.Link(I=[0, 1, 2, 3, 4, 5]) + + s = l1._dyn2list() + + print(s) + + ans = [" 0", " 0, 0, 0", " 0, 1, 2, 3, 4, 5", " 0", " 0", " 0, 0", " 0"] + + self.assertEqual(s, ans) + + def test_init_fail4(self): + with self.assertRaises(TypeError): + rtb.Link(2.0) # type: ignore + + def test_ets2_A(self): + e1 = rtb.ETS2(rtb.ET2.R()) + link = rtb.Link2(e1) + + nt.assert_almost_equal(link.A(1.0).A, sm.SE2(0.0, 0.0, 1.0).A) + + def test_ets2_A2(self): + e1 = rtb.ETS2(rtb.ET2.R(1.0)) + link = rtb.Link2(e1) + + nt.assert_almost_equal(link.A().A, sm.SE2(0.0, 0.0, 1.0).A) + + +if __name__ == "__main__": unittest.main() diff --git a/tests/test_ERobot.py b/tests/test_ERobot.py index b59d765aa..be698bbb7 100644 --- a/tests/test_ERobot.py +++ b/tests/test_ERobot.py @@ -7,742 +7,23 @@ import numpy.testing as nt import numpy as np import roboticstoolbox as rtb -from roboticstoolbox import ERobot, ET, ETS, ERobot2, Link -from spatialmath import SE2, SE3 +from roboticstoolbox import ERobot, ET, ETS, Link + +# from spatialmath import SE2, SE3 import unittest import spatialmath as sm import spatialgeometry as gm from math import pi, sin, cos +try: + from sympy import symbols -class TestERobot(unittest.TestCase): - def test_init(self): - ets = ETS(rtb.ET.Rz()) - robot = ERobot( - ets, name="myname", manufacturer="I made it", comment="other stuff" - ) - self.assertEqual(robot.name, "myname") - self.assertEqual(robot.manufacturer, "I made it") - self.assertEqual(robot.comment, "other stuff") - - def test_init_ets(self): - ets = ( - rtb.ET.tx(-0.0825) - * rtb.ET.Rz() - * rtb.ET.tx(-0.0825) - * rtb.ET.tz() - * rtb.ET.tx(0.1) - ) - - robot = ERobot(ets) - self.assertEqual(robot.n, 2) - self.assertIsInstance(robot[0], Link) - self.assertIsInstance(robot[1], Link) - self.assertTrue(robot[0].isrevolute) - self.assertTrue(robot[1].isprismatic) - - self.assertIs(robot[0].parent, None) - self.assertIs(robot[1].parent, robot[0]) - self.assertIs(robot[2].parent, robot[1]) - - self.assertEqual(robot[0].children, [robot[1]]) - self.assertEqual(robot[1].children, [robot[2]]) - self.assertEqual(robot[2].children, []) - - def test_init_elink(self): - link1 = Link(ETS(ET.Rx()), name="link1") - link2 = Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2", parent=link1) - link3 = Link(ETS(ET.tx(1)), name="ee_1", parent=link2) - robot = ERobot([link1, link2, link3]) - self.assertEqual(robot.n, 2) - self.assertIsInstance(robot[0], Link) - self.assertIsInstance(robot[1], Link) - self.assertIsInstance(robot[2], Link) - self.assertTrue(robot[0].isrevolute) - self.assertTrue(robot[1].isprismatic) - - self.assertFalse(robot[2].isrevolute) - self.assertFalse(robot[2].isprismatic) - - self.assertIs(robot[0].parent, None) - self.assertIs(robot[1].parent, robot[0]) - self.assertIs(robot[2].parent, robot[1]) - - self.assertEqual(robot[0].children, [robot[1]]) - self.assertEqual(robot[1].children, [robot[2]]) - self.assertEqual(robot[2].children, []) - - link1 = Link(ETS(ET.Rx()), name="link1") - link2 = Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2", parent="link1") - link3 = Link(ETS(ET.tx(1)), name="ee_1", parent="link2") - robot = ERobot([link1, link2, link3]) - self.assertEqual(robot.n, 2) - self.assertIsInstance(robot[0], Link) - self.assertIsInstance(robot[1], Link) - self.assertIsInstance(robot[2], Link) - self.assertTrue(robot[0].isrevolute) - self.assertTrue(robot[1].isprismatic) - - self.assertIs(robot[0].parent, None) - self.assertIs(robot[1].parent, robot[0]) - self.assertIs(robot[2].parent, robot[1]) - - self.assertEqual(robot[0].children, [robot[1]]) - self.assertEqual(robot[1].children, [robot[2]]) - self.assertEqual(robot[2].children, []) - - def test_init_elink_autoparent(self): - links = [ - Link(ETS(ET.Rx()), name="link1"), - Link(ET.tx(1) * ET.ty(-0.5) * ET.tz(), name="link2"), - Link(ETS(ET.tx(1)), name="ee_1"), - ] - robot = ERobot(links) - self.assertEqual(robot.n, 2) - self.assertIsInstance(robot[0], Link) - self.assertIsInstance(robot[1], Link) - self.assertIsInstance(robot[2], Link) - self.assertTrue(robot[0].isrevolute) - self.assertTrue(robot[1].isprismatic) - self.assertIs(robot[0].parent, None) - self.assertIs(robot[1].parent, robot[0]) - self.assertIs(robot[2].parent, robot[1]) - - self.assertEqual(robot[0].children, [robot[1]]) - self.assertEqual(robot[1].children, [robot[2]]) - self.assertEqual(robot[2].children, []) - - def test_init_elink_branched(self): - robot = ERobot( - [ - Link(ETS(ET.Rz()), name="link1"), - Link( - ETS(ET.tx(1)) * ET.ty(-0.5) * ET.Rz(), name="link2", parent="link1" - ), - Link(ETS(ET.tx(1)), name="ee_1", parent="link2"), - Link(ET.tx(1) * ET.ty(0.5) * ET.Rz(), name="link3", parent="link1"), - Link(ETS(ET.tx(1)), name="ee_2", parent="link3"), - ] - ) - self.assertEqual(robot.n, 3) - for i in range(5): - self.assertIsInstance(robot[i], Link) - self.assertTrue(robot[0].isrevolute) - self.assertTrue(robot[1].isrevolute) - self.assertTrue(robot[3].isrevolute) - - self.assertIs(robot[0].parent, None) - self.assertIs(robot[1].parent, robot[0]) - self.assertIs(robot[2].parent, robot[1]) - self.assertIs(robot[3].parent, robot[0]) - self.assertIs(robot[4].parent, robot[3]) - - self.assertEqual(robot[0].children, [robot[1], robot[3]]) - self.assertEqual(robot[1].children, [robot[2]]) - self.assertEqual(robot[2].children, []) - self.assertEqual(robot[3].children, [robot[4]]) - self.assertEqual(robot[2].children, []) - - def test_init_bases(self): - e1 = Link() - e2 = Link() - e3 = Link(parent=e1) - e4 = Link(parent=e2) - - with self.assertRaises(ValueError): - ERobot([e1, e2, e3, e4]) - - def test_jindex(self): - e1 = Link(ETS(ET.Rz()), jindex=0) - e2 = Link(ETS(ET.Rz()), jindex=1, parent=e1) - e3 = Link(ETS(ET.Rz()), jindex=2, parent=e2) - e4 = Link(ETS(ET.Rz()), jindex=0, parent=e3) - - # with self.assertRaises(ValueError): - ERobot([e1, e2, e3, e4], gripper_links=e4) - - def test_jindex_fail(self): - e1 = Link(rtb.ETS(rtb.ET.Rz()), jindex=0) - e2 = Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e1) - e3 = Link(rtb.ETS(rtb.ET.Rz()), jindex=2, parent=e2) - e4 = Link(rtb.ETS(rtb.ET.Rz()), jindex=5, parent=e3) - - with self.assertRaises(ValueError): - ERobot([e1, e2, e3, e4]) - - e1 = Link(rtb.ETS(rtb.ET.Rz()), jindex=0) - e2 = Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e1) - e3 = Link(rtb.ETS(rtb.ET.Rz()), jindex=2, parent=e2) - e4 = Link(rtb.ETS(rtb.ET.Rz()), parent=e3) - - with self.assertRaises(ValueError): - ERobot([e1, e2, e3, e4]) - - def test_panda(self): - panda = rtb.models.ETS.Panda() - qz = np.array([0, 0, 0, 0, 0, 0, 0]) - qr = panda.qr - - nt.assert_array_almost_equal(panda.qr, qr) - nt.assert_array_almost_equal(panda.qz, qz) - nt.assert_array_almost_equal(panda.gravity, np.r_[0, 0, -9.81]) - - def test_q(self): - panda = rtb.models.ETS.Panda() - - q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - q3 = np.expand_dims(q1, 0) - - panda.q = q1 - nt.assert_array_almost_equal(panda.q, q1) - panda.q = q2 - nt.assert_array_almost_equal(panda.q, q2) - panda.q = q3 - nt.assert_array_almost_equal(np.expand_dims(panda.q, 0), q3) - - def test_getters(self): - panda = rtb.models.ETS.Panda() - - panda.qdd = np.ones((7, 1)) - panda.qd = np.ones((1, 7)) - panda.qdd = panda.qd - panda.qd = panda.qdd - - def test_control_mode(self): - panda = rtb.models.ETS.Panda() - panda.control_mode = "v" - self.assertEqual(panda.control_mode, "v") - - def test_base(self): - panda = rtb.models.ETS.Panda() - - pose = sm.SE3() - - panda.base = pose.A - nt.assert_array_almost_equal(np.eye(4), panda.base.A) - - panda.base = pose - nt.assert_array_almost_equal(np.eye(4), panda.base.A) - - def test_fkine(self): - panda = rtb.models.ETS.Panda() - q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - q3 = np.expand_dims(q1, 0) - - ans = np.array( - [ - [-0.50827907, -0.57904589, 0.63746234, 0.44682295], - [0.83014553, -0.52639462, 0.18375824, 0.16168396], - [0.22915229, 0.62258699, 0.74824773, 0.96798113], - [0.0, 0.0, 0.0, 1.0], - ] - ) - - # panda.q = q1 - nt.assert_array_almost_equal(panda.fkine(q1).A, ans) - - def test_jacob0(self): - panda = rtb.models.ETS.Panda() - q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - q3 = np.expand_dims(q1, 0) - q4 = np.expand_dims(q1, 1) - - ans = np.array( - [ - [ - -1.61683957e-01, - 1.07925929e-01, - -3.41453006e-02, - 3.35029257e-01, - -1.07195463e-02, - 1.03187865e-01, - 0.00000000e00, - ], - [ - 4.46822947e-01, - 6.25741987e-01, - 4.16474664e-01, - -8.04745724e-02, - 7.78257566e-02, - -1.17720983e-02, - 0.00000000e00, - ], - [ - 0.00000000e00, - -2.35276631e-01, - -8.20187641e-02, - -5.14076923e-01, - -9.98040745e-03, - -2.02626953e-01, - 0.00000000e00, - ], - [ - 1.29458954e-16, - -9.85449730e-01, - 3.37672585e-02, - -6.16735653e-02, - 6.68449878e-01, - -1.35361558e-01, - 6.37462344e-01, - ], - [ - 9.07021273e-18, - 1.69967143e-01, - 1.95778638e-01, - 9.79165111e-01, - 1.84470262e-01, - 9.82748279e-01, - 1.83758244e-01, - ], - [ - 1.00000000e00, - -2.26036604e-17, - 9.80066578e-01, - -1.93473657e-01, - 7.20517510e-01, - -1.26028049e-01, - 7.48247732e-01, - ], - ] - ) + _sympy = True +except ModuleNotFoundError: + _sympy = False - panda.q = q1 - # nt.assert_array_almost_equal(panda.jacob0(), ans) - nt.assert_array_almost_equal(panda.jacob0(q2), ans) - nt.assert_array_almost_equal(panda.jacob0(q3), ans) - nt.assert_array_almost_equal(panda.jacob0(q4), ans) - self.assertRaises(TypeError, panda.jacob0, "Wfgsrth") - - def test_hessian0(self): - panda = rtb.models.ETS.Panda() - q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - q3 = np.expand_dims(q1, 0) - q4 = np.expand_dims(q1, 1) - - ans = np.array( - [ - [ - [ - -4.46822947e-01, - -6.25741987e-01, - -4.16474664e-01, - 8.04745724e-02, - -7.78257566e-02, - 1.17720983e-02, - 0.00000000e00, - ], - [ - -6.25741987e-01, - -3.99892968e-02, - -1.39404950e-02, - -8.73761859e-02, - -1.69634134e-03, - -3.44399243e-02, - 0.00000000e00, - ], - [ - -4.16474664e-01, - -1.39404950e-02, - -4.24230421e-01, - -2.17748413e-02, - -7.82283735e-02, - -2.81325889e-02, - 0.00000000e00, - ], - [ - 8.04745724e-02, - -8.73761859e-02, - -2.17748413e-02, - -5.18935898e-01, - 5.28476698e-03, - -2.00682834e-01, - 0.00000000e00, - ], - [ - -7.78257566e-02, - -1.69634134e-03, - -7.82283735e-02, - 5.28476698e-03, - -5.79159088e-02, - -2.88966443e-02, - 0.00000000e00, - ], - [ - 1.17720983e-02, - -3.44399243e-02, - -2.81325889e-02, - -2.00682834e-01, - -2.88966443e-02, - -2.00614904e-01, - 0.00000000e00, - ], - [ - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - ], - ], - [ - [ - -1.61683957e-01, - 1.07925929e-01, - -3.41453006e-02, - 3.35029257e-01, - -1.07195463e-02, - 1.03187865e-01, - 0.00000000e00, - ], - [ - 1.07925929e-01, - -2.31853293e-01, - -8.08253690e-02, - -5.06596965e-01, - -9.83518983e-03, - -1.99678676e-01, - 0.00000000e00, - ], - [ - -3.41453006e-02, - -8.08253690e-02, - -3.06951191e-02, - 3.45709946e-01, - -1.01688580e-02, - 1.07973135e-01, - 0.00000000e00, - ], - [ - 3.35029257e-01, - -5.06596965e-01, - 3.45709946e-01, - -9.65242924e-02, - 1.45842251e-03, - -3.24608603e-02, - 0.00000000e00, - ], - [ - -1.07195463e-02, - -9.83518983e-03, - -1.01688580e-02, - 1.45842251e-03, - -1.05221866e-03, - 2.09794626e-01, - 0.00000000e00, - ], - [ - 1.03187865e-01, - -1.99678676e-01, - 1.07973135e-01, - -3.24608603e-02, - 2.09794626e-01, - -4.04324654e-02, - 0.00000000e00, - ], - [ - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - ], - ], - [ - [ - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - ], - [ - 0.00000000e00, - -6.34981134e-01, - -4.04611266e-01, - 2.23596800e-02, - -7.48714002e-02, - -5.93773551e-03, - 0.00000000e00, - ], - [ - 0.00000000e00, - -4.04611266e-01, - 2.07481281e-02, - -6.83089775e-02, - 4.72662062e-03, - -2.05994912e-02, - 0.00000000e00, - ], - [ - 0.00000000e00, - 2.23596800e-02, - -6.83089775e-02, - -3.23085806e-01, - 5.69641385e-03, - -1.00311930e-01, - 0.00000000e00, - ], - [ - 0.00000000e00, - -7.48714002e-02, - 4.72662062e-03, - 5.69641385e-03, - 5.40000550e-02, - -2.69041502e-02, - 0.00000000e00, - ], - [ - 0.00000000e00, - -5.93773551e-03, - -2.05994912e-02, - -1.00311930e-01, - -2.69041502e-02, - -9.98142073e-02, - 0.00000000e00, - ], - [ - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - 0.00000000e00, - ], - ], - [ - [ - -9.07021273e-18, - -2.77555756e-17, - -2.77555756e-17, - -1.11022302e-16, - -2.77555756e-17, - 0.00000000e00, - -2.77555756e-17, - ], - [ - -1.69967143e-01, - -1.97756387e-17, - 4.11786040e-17, - -1.48932398e-16, - -5.07612940e-17, - -8.38219650e-17, - -4.90138154e-17, - ], - [ - -1.95778638e-01, - 1.66579116e-01, - -1.38777878e-17, - 1.04083409e-17, - -1.38777878e-17, - 3.46944695e-18, - 0.00000000e00, - ], - [ - -9.79165111e-01, - -3.28841647e-02, - -9.97525009e-01, - -4.16333634e-17, - -1.14491749e-16, - 1.38777878e-17, - -6.24500451e-17, - ], - [ - -1.84470262e-01, - 1.22464303e-01, - -3.97312016e-02, - 7.41195745e-01, - -2.77555756e-17, - 1.12757026e-16, - 2.77555756e-17, - ], - [ - -9.82748279e-01, - -2.14206274e-02, - -9.87832342e-01, - 6.67336352e-02, - -7.31335770e-01, - 2.08166817e-17, - -6.07153217e-17, - ], - [ - -1.83758244e-01, - 1.27177529e-01, - -3.36043908e-02, - 7.68210453e-01, - 5.62842325e-03, - 7.58497864e-01, - 0.00000000e00, - ], - ], - [ - [ - 1.29458954e-16, - -1.11022302e-16, - 8.67361738e-17, - -4.16333634e-17, - 5.55111512e-17, - 2.77555756e-17, - 5.55111512e-17, - ], - [ - -9.85449730e-01, - -6.36381327e-17, - -1.02735399e-16, - -1.83043043e-17, - -5.63484308e-17, - 8.08886307e-18, - 1.07112702e-18, - ], - [ - 3.37672585e-02, - 9.65806345e-01, - 8.32667268e-17, - -2.55871713e-17, - 1.07552856e-16, - 2.08166817e-17, - -5.20417043e-18, - ], - [ - -6.16735653e-02, - -1.90658563e-01, - -5.39111251e-02, - -6.59194921e-17, - -2.77555756e-17, - 2.38524478e-17, - -4.16333634e-17, - ], - [ - 6.68449878e-01, - 7.10033786e-01, - 6.30795483e-01, - -8.48905588e-02, - 0.00000000e00, - 3.46944695e-17, - 2.77555756e-17, - ], - [ - -1.35361558e-01, - -1.24194307e-01, - -1.28407717e-01, - 1.84162966e-02, - -1.32869389e-02, - 2.77555756e-17, - -2.08166817e-17, - ], - [ - 6.37462344e-01, - 7.37360525e-01, - 5.99489263e-01, - -7.71850655e-02, - -4.08633244e-02, - 2.09458434e-02, - 0.00000000e00, - ], - ], - [ - [ - 0.00000000e00, - -6.59521910e-17, - -1.31033786e-16, - -1.92457571e-16, - 1.54134782e-17, - -7.69804929e-17, - 1.11140361e-17, - ], - [ - 0.00000000e00, - -2.77555756e-17, - 7.15573434e-17, - 1.65666092e-16, - 1.38777878e-17, - -8.67361738e-18, - 3.46944695e-17, - ], - [ - 0.00000000e00, - -1.98669331e-01, - 8.67361738e-18, - -1.46584134e-16, - 6.02816408e-17, - -3.12250226e-17, - 6.11490025e-17, - ], - [ - 0.00000000e00, - -9.54435515e-01, - 4.51380881e-02, - 1.38777878e-17, - 1.08420217e-16, - 3.46944695e-18, - 6.24500451e-17, - ], - [ - 0.00000000e00, - -2.95400686e-01, - -1.24639152e-01, - -6.65899738e-01, - -4.85722573e-17, - -5.20417043e-18, - -5.55111512e-17, - ], - [ - 0.00000000e00, - -9.45442009e-01, - 5.96856167e-02, - 7.19317248e-02, - 6.81888149e-01, - -2.77555756e-17, - 1.04083409e-17, - ], - [ - 0.00000000e00, - -2.89432165e-01, - -1.18596498e-01, - -6.35513913e-01, - 5.24032975e-03, - -6.51338823e-01, - 0.00000000e00, - ], - ], - ] - ) - - ans_new = np.empty((7, 6, 7)) - - for i in range(7): - ans_new[i, :, :] = ans[:, :, i] - - nt.assert_array_almost_equal(panda.hessian0(q1), ans_new) - nt.assert_array_almost_equal(panda.hessian0(q2), ans_new) - nt.assert_array_almost_equal(panda.hessian0(q3), ans_new) - nt.assert_array_almost_equal(panda.hessian0(q4), ans_new) - nt.assert_array_almost_equal(panda.hessian0(J0=panda.jacob0(q1)), ans_new) - nt.assert_array_almost_equal(panda.hessian0(q1, J0=panda.jacob0(q1)), ans_new) - - def test_manipulability(self): - panda = rtb.models.ETS.Panda() - q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - - ans = 0.006559178039088341 - - panda.q = q1 - nt.assert_array_almost_equal(panda.manipulability(q2), ans) - # self.assertRaises(ValueError, panda.manipulability) - self.assertRaises(TypeError, panda.manipulability, "Wfgsrth") - self.assertRaises(ValueError, panda.manipulability, [1, 3]) +class TestERobot(unittest.TestCase): def test_jacobm(self): panda = rtb.models.ETS.Panda() q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) @@ -777,27 +58,6 @@ def test_jacobm(self): ValueError, panda.jacobm, [1, 3], panda.jacob0(q1), np.array([1, 2, 3]) ) - # def test_jacobe(self): - # pdh = rtb.models.DH.Panda() - # panda = rtb.models.ETS.Panda() - # q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - # panda.q = q1 - - # # nt.assert_array_almost_equal(panda.jacobe(), pdh.jacobe(q1)) - # nt.assert_array_almost_equal(panda.jacobe(q1), pdh.jacobe(q1)) - - def test_init2(self): - l0 = Link() - l1 = Link(parent=l0) - r = ERobot([l0, l1], base=sm.SE3.Rx(1.3)) - # r.base_link = l1 - - with self.assertRaises(TypeError): - ERobot(l0, base=sm.SE3.Rx(1.3)) - - with self.assertRaises(TypeError): - ERobot([1, 2], base=sm.SE3.Rx(1.3)) - def test_dict(self): panda = rtb.models.Panda() panda.grippers[0].links[0].collision.append(gm.Cuboid([1, 1, 1])) @@ -811,81 +71,6 @@ def test_fkdict(self): panda.grippers[0].links[0].collision.append(gm.Cuboid([1, 1, 1])) panda._fk_dict() - def test_elinks(self): - panda = rtb.models.Panda() - self.assertEqual(panda.elinks[0], panda.link_dict[panda.elinks[0].name]) - - def test_base_link_setter(self): - panda = rtb.models.Panda() - - with self.assertRaises(TypeError): - panda.base_link = [1] - - def test_ee_link_setter(self): - panda = rtb.models.Panda() - - panda.ee_links = panda.links[5] - - with self.assertRaises(TypeError): - panda.ee_links = [1] # type: ignore - - def test_qlim(self): - panda = rtb.models.ETS.Panda() - - self.assertEqual(panda.qlim.shape[0], 2) - self.assertEqual(panda.qlim.shape[1], panda.n) - - def test_manuf(self): - panda = rtb.models.ETS.Panda() - - self.assertIsInstance(panda.manufacturer, str) - - def test_complex(self): - l0 = Link(rtb.ET.tx(0.1) * rtb.ET.Rx()) - l1 = Link(rtb.ET.tx(0.1) * rtb.ET.Ry(), parent=l0) - l2 = Link(rtb.ET.tx(0.1) * rtb.ET.Rz(), parent=l1) - l3 = Link(rtb.ET.tx(0.1) * rtb.ET.tx(), parent=l2) - l4 = Link(rtb.ET.tx(0.1) * rtb.ET.ty(), parent=l3) - l5 = Link(rtb.ET.tx(0.1) * rtb.ET.tz(), parent=l4) - - r = ERobot([l0, l1, l2, l3, l4, l5]) - q = [1, 2, 3, 1, 2, 3] - - ans = np.array( - [ - [-0.0, 0.08752679, -0.74761985, 0.41198225, 0.05872664, 0.90929743], - [ - 1.46443609, - 2.80993063, - 0.52675075, - -0.68124272, - -0.64287284, - 0.35017549, - ], - [ - -1.04432, - -1.80423571, - -2.20308833, - 0.60512725, - -0.76371834, - -0.2248451, - ], - [1.0, 0.0, 0.90929743, 0.0, 0.0, 0.0], - [0.0, 0.54030231, 0.35017549, 0.0, 0.0, 0.0], - [0.0, 0.84147098, -0.2248451, 0.0, 0.0, 0.0], - ] - ) - - nt.assert_array_almost_equal(r.jacob0(q), ans) - - def test_control_mode2(self): - panda = rtb.models.ETS.Panda() - - panda.control_mode = "p" - - with self.assertRaises(ValueError): - panda.control_mode = "z" - def test_dist(self): s0 = gm.Cuboid([1, 1, 1], pose=sm.SE3(0, 0, 0)) s1 = gm.Cuboid([1, 1, 1], pose=sm.SE3(3, 0, 0)) @@ -922,30 +107,30 @@ def test_invdyn(self): tau = robot.rne(z, z, z) / 9.81 nt.assert_array_almost_equal(tau, np.r_[-2, -0.5]) - tau = robot.rne([0, -pi / 2], z, z) / 9.81 + tau = robot.rne(np.array([0.0, -pi / 2.0]), z, z) / 9.81 nt.assert_array_almost_equal(tau, np.r_[-1.5, 0]) - tau = robot.rne([-pi / 2, pi / 2], z, z) / 9.81 + tau = robot.rne(np.array([-pi / 2, pi / 2]), z, z) / 9.81 nt.assert_array_almost_equal(tau, np.r_[-0.5, -0.5]) - tau = robot.rne([-pi / 2, 0], z, z) / 9.81 + tau = robot.rne(np.array([-pi / 2, 0]), z, z) / 9.81 nt.assert_array_almost_equal(tau, np.r_[0, 0]) # check velocity terms robot.gravity = [0, 0, 0] - q = [0, -pi / 2] + q = np.array([0, -pi / 2]) h = -0.5 * sin(q[1]) - tau = robot.rne(q, [0, 0], z) + tau = robot.rne(q, np.array([0, 0]), z) nt.assert_array_almost_equal(tau, np.r_[0, 0] * h) - tau = robot.rne(q, [1, 0], z) + tau = robot.rne(q, np.array([1, 0]), z) nt.assert_array_almost_equal(tau, np.r_[0, -1] * h) - tau = robot.rne(q, [0, 1], z) + tau = robot.rne(q, np.array([0, 1]), z) nt.assert_array_almost_equal(tau, np.r_[1, 0] * h) - tau = robot.rne(q, [1, 1], z) + tau = robot.rne(q, np.array([1, 1]), z) nt.assert_array_almost_equal(tau, np.r_[3, -1] * h) # check inertial terms @@ -955,16 +140,16 @@ def test_invdyn(self): d21 = d12 d22 = 0.25 - tau = robot.rne(q, z, [0, 0]) + tau = robot.rne(q, z, np.array([0, 0])) nt.assert_array_almost_equal(tau, np.r_[0, 0]) - tau = robot.rne(q, z, [1, 0]) + tau = robot.rne(q, z, np.array([1, 0])) nt.assert_array_almost_equal(tau, np.r_[d11, d21]) - tau = robot.rne(q, z, [0, 1]) + tau = robot.rne(q, z, np.array([0, 1])) nt.assert_array_almost_equal(tau, np.r_[d12, d22]) - tau = robot.rne(q, z, [1, 1]) + tau = robot.rne(q, z, np.array([1, 1])) nt.assert_array_almost_equal(tau, np.r_[d11 + d12, d21 + d22]) @@ -976,16 +161,16 @@ def test_plot(self): def test_teach(self): robot = rtb.models.ETS.Planar2() - e = robot.teach(block=False, name=True) + e = robot.teach(robot.qz, block=False) e.close() - e = robot.teach(robot.qz, block=False, name=True) + e = robot.teach(robot.qz, block=False) e.close() def test_plot_with_vellipse(self): robot = rtb.models.ETS.Planar2() e = robot.plot( - robot.qz, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2] + robot.qb, block=False, name=True, vellipse=True, limits=[1, 2, 1, 2] ) e.step() e.close() @@ -998,6 +183,40 @@ def test_plot_with_fellipse(self): e.step() e.close() + def test_base(self): + robot = rtb.models.ETS.Planar2() + nt.assert_almost_equal(robot.base.A, sm.SE2().A) + + def test_jacobe(self): + robot = rtb.models.ETS.Planar2() + J = robot.jacobe(robot.qz) + + a1 = np.array([[0.0, 0.0], [2.0, 1.0], [1.0, 1.0]]) + + nt.assert_almost_equal(J, a1) + + @unittest.skipUnless(_sympy, "sympy not installed") + def test_symdyn(self): + + a1, a2, r1, r2, m1, m2, g = symbols("a1 a2 r1 r2 m1 m2 g") + link1 = Link(ET.Ry(flip=True), m=m1, r=[r1, 0, 0], name="link0") + link2 = Link(ET.tx(a1) * ET.Ry(flip=True), m=m2, r=[r2, 0, 0], name="link1") + robot = ERobot([link1, link2]) + + q = symbols("q:2") + qd = symbols("qd:2") + qdd = symbols("qdd:2") + Q = robot.rne(q, qd, qdd, gravity=[0, 0, g], symbolic=True) + + self.assertEqual( + str(Q[0]), + "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) - a1*(m2*(a1*qd0*qd1*cos(q1) - a1*qdd0*sin(q1) - g*sin(q0)*cos(q1) - g*sin(q1)*cos(q0)) + (qd0 + qd1)*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1)))*sin(q1) - a1*(-a1*m2*qd0*(-qd0 - qd1)*sin(q1) - m2*r2*(qdd0 + qdd1) + m2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1)))*cos(q1) + g*m1*r1*cos(q0) + m1*qdd0*r1**2 + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1))", + ) + self.assertEqual( + str(Q[1]), + "a1**2*m2*qd0**2*sin(q1)*cos(q1) + a1*qd0*(-a1*m2*qd0*cos(q1) - m2*r2*(qd0 + qd1))*sin(q1) + m2*r2**2*(qdd0 + qdd1) - m2*r2*(-a1*qd0*qd1*sin(q1) - a1*qdd0*cos(q1) + g*sin(q0)*sin(q1) - g*cos(q0)*cos(q1))", + ) + if __name__ == "__main__": # pragma nocover diff --git a/tests/test_ET.py b/tests/test_ET.py index a86bdb1d3..f820ad098 100644 --- a/tests/test_ET.py +++ b/tests/test_ET.py @@ -106,9 +106,7 @@ def test_str_se3(self): self.assertEqual(str(a), "SE3(1, 0, 0)") self.assertEqual(str(b), "SE3(-122.7°, 65.41°, -8.113°)") - self.assertEqual( - str(c), "SE3(1, 0, 0; -122.7°, 65.41°, -8.113°)" - ) + self.assertEqual(str(c), "SE3(1, 0, 0; -122.7°, 65.41°, -8.113°)") def test_repr(self): rx = rtb.ET.Rx(1.543, jindex=5, flip=True, qlim=[-1, 1]) diff --git a/tests/test_ETS.py b/tests/test_ETS.py index 14460149a..680bf4c69 100644 --- a/tests/test_ETS.py +++ b/tests/test_ETS.py @@ -29,8 +29,8 @@ def test_bad_arg(self): def test_args(self): deg = np.pi / 180 - mm = 1e-3 - tool_offset = (103) * mm + # mm = 1e-3 + # tool_offset = (103) * mm l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) @@ -54,7 +54,7 @@ def test_args(self): r3 = rtb.ETS(l0 + l1 + l2 + l3 + l3 + l4 + l5) r4 = rtb.ETS([l0, l1, l2, l3, l3, l4, l5]) r5 = rtb.ETS([l0, l1, l2, l3, l3, l4, rtb.ET.Rx(90 * deg), rtb.ET.Rz(jindex=5)]) - r6 = rtb.ETS([r1]) + # r6 = rtb.ETS([r1]) r7 = rtb.ETS(rtb.ET.Rx(1.0)) self.assertEqual(r1, r2) @@ -109,7 +109,7 @@ def test_str_flip(self): def test_str_sym(self): x = sympy.Symbol("x") - rx = rtb.ET.Rx(x) + rx = rtb.ET.Rx(x) # type: ignore a = rtb.ET.Rx(jindex=2) b = rtb.ET.Ry(jindex=5) c = rtb.ET.Rz(jindex=7) @@ -138,7 +138,7 @@ def ets_mul(self): def test_n(self): rx = rtb.ET.Rx(1.543) ry = rtb.ET.Ry(1.543) - rz = rtb.ET.Rz(1.543) + # rz = rtb.ET.Rz(1.543) a = rtb.ET.Rx() b = rtb.ET.Ry() @@ -184,7 +184,7 @@ def test_fkine(self): * SE3.Tz(q[5]) ) - nt.assert_almost_equal(r.fkine(q), ans.A) + nt.assert_almost_equal(r.fkine(q).A, ans.A) def test_fkine_sym(self): x = sympy.Symbol("x") @@ -193,7 +193,7 @@ def test_fkine_sym(self): q = np.array([y, z]) qt = np.array([[1.0, y], [z, y], [x, 2.0]]) - a = rtb.ET.Rx(x) + a = rtb.ET.Rx(x) # type: ignore b = rtb.ET.Ry(jindex=0) c = rtb.ET.tz(jindex=1) @@ -227,9 +227,37 @@ def test_fkine_sym(self): q2 = [y] ans6 = SE3.Rx(y) * tool - nt.assert_almost_equal(r2.fkine(q2, tool=tool).A, sympy.simplify(ans6.A)) # type: ignore + nt.assert_almost_equal( + r2.fkine(q2, tool=tool).A, sympy.simplify(ans6.A) # type: ignore + ) # nt.assert_almost_equal(r2.fkine(q2, tool=tool), ans6) # type: ignore + def test_fkine_traj(self): + robot = rtb.ERobot( + [ + rtb.Link(rtb.ET.Rx()), + rtb.Link(rtb.ET.Ry()), + rtb.Link(rtb.ET.Rz()), + rtb.Link(rtb.ET.tx()), + rtb.Link(rtb.ET.ty()), + rtb.Link(rtb.ET.tz()), + ] + ) + + ets = robot.ets() + + qt = np.arange(10 * ets.n).reshape(10, ets.n) + + T_individual = [] + + for q in qt: + T_individual.append(ets.eval(q)) + + T_traj = ets.eval(qt) + + for i in range(10): + nt.assert_allclose(T_traj[i, :, :], T_individual[i]) + def test_jacob0_panda(self): deg = np.pi / 180 mm = 1e-3 @@ -360,9 +388,9 @@ def test_jacobe_panda(self): r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) - q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - q3 = np.expand_dims(q1, 0) - q4 = np.expand_dims(q1, 1) + # q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + # q3 = np.expand_dims(q1, 0) + # q4 = np.expand_dims(q1, 1) ans = tr2jac(r.eval(q1).T) @ r.jacob0(q1) @@ -381,14 +409,14 @@ def test_pop(self): r = a + b + c + d - nt.assert_almost_equal(r.fkine(q), ans1.A) + nt.assert_almost_equal(r.fkine(q).A, ans1.A) et = r.pop(1) - nt.assert_almost_equal(r.fkine(q), ans2.A) + nt.assert_almost_equal(r.fkine(q).A, ans2.A) nt.assert_almost_equal(et.A(q[1]), SE3.Ry(q[1]).A) et = r.pop() - nt.assert_almost_equal(r.fkine(q), ans3.A) + nt.assert_almost_equal(r.fkine(q).A, ans3.A) nt.assert_almost_equal(et.A(), SE3.Tz(1.543).A) def test_inv(self): @@ -403,10 +431,10 @@ def test_inv(self): r = a + b + c + d r_inv = r.inv() - nt.assert_almost_equal(r_inv.fkine(q), ans1.inv().A) + nt.assert_almost_equal(r_inv.fkine(q).A, ans1.inv().A) def test_jointset(self): - q = [1.0, 2.0, 3.0] + # q = [1.0, 2.0, 3.0] a = rtb.ET.Rx(jindex=0) b = rtb.ET.Ry(jindex=1) c = rtb.ET.Rz(jindex=2) @@ -559,7 +587,7 @@ def test_insert(self): nt.assert_almost_equal(r.fkine(q).A, r5.fkine(q).A) def test_jacob0(self): - q = [0] + q = [0.0] rx = rtb.ETS(rtb.ET.Rx()) ry = rtb.ETS(rtb.ET.Ry()) rz = rtb.ETS(rtb.ET.Rz()) @@ -578,7 +606,7 @@ def test_jacob0(self): nt.assert_almost_equal(r.jacob0(q), np.eye(6)) def test_jacobe(self): - q = [0] + q = [0.0] rx = rtb.ETS(rtb.ET.Rx()) ry = rtb.ETS(rtb.ET.Ry()) rz = rtb.ETS(rtb.ET.Rz()) @@ -1749,10 +1777,10 @@ def test_plot(self): tz = rtb.ETS(rtb.ET.tz(jindex=5, qlim=[-1, 1])) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) r = tx + ty + tz + rx + ry + rz + a - r.plot(q=q2, block=False) + r.plot(q=q2, block=False, backend="pyplot") def test_teach(self): - x = sympy.Symbol("x") + # x = sympy.Symbol("x") q2 = np.array([0, 1, 2, 3, 4, 5]) rx = rtb.ETS(rtb.ET.Rx(jindex=0)) ry = rtb.ETS(rtb.ET.Ry(jindex=1)) @@ -1762,9 +1790,2582 @@ def test_teach(self): tz = rtb.ETS(rtb.ET.tz(jindex=5, qlim=[-1, 1])) a = rtb.ETS(rtb.ET.SE3(np.eye(4))) r = tx + ty + tz + rx + ry + rz + a - r.teach(q=q2, block=False) + r.teach(q=q2, block=False, backend="pyplot") + def test_partial_fkine(self): + deg = np.pi / 180 + mm = 1e-3 + tool_offset = (103) * mm -if __name__ == "__main__": + l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + + l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + + l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + + l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + + l4 = ( + rtb.ET.tx(-0.0825) + * rtb.ET.Rx(-90 * deg) + * rtb.ET.tz(0.384) + * rtb.ET.Rz(jindex=4) + ) + + l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + + l6 = ( + rtb.ET.tx(0.088) + * rtb.ET.Rx(90 * deg) + * rtb.ET.tz(0.107) + * rtb.ET.Rz(jindex=6) + ) + + ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + + r = l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee + r2 = rtb.Robot(r) + + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + + ans = np.array( + [ + [ + [ + [ + 1.61683957e-01, + -1.07925929e-01, + 3.41453006e-02, + -3.35029257e-01, + 1.07195463e-02, + -1.03187865e-01, + 0.00000000e00, + ], + [ + -4.46822947e-01, + -6.25741987e-01, + -4.16474664e-01, + 8.04745724e-02, + -7.78257566e-02, + 1.17720983e-02, + 0.00000000e00, + ], + [ + -3.17334834e-18, + 1.10026604e-17, + 2.42699171e-18, + 1.47042021e-17, + 2.53075280e-19, + 4.65397353e-18, + 0.00000000e00, + ], + [ + 0.00000000e00, + 9.85449730e-01, + -3.37672585e-02, + 6.16735653e-02, + -6.68449878e-01, + 1.35361558e-01, + -6.37462344e-01, + ], + [ + 0.00000000e00, + -1.69967143e-01, + -1.95778638e-01, + -9.79165111e-01, + -1.84470262e-01, + -9.82748279e-01, + -1.83758244e-01, + ], + [ + 0.00000000e00, + -4.38924163e-17, + 3.44245060e-18, + 6.56556980e-18, + 3.26553464e-17, + 3.19571960e-18, + 3.12169108e-17, + ], + ], + [ + [ + -7.73258609e-18, + 2.31853293e-01, + 8.08253690e-02, + 5.06596965e-01, + 9.83518983e-03, + 1.99678676e-01, + 0.00000000e00, + ], + [ + 5.05851038e-17, + -3.99892968e-02, + -1.39404950e-02, + -8.73761859e-02, + -1.69634134e-03, + -3.44399243e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + -1.38777878e-17, + -1.38777878e-17, + 5.55111512e-17, + 0.00000000e00, + -1.38777878e-17, + 0.00000000e00, + ], + [ + -9.85449730e-01, + 0.00000000e00, + -9.65806345e-01, + 1.90658563e-01, + -7.10033786e-01, + 1.24194307e-01, + -7.37360525e-01, + ], + [ + 1.69967143e-01, + 0.00000000e00, + 1.66579116e-01, + -3.28841647e-02, + 1.22464303e-01, + -2.14206274e-02, + 1.27177529e-01, + ], + [ + 4.38924163e-17, + 0.00000000e00, + 4.85722573e-17, + 2.77555756e-17, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + 1.58461042e-01, + -1.13719243e-01, + 3.06951191e-02, + -3.45709946e-01, + 1.01688580e-02, + -1.07973135e-01, + 0.00000000e00, + ], + [ + -4.37916236e-01, + -6.59330946e-01, + -4.24230421e-01, + -2.17748413e-02, + -7.82283735e-02, + -2.81325889e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 1.38777878e-17, + 1.73472348e-17, + 0.00000000e00, + 6.07153217e-18, + 0.00000000e00, + ], + [ + 3.37672585e-02, + 9.65806345e-01, + 0.00000000e00, + 5.39111251e-02, + -6.30795483e-01, + 1.28407717e-01, + -5.99489263e-01, + ], + [ + 1.95778638e-01, + -1.66579116e-01, + 0.00000000e00, + -9.97525009e-01, + -3.97312016e-02, + -9.87832342e-01, + -3.36043908e-02, + ], + [ + -3.44245060e-18, + -4.85722573e-17, + 0.00000000e00, + 0.00000000e00, + 4.16333634e-17, + 0.00000000e00, + 3.46944695e-17, + ], + ], + [ + [ + -3.12815864e-02, + 3.53911729e-02, + -1.54782657e-03, + 9.65242924e-02, + -1.45842251e-03, + 3.24608603e-02, + 0.00000000e00, + ], + [ + 8.64484696e-02, + -1.09310078e-01, + 2.66964059e-04, + -5.18935898e-01, + 5.28476698e-03, + -2.00682834e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.38777878e-17, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -6.16735653e-02, + -1.90658563e-01, + -5.39111251e-02, + 0.00000000e00, + 8.48905588e-02, + -1.84162966e-02, + 7.71850655e-02, + ], + [ + 9.79165111e-01, + 3.28841647e-02, + 9.97525009e-01, + 0.00000000e00, + 7.41195745e-01, + 6.67336352e-02, + 7.68210453e-01, + ], + [ + -6.56556980e-18, + -2.77555756e-17, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -2.77555756e-17, + ], + ], + [ + [ + 1.16496122e-01, + -2.35033157e-01, + -3.02231459e-02, + -5.85029103e-01, + 1.05221866e-03, + -2.09794626e-01, + 0.00000000e00, + ], + [ + -3.21943757e-01, + -4.94259600e-01, + -3.15207311e-01, + -3.68485664e-02, + -5.79159088e-02, + -2.88966443e-02, + 0.00000000e00, + ], + [ + 1.38777878e-17, + 0.00000000e00, + 1.38777878e-17, + 0.00000000e00, + 0.00000000e00, + 1.38777878e-17, + 0.00000000e00, + ], + [ + 6.68449878e-01, + 7.10033786e-01, + 6.30795483e-01, + -8.48905588e-02, + 0.00000000e00, + 1.32869389e-02, + 4.08633244e-02, + ], + [ + 1.84470262e-01, + -1.22464303e-01, + 3.97312016e-02, + -7.41195745e-01, + 0.00000000e00, + -7.31335770e-01, + 5.62842325e-03, + ], + [ + -3.26553464e-17, + 0.00000000e00, + -4.16333634e-17, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + -2.03767136e-02, + 4.54491056e-02, + 6.79892209e-03, + 1.11809337e-01, + 0.00000000e00, + 4.04324654e-02, + 0.00000000e00, + ], + [ + 5.63122241e-02, + -1.52356663e-01, + -2.81163100e-02, + -5.15350265e-01, + -5.20417043e-18, + -2.00614904e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + -5.55111512e-17, + 1.38777878e-17, + -1.38777878e-17, + 3.46944695e-18, + 0.00000000e00, + ], + [ + -1.35361558e-01, + -1.24194307e-01, + -1.28407717e-01, + 1.84162966e-02, + -1.32869389e-02, + 0.00000000e00, + -2.09458434e-02, + ], + [ + 9.82748279e-01, + 2.14206274e-02, + 9.87832342e-01, + -6.67336352e-02, + 7.31335770e-01, + 0.00000000e00, + 7.58497864e-01, + ], + [ + -3.19571960e-18, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + 1.20979654e-01, + -2.30735324e-01, + -2.67347299e-02, + -5.78389562e-01, + 1.65874227e-03, + -2.06377139e-01, + 0.00000000e00, + ], + [ + -3.34334256e-01, + -5.11444043e-01, + -3.26697847e-01, + -3.42509562e-02, + -6.00669280e-02, + -2.84259272e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 6.93889390e-18, + 0.00000000e00, + 0.00000000e00, + 1.38777878e-17, + 0.00000000e00, + ], + [ + 6.37462344e-01, + 7.37360525e-01, + 5.99489263e-01, + -7.71850655e-02, + -4.08633244e-02, + 2.09458434e-02, + 0.00000000e00, + ], + [ + 1.83758244e-01, + -1.27177529e-01, + 3.36043908e-02, + -7.68210453e-01, + -5.62842325e-03, + -7.58497864e-01, + 0.00000000e00, + ], + [ + -3.12169108e-17, + 0.00000000e00, + -3.46944695e-17, + 2.77555756e-17, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + ], + [ + [ + [ + -1.07925929e-01, + 2.31853293e-01, + 8.08253690e-02, + 5.06596965e-01, + 9.83518983e-03, + 1.99678676e-01, + 0.00000000e00, + ], + [ + -6.25741987e-01, + -3.99892968e-02, + -1.39404950e-02, + -8.73761859e-02, + -1.69634134e-03, + -3.44399243e-02, + 0.00000000e00, + ], + [ + 1.10026604e-17, + -1.03268599e-17, + -3.60000174e-18, + -2.25640783e-17, + -4.38064199e-19, + -8.89378659e-18, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + -9.65806345e-01, + 1.90658563e-01, + -7.10033786e-01, + 1.24194307e-01, + -7.37360525e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 1.66579116e-01, + -3.28841647e-02, + 1.22464303e-01, + -2.14206274e-02, + 1.27177529e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 4.30174903e-17, + -8.49202631e-18, + 3.16252545e-17, + -5.53167558e-18, + 3.28424010e-17, + ], + ], + [ + [ + 1.26880993e-17, + -1.07925929e-01, + -6.87706209e-02, + 3.80041092e-03, + -1.27256780e-02, + -1.00921994e-03, + 0.00000000e00, + ], + [ + 7.35641246e-17, + -6.25741987e-01, + -3.98724063e-01, + 2.20343406e-02, + -7.37820011e-02, + -5.85133986e-03, + 0.00000000e00, + ], + [ + -1.24900090e-16, + 2.35276631e-01, + 8.20187641e-02, + 5.14076923e-01, + 9.98040745e-03, + 2.02626953e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + -3.37672585e-02, + -1.62222678e-01, + -5.02084106e-02, + -1.60694077e-01, + -4.91939581e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + -1.95778638e-01, + -9.40548220e-01, + -2.91102526e-01, + -9.31685572e-01, + -2.85220849e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + -9.80066578e-01, + 1.93473657e-01, + -7.20517510e-01, + 1.26028049e-01, + -7.48247732e-01, + ], + ], + [ + [ + -1.70045802e-02, + -4.16333634e-17, + 3.52650006e-03, + -1.16102817e-02, + 8.03370202e-04, + -3.50123667e-03, + 0.00000000e00, + ], + [ + -5.81147164e-01, + 3.81639165e-17, + 2.04462373e-02, + -6.73150634e-02, + 4.65784701e-03, + -2.02997631e-02, + 0.00000000e00, + ], + [ + 3.56738040e-01, + -9.54097912e-17, + 1.02353729e-01, + -3.36978765e-01, + 2.33171516e-02, + -1.01620481e-01, + 0.00000000e00, + ], + [ + 9.65806345e-01, + 3.37672585e-02, + 0.00000000e00, + 7.67199187e-03, + -2.11845605e-02, + 1.01445937e-02, + -2.01575078e-02, + ], + [ + -1.66579116e-01, + 1.95778638e-01, + 0.00000000e00, + 4.44813167e-02, + -1.22825619e-01, + 5.88171749e-02, + -1.16870886e-01, + ], + [ + -4.30174903e-17, + 9.80066578e-01, + 0.00000000e00, + 2.22673179e-01, + -6.14864240e-01, + 2.94438391e-01, + -5.85054890e-01, + ], + ], + [ + [ + 4.47344513e-01, + -2.45209972e-02, + 1.31697505e-03, + -5.49139714e-02, + 9.68203187e-04, + -1.70497322e-02, + 0.00000000e00, + ], + [ + 2.75381501e-01, + -1.42169890e-01, + 7.63566820e-03, + -3.18384821e-01, + 5.61352949e-03, + -9.88523645e-02, + 0.00000000e00, + ], + [ + 5.60528715e-01, + 5.34553435e-02, + -1.57068039e-03, + 1.83321890e-01, + -2.33543881e-03, + 6.60980339e-02, + 0.00000000e00, + ], + [ + -1.90658563e-01, + 1.62222678e-01, + -7.67199187e-03, + 0.00000000e00, + -1.13181076e-01, + 1.22260298e-02, + -1.08016484e-01, + ], + [ + 3.28841647e-02, + 9.40548220e-01, + -4.44813167e-02, + 0.00000000e00, + -6.56210717e-01, + 7.08850988e-02, + -6.26267014e-01, + ], + [ + 8.49202631e-18, + -1.93473657e-01, + -2.22673179e-01, + 0.00000000e00, + -4.23235448e-02, + -2.94908598e-02, + -5.45085339e-02, + ], + ], + [ + [ + 5.42292833e-02, + 6.77094759e-02, + 4.83881552e-02, + -1.96475479e-02, + 9.17823507e-03, + -4.57282155e-03, + 0.00000000e00, + ], + [ + -4.03096507e-01, + 3.92571668e-01, + 2.80548897e-01, + -1.13914198e-01, + 5.32143397e-02, + -2.65126876e-02, + 0.00000000e00, + ], + [ + 3.57094795e-01, + -1.47605469e-01, + 2.37914951e-02, + -5.70253726e-01, + 1.08807101e-02, + -2.01830577e-01, + 0.00000000e00, + ], + [ + 7.10033786e-01, + 5.02084106e-02, + 2.11845605e-02, + 1.13181076e-01, + 0.00000000e00, + 1.15898581e-01, + 8.90683876e-04, + ], + [ + -1.22464303e-01, + 2.91102526e-01, + 1.22825619e-01, + 6.56210717e-01, + 0.00000000e00, + 6.71966493e-01, + 5.16408154e-03, + ], + [ + -3.16252545e-17, + 7.20517510e-01, + 6.14864240e-01, + 4.23235448e-02, + 0.00000000e00, + 1.37396662e-01, + 3.93121050e-02, + ], + ], + [ + [ + 4.36046879e-01, + -3.24238553e-02, + -3.87838234e-03, + -5.41101044e-02, + 0.00000000e00, + -1.69651356e-02, + 0.00000000e00, + ], + [ + 2.31723847e-01, + -1.87989743e-01, + -2.24864098e-02, + -3.13724093e-01, + 5.20417043e-18, + -9.83618836e-02, + 0.00000000e00, + ], + [ + 5.70686384e-01, + 7.06834356e-02, + 1.14788448e-02, + 1.97775093e-01, + 8.67361738e-19, + 7.39421042e-02, + 0.00000000e00, + ], + [ + -1.24194307e-01, + 1.60694077e-01, + -1.01445937e-02, + -1.22260298e-02, + -1.15898581e-01, + 0.00000000e00, + -1.10706199e-01, + ], + [ + 2.14206274e-02, + 9.31685572e-01, + -5.88171749e-02, + -7.08850988e-02, + -6.71966493e-01, + 0.00000000e00, + -6.41861667e-01, + ], + [ + 5.53167558e-18, + -1.26028049e-01, + -2.94438391e-01, + 2.94908598e-02, + -1.37396662e-01, + 0.00000000e00, + -1.49560791e-01, + ], + ], + [ + [ + 4.85696011e-02, + 6.44268400e-02, + 4.61905080e-02, + -1.91831542e-02, + 8.76704022e-03, + -4.49833175e-03, + 0.00000000e00, + ], + [ + -4.21413485e-01, + 3.73539326e-01, + 2.67807194e-01, + -1.11221697e-01, + 5.08302797e-02, + -2.60808044e-02, + 0.00000000e00, + ], + [ + 3.59829170e-01, + -1.40449380e-01, + 2.91821673e-02, + -5.64152301e-01, + 1.18440113e-02, + -1.98542822e-01, + 0.00000000e00, + ], + [ + 7.37360525e-01, + 4.91939581e-02, + 2.01575078e-02, + 1.08016484e-01, + -8.90683876e-04, + 1.10706199e-01, + 0.00000000e00, + ], + [ + -1.27177529e-01, + 2.85220849e-01, + 1.16870886e-01, + 6.26267014e-01, + -5.16408154e-03, + 6.41861667e-01, + 0.00000000e00, + ], + [ + -3.28424010e-17, + 7.48247732e-01, + 5.85054890e-01, + 5.45085339e-02, + -3.93121050e-02, + 1.49560791e-01, + 0.00000000e00, + ], + ], + ], + [ + [ + [ + 3.41453006e-02, + 8.08253690e-02, + 3.06951191e-02, + -3.45709946e-01, + 1.01688580e-02, + -1.07973135e-01, + 0.00000000e00, + ], + [ + -4.16474664e-01, + -1.39404950e-02, + -4.24230421e-01, + -2.17748413e-02, + -7.82283735e-02, + -2.81325889e-02, + 0.00000000e00, + ], + [ + 2.42699171e-18, + -3.60000174e-18, + 2.66095901e-18, + 1.61807814e-17, + 2.82387683e-19, + 5.25873718e-18, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 5.39111251e-02, + -6.30795483e-01, + 1.28407717e-01, + -5.99489263e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -9.97525009e-01, + -3.97312016e-02, + -9.87832342e-01, + -3.36043908e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.10071903e-18, + 2.95240677e-17, + 3.56586331e-18, + 2.80188451e-17, + ], + ], + [ + [ + 8.10577678e-19, + -6.87706209e-02, + 3.52650006e-03, + -1.16102817e-02, + 8.03370202e-04, + -3.50123667e-03, + 0.00000000e00, + ], + [ + 4.99661867e-17, + -3.98724063e-01, + 2.04462373e-02, + -6.73150634e-02, + 4.65784701e-03, + -2.02997631e-02, + 0.00000000e00, + ], + [ + 1.04435486e-01, + 8.20187641e-02, + 1.02353729e-01, + -3.36978765e-01, + 2.33171516e-02, + -1.01620481e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.67199187e-03, + -2.11845605e-02, + 1.01445937e-02, + -2.01575078e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 4.44813167e-02, + -1.22825619e-01, + 5.88171749e-02, + -1.16870886e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 2.22673179e-01, + -6.14864240e-01, + 2.94438391e-01, + -5.85054890e-01, + ], + ], + [ + [ + 3.34646679e-02, + 0.00000000e00, + 3.41453006e-02, + -3.52192202e-01, + 1.08915293e-02, + -1.09853801e-01, + 0.00000000e00, + ], + [ + -4.08172899e-01, + 3.81639165e-17, + -4.16474664e-01, + -1.90341873e-02, + -7.68286193e-02, + -2.68762218e-02, + 0.00000000e00, + ], + [ + 8.03838495e-02, + -7.37257477e-18, + 8.20187641e-02, + 1.59367259e-02, + 1.49720700e-02, + 9.15371670e-03, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 6.16735653e-02, + -6.42623254e-01, + 1.37533281e-01, + -6.10758051e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -9.79165111e-01, + -3.47305003e-02, + -9.70156883e-01, + -2.89298617e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.93473657e-01, + 2.90787547e-02, + 1.89060494e-01, + 2.68221308e-02, + ], + ], + [ + [ + -2.67749497e-02, + -4.27379584e-01, + 0.00000000e00, + 3.13469337e-02, + -3.14115013e-04, + 1.21748711e-02, + 0.00000000e00, + ], + [ + 7.32787717e-02, + -2.52079454e-01, + 1.38777878e-17, + -4.97682007e-01, + 4.98707121e-03, + -1.93295280e-01, + 0.00000000e00, + ], + [ + -4.45303046e-02, + -5.99740038e-01, + 0.00000000e00, + 9.83372027e-02, + -9.85397553e-04, + 3.81932978e-02, + 0.00000000e00, + ], + [ + -5.39111251e-02, + -7.67199187e-03, + -6.16735653e-02, + 0.00000000e00, + -4.71705446e-02, + -3.96650160e-03, + -4.87735455e-02, + ], + [ + 9.97525009e-01, + -4.44813167e-02, + 9.79165111e-01, + 0.00000000e00, + 7.48906785e-01, + 6.29744683e-02, + 7.74356953e-01, + ], + [ + -7.10071903e-18, + -2.22673179e-01, + -1.93473657e-01, + 0.00000000e00, + -1.47976815e-01, + -1.24431524e-02, + -1.53005525e-01, + ], + ], + [ + [ + 8.02939201e-02, + -8.68221386e-02, + 2.61157664e-02, + -5.95998728e-01, + 1.16033016e-02, + -2.10879959e-01, + 0.00000000e00, + ], + [ + -2.79925137e-01, + 2.37618361e-01, + -3.18537393e-01, + -3.22106832e-02, + -5.85848803e-02, + -2.74121559e-02, + 0.00000000e00, + ], + [ + 1.38239466e-01, + -1.44396771e-01, + 6.27314110e-02, + 2.69689911e-02, + 1.13031672e-02, + 1.27415350e-02, + 0.00000000e00, + ], + [ + 6.30795483e-01, + 2.11845605e-02, + 6.42623254e-01, + 4.71705446e-02, + 0.00000000e00, + 1.46521218e-01, + 4.10747232e-02, + ], + [ + 3.97312016e-02, + 1.22825619e-01, + 3.47305003e-02, + -7.48906785e-01, + 0.00000000e00, + -7.39783239e-01, + 5.33927794e-03, + ], + [ + -2.95240677e-17, + 6.14864240e-01, + -2.90787547e-02, + 1.47976815e-01, + 0.00000000e00, + 1.42731258e-01, + -2.48176748e-03, + ], + ], + [ + [ + -3.09721687e-02, + -4.14953750e-01, + 2.19603632e-03, + 4.72532385e-02, + 1.73472348e-18, + 2.00850184e-02, + 0.00000000e00, + ], + [ + 4.28372826e-02, + -2.78984158e-01, + -2.67853401e-02, + -4.94327552e-01, + -5.20417043e-18, + -1.93245510e-01, + 0.00000000e00, + ], + [ + -4.82359054e-02, + -5.79629005e-01, + 5.27499193e-03, + 9.71190784e-02, + 1.38777878e-17, + 3.79108193e-02, + 0.00000000e00, + ], + [ + -1.28407717e-01, + -1.01445937e-02, + -1.37533281e-01, + 3.96650160e-03, + -1.46521218e-01, + 0.00000000e00, + -1.48046549e-01, + ], + [ + 9.87832342e-01, + -5.88171749e-02, + 9.70156883e-01, + -6.29744683e-02, + 7.39783239e-01, + 0.00000000e00, + 7.65372332e-01, + ], + [ + -3.56586331e-18, + -2.94438391e-01, + -1.89060494e-01, + 1.24431524e-02, + -1.42731258e-01, + 0.00000000e00, + -1.47790395e-01, + ], + ], + [ + [ + 7.85407802e-02, + -8.07082629e-02, + 2.70032608e-02, + -5.88956619e-01, + 1.17240955e-02, + -2.07444792e-01, + 0.00000000e00, + ], + [ + -2.92451072e-01, + 2.26787537e-01, + -3.29362278e-01, + -2.97571131e-02, + -6.06113306e-02, + -2.69656206e-02, + 0.00000000e00, + ], + [ + 1.36676893e-01, + -1.34689562e-01, + 6.48632182e-02, + 2.62362355e-02, + 1.17038102e-02, + 1.25339795e-02, + 0.00000000e00, + ], + [ + 5.99489263e-01, + 2.01575078e-02, + 6.10758051e-01, + 4.87735455e-02, + -4.10747232e-02, + 1.48046549e-01, + 0.00000000e00, + ], + [ + 3.36043908e-02, + 1.16870886e-01, + 2.89298617e-02, + -7.74356953e-01, + -5.33927794e-03, + -7.65372332e-01, + 0.00000000e00, + ], + [ + -2.80188451e-17, + 5.85054890e-01, + -2.68221308e-02, + 1.53005525e-01, + 2.48176748e-03, + 1.47790395e-01, + 0.00000000e00, + ], + ], + ], + [ + [ + [ + -3.35029257e-01, + 5.06596965e-01, + -3.45709946e-01, + 9.65242924e-02, + -1.45842251e-03, + 3.24608603e-02, + 0.00000000e00, + ], + [ + 8.04745724e-02, + -8.73761859e-02, + -2.17748413e-02, + -5.18935898e-01, + 5.28476698e-03, + -2.00682834e-01, + 0.00000000e00, + ], + [ + 1.47042021e-17, + -2.25640783e-17, + 1.61807814e-17, + 5.30332170e-19, + 1.65636617e-20, + 4.29940126e-19, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 8.48905588e-02, + -1.84162966e-02, + 7.71850655e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.41195745e-01, + 6.67336352e-02, + 7.68210453e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.10485573e-17, + 2.09158392e-19, + -1.09523226e-17, + ], + ], + [ + [ + 2.35023767e-17, + 3.80041092e-03, + -1.16102817e-02, + -5.49139714e-02, + 9.68203187e-04, + -1.70497322e-02, + 0.00000000e00, + ], + [ + -1.27668360e-17, + 2.20343406e-02, + -6.73150634e-02, + -3.18384821e-01, + 5.61352949e-03, + -9.88523645e-02, + 0.00000000e00, + ], + [ + -3.43832524e-01, + 5.14076923e-01, + -3.36978765e-01, + 1.83321890e-01, + -2.33543881e-03, + 6.60980339e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.13181076e-01, + 1.22260298e-02, + -1.08016484e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.56210717e-01, + 7.08850988e-02, + -6.26267014e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -4.23235448e-02, + -2.94908598e-02, + -5.45085339e-02, + ], + ], + [ + [ + -3.28350978e-01, + 5.00876302e-01, + -3.52192202e-01, + 3.13469337e-02, + -3.14115013e-04, + 1.21748711e-02, + 0.00000000e00, + ], + [ + 7.88704387e-02, + -8.63895046e-02, + -1.90341873e-02, + -4.97682007e-01, + 4.98707121e-03, + -1.93295280e-01, + 0.00000000e00, + ], + [ + -4.44218266e-03, + -6.93889390e-18, + 1.59367259e-02, + 9.83372027e-02, + -9.85397553e-04, + 3.81932978e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -4.71705446e-02, + -3.96650160e-03, + -4.87735455e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.48906785e-01, + 6.29744683e-02, + 7.74356953e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.47976815e-01, + -1.24431524e-02, + -1.53005525e-01, + ], + ], + [ + [ + 6.48193356e-02, + -7.61193490e-02, + 0.00000000e00, + -3.35029257e-01, + 5.85989604e-03, + -1.04502264e-01, + 0.00000000e00, + ], + [ + -1.55697098e-02, + 1.82839914e-02, + 1.90819582e-17, + 8.04745724e-02, + -6.71145042e-04, + 3.26402475e-02, + 0.00000000e00, + ], + [ + -9.94603424e-02, + 1.16799354e-01, + 9.71445147e-17, + 5.14076923e-01, + -5.26460556e-03, + 1.98503607e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.68449878e-01, + 7.39961036e-02, + -6.37206328e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.84470262e-01, + -8.47491453e-03, + -1.87822895e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.20517510e-01, + -6.64790460e-02, + -7.47444596e-01, + ], + ], + [ + [ + 5.61448369e-02, + 8.05790870e-01, + 2.26019439e-02, + 2.77555756e-17, + 5.26713933e-02, + 1.42461283e-02, + 0.00000000e00, + ], + [ + 1.65648643e-01, + 2.46157927e-02, + 1.13501279e-01, + 8.32667268e-17, + 1.45355786e-02, + 3.93146458e-03, + 0.00000000e00, + ], + [ + 5.26562926e-01, + 1.50442818e-01, + 5.40897217e-01, + 1.52655666e-16, + 5.67741313e-02, + 1.53558033e-02, + 0.00000000e00, + ], + [ + -8.48905588e-02, + 1.13181076e-01, + 4.71705446e-02, + 6.68449878e-01, + 0.00000000e00, + 6.65110413e-01, + -2.77482876e-03, + ], + [ + -7.41195745e-01, + 6.56210717e-01, + -7.48906785e-01, + 1.84470262e-01, + 0.00000000e00, + 1.83548679e-01, + -7.65761810e-04, + ], + [ + 1.10485573e-17, + 4.23235448e-02, + 1.47976815e-01, + 7.20517510e-01, + 0.00000000e00, + 7.16917924e-01, + -2.99096877e-03, + ], + ], + [ + [ + 1.00823383e-02, + -9.12151146e-02, + -5.50296030e-02, + -3.33355506e-01, + 6.93889390e-18, + -1.05557206e-01, + 0.00000000e00, + ], + [ + -2.17722592e-02, + 3.75026544e-02, + -3.48488893e-03, + 8.00725347e-02, + 1.51788304e-18, + 3.26578012e-02, + 0.00000000e00, + ], + [ + -9.16405905e-02, + 1.94212992e-01, + 3.02504923e-03, + 5.11508680e-01, + 5.20417043e-18, + 1.98928729e-01, + 0.00000000e00, + ], + [ + 1.84162966e-02, + -1.22260298e-02, + 3.96650160e-03, + -7.39961036e-02, + -6.65110413e-01, + 0.00000000e00, + -6.33715782e-01, + ], + [ + -6.67336352e-02, + -7.08850988e-02, + -6.29744683e-02, + 8.47491453e-03, + -1.83548679e-01, + 0.00000000e00, + -1.86919743e-01, + ], + [ + -2.09158392e-19, + 2.94908598e-02, + 1.24431524e-02, + 6.64790460e-02, + -7.16917924e-01, + 0.00000000e00, + -7.43986450e-01, + ], + ], + [ + [ + 3.32773173e-02, + 7.98996386e-01, + -2.22953651e-04, + 1.39075322e-03, + 5.01851912e-02, + 1.40140634e-02, + 0.00000000e00, + ], + [ + 1.62967320e-01, + 3.25210512e-02, + 1.11958911e-01, + -3.34061184e-04, + 1.48025397e-02, + 3.86742228e-03, + 0.00000000e00, + ], + [ + 5.29555141e-01, + 1.82151411e-01, + 5.41683062e-01, + -2.13400507e-03, + 5.89177408e-02, + 1.51056622e-02, + 0.00000000e00, + ], + [ + -7.71850655e-02, + 1.08016484e-01, + 4.87735455e-02, + 6.37206328e-01, + 2.77482876e-03, + 6.33715782e-01, + 0.00000000e00, + ], + [ + -7.68210453e-01, + 6.26267014e-01, + -7.74356953e-01, + 1.87822895e-01, + 7.65761810e-04, + 1.86919743e-01, + 0.00000000e00, + ], + [ + 1.09523226e-17, + 5.45085339e-02, + 1.53005525e-01, + 7.47444596e-01, + 2.99096877e-03, + 7.43986450e-01, + 0.00000000e00, + ], + ], + ], + [ + [ + [ + 1.07195463e-02, + 9.83518983e-03, + 1.01688580e-02, + -1.45842251e-03, + 1.05221866e-03, + -2.09794626e-01, + 0.00000000e00, + ], + [ + -7.78257566e-02, + -1.69634134e-03, + -7.82283735e-02, + 5.28476698e-03, + -5.79159088e-02, + -2.88966443e-02, + 0.00000000e00, + ], + [ + 2.53075280e-19, + -4.38064199e-19, + 2.82387683e-19, + 1.65636617e-20, + 5.08258833e-19, + 9.97012277e-18, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.32869389e-02, + 4.08633244e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.31335770e-01, + 5.62842325e-03, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 6.41806152e-18, + -1.94195804e-18, + ], + ], + [ + [ + -1.61086476e-19, + -1.27256780e-02, + 8.03370202e-04, + 9.68203187e-04, + 9.17823507e-03, + -4.57282155e-03, + 0.00000000e00, + ], + [ + 9.37818968e-18, + -7.37820011e-02, + 4.65784701e-03, + 5.61352949e-03, + 5.32143397e-02, + -2.65126876e-02, + 0.00000000e00, + ], + [ + 2.37913955e-02, + 9.98040745e-03, + 2.33171516e-02, + -2.33543881e-03, + 1.08807101e-02, + -2.01830577e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.15898581e-01, + 8.90683876e-04, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 6.71966493e-01, + 5.16408154e-03, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.37396662e-01, + 3.93121050e-02, + ], + ], + [ + [ + 1.05058690e-02, + -5.01907994e-03, + 1.08915293e-02, + -3.14115013e-04, + 1.16033016e-02, + -2.10879959e-01, + 0.00000000e00, + ], + [ + -7.62744229e-02, + 8.65674476e-04, + -7.68286193e-02, + 4.98707121e-03, + -5.85848803e-02, + -2.74121559e-02, + 0.00000000e00, + ], + [ + 1.48746510e-02, + -1.24683250e-18, + 1.49720700e-02, + -9.85397553e-04, + 1.13031672e-02, + 1.27415350e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.46521218e-01, + 4.10747232e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.39783239e-01, + 5.33927794e-03, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.42731258e-01, + -2.48176748e-03, + ], + ], + [ + [ + -2.07394982e-03, + -7.52143130e-02, + 2.66073585e-03, + 5.85989604e-03, + 5.26713933e-02, + 1.42461283e-02, + 0.00000000e00, + ], + [ + 1.50572337e-02, + -4.28938883e-03, + 1.54266371e-02, + -6.71145042e-04, + 1.45355786e-02, + 3.93146458e-03, + 0.00000000e00, + ], + [ + 7.68653783e-02, + 2.26756948e-03, + 7.72256438e-02, + -5.26460556e-03, + 5.67741313e-02, + 1.53558033e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 6.65110413e-01, + -2.77482876e-03, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.83548679e-01, + -7.65761810e-04, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.16917924e-01, + -2.99096877e-03, + ], + ], + [ + [ + 7.72362080e-03, + -6.72512034e-03, + 8.19876123e-03, + 8.67361738e-19, + 1.07195463e-02, + -1.56123717e-01, + 0.00000000e00, + ], + [ + -5.60748204e-02, + 4.88255347e-02, + -5.95244219e-02, + -6.07153217e-18, + -7.78257566e-02, + -2.83646224e-03, + 0.00000000e00, + ], + [ + 7.19105833e-03, + -6.26140691e-03, + 7.63343666e-03, + 7.58941521e-19, + 9.98040745e-03, + 1.45567764e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.35361558e-01, + 3.04094258e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -9.82748279e-01, + 5.52479723e-04, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 1.26028049e-01, + -2.83533610e-02, + ], + ], + [ + [ + -3.06034236e-01, + -4.98379189e-01, + -2.79535843e-01, + 6.74871090e-02, + -6.93889390e-18, + 1.07195463e-02, + 0.00000000e00, + ], + [ + -1.00442146e-01, + -1.08393727e-01, + -7.27677599e-02, + -1.47405316e-01, + -8.67361738e-18, + -7.78257566e-02, + 0.00000000e00, + ], + [ + -2.50991746e-01, + -4.53195110e-01, + -2.26781234e-01, + 5.79144367e-02, + -6.93889390e-18, + 9.98040745e-03, + 0.00000000e00, + ], + [ + -1.32869389e-02, + -1.15898581e-01, + -1.46521218e-01, + -6.65110413e-01, + -1.35361558e-01, + 0.00000000e00, + -1.35244491e-01, + ], + [ + 7.31335770e-01, + -6.71966493e-01, + 7.39783239e-01, + -1.83548679e-01, + 9.82748279e-01, + 0.00000000e00, + 9.81898349e-01, + ], + [ + -6.41806152e-18, + -1.37396662e-01, + -1.42731258e-01, + -7.16917924e-01, + -1.26028049e-01, + 0.00000000e00, + -1.25919053e-01, + ], + ], + [ + [ + 5.67937662e-03, + -6.39875381e-05, + 9.64647527e-03, + 2.13841071e-02, + 1.07102755e-02, + -1.53580512e-01, + 0.00000000e00, + ], + [ + -5.90802231e-02, + 4.83482186e-02, + -6.12645420e-02, + 4.97217187e-03, + -7.77584490e-02, + -2.79025718e-03, + 0.00000000e00, + ], + [ + 3.37578191e-03, + 1.97430655e-03, + 8.84164962e-03, + 1.31960342e-02, + 9.97177590e-03, + 1.43196511e-01, + 0.00000000e00, + ], + [ + -4.08633244e-02, + -8.90683876e-04, + -4.10747232e-02, + 2.77482876e-03, + -3.04094258e-02, + 1.35244491e-01, + 0.00000000e00, + ], + [ + -5.62842325e-03, + -5.16408154e-03, + -5.33927794e-03, + 7.65761810e-04, + -5.52479723e-04, + -9.81898349e-01, + 0.00000000e00, + ], + [ + 1.94195804e-18, + -3.93121050e-02, + 2.48176748e-03, + 2.99096877e-03, + 2.83533610e-02, + 1.25919053e-01, + 0.00000000e00, + ], + ], + ], + [ + [ + [ + -1.03187865e-01, + 1.99678676e-01, + -1.07973135e-01, + 3.24608603e-02, + -2.09794626e-01, + 4.04324654e-02, + 0.00000000e00, + ], + [ + 1.17720983e-02, + -3.44399243e-02, + -2.81325889e-02, + -2.00682834e-01, + -2.88966443e-02, + -2.00614904e-01, + 0.00000000e00, + ], + [ + 4.65397353e-18, + -8.89378659e-18, + 5.25873718e-18, + 4.29940126e-19, + 9.97012277e-18, + 6.10077242e-20, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -2.09458434e-02, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.58497864e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.32539630e-18, + ], + ], + [ + [ + 7.34083974e-18, + -1.00921994e-03, + -3.50123667e-03, + -1.70497322e-02, + -4.57282155e-03, + -1.69651356e-02, + 0.00000000e00, + ], + [ + -2.38093275e-18, + -5.85133986e-03, + -2.02997631e-02, + -9.88523645e-02, + -2.65126876e-02, + -9.83618836e-02, + 0.00000000e00, + ], + [ + -1.03687324e-01, + 2.02626953e-01, + -1.01620481e-01, + 6.60980339e-02, + -2.01830577e-01, + 7.39421042e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.10706199e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.41861667e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.49560791e-01, + ], + ], + [ + [ + -1.01130978e-01, + 1.94535915e-01, + -1.09853801e-01, + 1.21748711e-02, + -2.10879959e-01, + 2.00850184e-02, + 0.00000000e00, + ], + [ + 1.15374401e-02, + -3.35529177e-02, + -2.68762218e-02, + -1.93295280e-01, + -2.74121559e-02, + -1.93245510e-01, + 0.00000000e00, + ], + [ + 1.17964594e-03, + -3.46944695e-18, + 9.15371670e-03, + 3.81932978e-02, + 1.27415350e-02, + 3.79108193e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.48046549e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 7.65372332e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.47790395e-01, + ], + ], + [ + [ + 1.99641336e-02, + -4.44465872e-02, + 7.19654101e-04, + -1.04502264e-01, + 1.42461283e-02, + -1.05557206e-01, + 0.00000000e00, + ], + [ + -2.27759091e-03, + 6.29701679e-03, + 4.17247079e-03, + 3.26402475e-02, + 3.93146458e-03, + 3.26578012e-02, + 0.00000000e00, + ], + [ + -1.78907915e-02, + 4.60372682e-02, + 2.08873614e-02, + 1.98503607e-01, + 1.53558033e-02, + 1.98928729e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.33715782e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.86919743e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.43986450e-01, + ], + ], + [ + [ + -7.43486637e-02, + 1.42776647e-01, + -8.15965276e-02, + 4.88405015e-03, + -1.56123717e-01, + 1.07195463e-02, + 0.00000000e00, + ], + [ + 8.48200297e-03, + -2.08454900e-02, + -6.50029553e-03, + -7.75419986e-02, + -2.83646224e-03, + -7.78257566e-02, + 0.00000000e00, + ], + [ + 6.68043138e-02, + -1.27122045e-01, + 7.73642547e-02, + 1.53215570e-02, + 1.45567764e-01, + 9.98040745e-03, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.35244491e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 9.81898349e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.25919053e-01, + ], + ], + [ + [ + 1.30045653e-02, + -3.10004133e-02, + -6.63647108e-03, + -1.02672356e-01, + 3.46944695e-18, + -1.03187865e-01, + 0.00000000e00, + ], + [ + -1.48361458e-03, + 3.53665533e-03, + 7.57116061e-04, + 1.17132869e-02, + 7.80625564e-18, + 1.17720983e-02, + 0.00000000e00, + ], + [ + -2.55366795e-02, + 6.08745931e-02, + 1.30318416e-02, + 2.01614663e-01, + 5.20417043e-17, + 2.02626953e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -6.37462344e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -1.83758244e-01, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.48247732e-01, + ], + ], + [ + [ + 2.13823046e-01, + 5.50959991e-01, + 1.84972186e-01, + -5.73283670e-02, + -1.11440324e-01, + 5.20417043e-18, + 0.00000000e00, + ], + [ + 1.14119484e-01, + 8.61759621e-02, + 7.65324714e-02, + 8.54932885e-02, + 1.00807085e-02, + 1.38777878e-17, + 0.00000000e00, + ], + [ + 4.05916016e-01, + 3.51404343e-01, + 3.90608748e-01, + -5.18727125e-02, + 1.98301371e-01, + 4.51028104e-17, + 0.00000000e00, + ], + [ + 2.09458434e-02, + 1.10706199e-01, + 1.48046549e-01, + 6.33715782e-01, + 1.35244491e-01, + 6.37462344e-01, + 0.00000000e00, + ], + [ + -7.58497864e-01, + 6.41861667e-01, + -7.65372332e-01, + 1.86919743e-01, + -9.81898349e-01, + 1.83758244e-01, + 0.00000000e00, + ], + [ + 6.32539630e-18, + 1.49560791e-01, + 1.47790395e-01, + 7.43986450e-01, + 1.25919053e-01, + 7.48247732e-01, + 0.00000000e00, + ], + ], + ], + [ + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + ], + ] + ) + + fk = r.partial_fkine0(q1, 3) + fk2 = r2.partial_fkine0(q1, 3) + + nt.assert_almost_equal(fk, ans) + nt.assert_almost_equal(fk2, ans) + + def test_qlim1(self): + rx = rtb.ETS(rtb.ET.Rx()) + + q = rx.qlim + nt.assert_equal(q, np.array([[-np.pi], [np.pi]])) + + def test_qlim2(self): + rx = rtb.ETS(rtb.ET.Rx(qlim=[-1, 1])) + + q = rx.qlim + nt.assert_equal(q, np.array([[-1], [1]])) + + def test_qlim3(self): + rx = rtb.ETS(rtb.ET.tx(qlim=[-1, 1])) + + q = rx.qlim + nt.assert_equal(q, np.array([[-1], [1]])) + + def test_qlim4(self): + rx = rtb.ETS(rtb.ET.tx()) + + with self.assertRaises(ValueError): + rx.qlim + def test_random_q(self): + rx = rtb.ETS(rtb.ET.Rx(qlim=[-1, 1])) + + q = rx.random_q() + self.assertTrue(-1 <= q <= 1) + + def test_random_q2(self): + rx = rtb.ETS([rtb.ET.Rx(qlim=[-1, 1]), rtb.ET.Rx(qlim=[1, 2])]) + + q = rx.random_q(10) + + self.assertTrue(np.all(-1 <= q[:, 0]) and np.all(q[:, 0] <= 1)) + self.assertTrue(np.all(1 <= q[:, 1]) and np.all(q[:, 1] <= 2)) + + def test_manip(self): + r = rtb.models.Panda() + ets = r.ets() + q = r.qr + + m1 = ets.manipulability(q) + m2 = ets.manipulability(q, axes="trans") + m3 = ets.manipulability(q, axes="rot") + + nt.assert_almost_equal(m1, 0.0837, decimal=4) + nt.assert_almost_equal(m2, 0.1438, decimal=4) + nt.assert_almost_equal(m3, 2.7455, decimal=4) + + def test_yoshi(self): + puma = rtb.models.Puma560() + ets = puma.ets() + q = puma.qn # type: ignore + + m1 = ets.manipulability(q, axes=[True, True, True, True, True, True]) + m2 = ets.manipulability(np.c_[q, q].T) + m3 = ets.manipulability(q, axes="trans") + m4 = ets.manipulability(q, axes="rot") + + a0 = 0.0805 + a2 = 0.1354 + a3 = 2.44949 + + nt.assert_almost_equal(m1, a0, decimal=4) + nt.assert_almost_equal(m2[0], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m2[1], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m3, a2, decimal=4) + nt.assert_almost_equal(m4, a3, decimal=4) + + with self.assertRaises(ValueError): + puma.manipulability(axes="abcdef") # type: ignore + + def test_cond(self): + r = rtb.models.Panda() + ets = r.ets() + + m = ets.manipulability(r.qr, method="invcondition") + + self.assertAlmostEqual(m, 0.11222, places=4) # type: ignore + + def test_minsingular(self): + r = rtb.models.Panda() + ets = r.ets() + + m = ets.manipulability(r.qr, method="minsingular") + + self.assertAlmostEqual(m, 0.209013, places=4) # type: ignore + + def test_manipulability_fail(self): + puma = rtb.models.Puma560() + ets = puma.ets() + + with self.assertRaises(ValueError): + ets.manipulability(q=[1, 2, 3.0], method="notamethod") # type: ignore + + def test_manip_fail2(self): + r = rtb.models.Panda() + ets = r.ets() + q = r.qr + + with self.assertRaises(ValueError): + ets.manipulability(q, axes="abcdef") # type: ignore + + +if __name__ == "__main__": unittest.main() diff --git a/tests/test_ETS2.py b/tests/test_ETS2.py index 9ef158e2a..7dea08126 100644 --- a/tests/test_ETS2.py +++ b/tests/test_ETS2.py @@ -1,3 +1,4 @@ + #!/usr/bin/env python3 # -*- coding: utf-8 -*- """ diff --git a/tests/test_Gripper.py b/tests/test_Gripper.py index c5fda94ba..135db0422 100644 --- a/tests/test_Gripper.py +++ b/tests/test_Gripper.py @@ -3,6 +3,9 @@ """ import roboticstoolbox as rtb +from spatialmath import SE3 +import numpy.testing as nt +import numpy as np import unittest @@ -14,7 +17,7 @@ def test_jindex_1(self): e5 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=0, parent=e3) e7 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e5) - rtb.ERobot([e1, e2, e3, e5, e7], gripper_links=e5) + rtb.Robot([e1, e2, e3, e5, e7], gripper_links=e5) def test_jindex_2(self): e1 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=0) @@ -23,7 +26,7 @@ def test_jindex_2(self): e4 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=1, parent=e3) with self.assertRaises(ValueError): - rtb.ERobot([e1, e2, e3, e4], gripper_links=e4) + rtb.Robot([e1, e2, e3, e4], gripper_links=e4) def test_jindex_3(self): e1 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=0) @@ -33,7 +36,7 @@ def test_jindex_3(self): e5 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e4) with self.assertRaises(ValueError): - rtb.ERobot([e1, e2, e3, e4, e5], gripper_links=e4) + rtb.Robot([e1, e2, e3, e4, e5], gripper_links=e4) def test_jindex_4(self): e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) @@ -44,4 +47,69 @@ def test_jindex_4(self): e6 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e5) e7 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e6) - rtb.ERobot([e1, e2, e3, e4, e5, e6, e7], gripper_links=e3) + rtb.Robot([e1, e2, e3, e4, e5, e6, e7], gripper_links=e3) + + def test_gripper_args(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + tool = SE3.Rx(1.0) + + g1 = rtb.Gripper([e2, e3], tool=tool) + + nt.assert_almost_equal(g1.tool.A, tool.A) + + def test_gripper_args2(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + tool = SE3.Rx(1.0) + + g2 = rtb.Gripper([e2, e3], tool=tool.A) + + nt.assert_almost_equal(g2.tool.A, tool.A) + + def test_init_fail(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz()), jindex=1) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1, jindex=2) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + with self.assertRaises(ValueError): + rtb.Gripper([e2, e3]) + + def test_str(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + tool = SE3.Rx(1.0) + + g1 = rtb.Gripper([e2, e3], tool=tool) + + s = g1.__str__() + + self.assertTrue(s.startswith('Gripper("", connected to , 2 joints, 2 links)')) + + def test_repr(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + g1 = rtb.Gripper([e2, e3]) + + s = g1.__repr__() + + ans = """Gripper(['Link([ET.Rz(jindex=0)], name = "", parent="")', 'Link([ET.Rz(jindex=0)], name = "", parent="")'], name="", tool=None)""" # noqa + + self.assertEqual(s, ans) + + def test_q(self): + e1 = rtb.Link(rtb.ETS(rtb.ET.Rz())) + e2 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e1) + e3 = rtb.Link(rtb.ETS(rtb.ET.Rz()), parent=e2) + + g1 = rtb.Gripper([e2, e3]) + + nt.assert_almost_equal(g1.q, np.zeros(2)) diff --git a/tests/test_IK.py b/tests/test_IK.py new file mode 100644 index 000000000..aacfe1029 --- /dev/null +++ b/tests/test_IK.py @@ -0,0 +1,846 @@ +""" +@author: Jesse Haviland +""" + +# import numpy.testing as nt +import roboticstoolbox as rtb +import numpy as np +import unittest +import numpy.testing as nt + +# import sympy +import pytest + +test_tol = 1e-5 + + +class TestIK(unittest.TestCase): + def test_IK_NR1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, tol=tol) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_NR2(self): + + q0 = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0] + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, tol=tol) + + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_NR3(self): + + q0 = [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0] + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, tol=tol) + + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_NR4(self): + + q0 = np.array( + [ + [1.0, 2.0, 3.0, 4.0, 5.0, 6.0, 7.0], + [1.0, 2.0, 1.0, 2.0, 1.0, 2.0, 1.0], + [1.0, 2.0, 3.0, 1.0, 2.0, 3.0, 1.0], + [2.0, 2.0, 2.0, 2.0, 2.0, 2.0, 2.0], + [0.0, -0.3, 0.0, -2.2, 0.0, 2.0, np.pi / 4], + ] + ) + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, tol=tol, slimit=5) + + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_NR5(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, tol=tol) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep.A, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_NR6(self): + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=False, slimit=1) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, False) + + def test_IK_NR7(self): + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -1.3, 0, 1.2, 0, 2.0, 0.1]) + + solver = rtb.IK_NR(joint_limits=True, seed=0, pinv=True, ilimit=2, slimit=1) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, False) + + @pytest.mark.filterwarnings("ignore::RuntimeWarning") + def test_IK_NR8(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR( + joint_limits=True, + seed=0, + pinv=True, + tol=tol, + kq=0.01, + km=1.0, + ) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + @pytest.mark.filterwarnings("ignore::RuntimeWarning") + def test_IK_LM1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_LM( + method="chan", joint_limits=True, seed=0, tol=tol, kq=0.1, km=0.1 + ) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_LM2(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_LM( + method="sugihara", k=0.0001, joint_limits=True, seed=0, tol=tol + ) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_LM3(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_LM( + method="wampler", k=0.001, joint_limits=True, seed=0, tol=tol + ) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + @pytest.mark.filterwarnings("ignore::RuntimeWarning") + def test_IK_GN1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_GN( + pinv=True, joint_limits=True, seed=0, tol=tol, kq=1.0, km=1.0 + ) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_GN2(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_GN(pinv=True, joint_limits=True, seed=0, tol=tol) + + sol = solver.solve(panda, Tep) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_GN3(self): + + tol = 1e-6 + + ur5 = rtb.models.UR5().ets() + + Tep = ur5.eval([0, -0.3, 0, -2.2, 0, 2.0]) + + solver = rtb.IK_GN(pinv=False, joint_limits=True, seed=0, tol=tol) + + sol = solver.solve(ur5, Tep) + + self.assertEqual(sol.success, True) + + Tq = ur5.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_QP1(self): + + q0 = np.array( + [ + -1.66441371, + -1.20998727, + 1.04248366, + -2.10222463, + 1.05097407, + 1.41173279, + 0.0053529, + ] + ) + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_QP(joint_limits=True, seed=0, tol=tol, kq=2.0, km=100.0) + + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_QP2(self): + + q0 = np.array( + [ + -1.66441371, + -1.20998727, + 1.04248366, + -2.10222463, + 1.05097407, + 1.41173279, + 0.0053529, + ] + ) + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_QP(joint_limits=True, seed=0, tol=tol) + + sol = solver.solve(panda, Tep, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_IK_QP3(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_QP( + joint_limits=True, + seed=0, + tol=tol, + kq=1000.0, + pi=4.0, + ps=2.0, + kj=10000.0, + slimit=1, + ) + + try: + solver.solve(panda, Tep) + except BaseException: + pass + + # self.assertEqual(sol.success, False) + + def test_ets_ikine_NR1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + panda2 = rtb.models.Panda() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR() + + sol = panda.ikine_NR(Tep, pinv=True, tol=tol) + sol2 = panda2.ikine_NR(Tep, pinv=True, tol=tol) + + self.assertEqual(sol.success, True) + self.assertEqual(sol2.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_NR2(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_NR() + + sol = panda.ikine_NR(Tep, pinv=True, tol=tol) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep.A, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_LM1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + panda2 = rtb.models.Panda() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_LM() + + sol = panda.ikine_LM(Tep, tol=tol) + sol2 = panda2.ikine_LM(Tep, tol=tol) + + self.assertEqual(sol.success, True) + self.assertEqual(sol2.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_LM2(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_LM() + + sol = panda.ikine_LM(Tep, tol=tol) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep.A, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_GN1(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + panda2 = rtb.models.Panda() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_GN() + + sol = panda.ikine_GN(Tep, pinv=True, tol=tol) + sol2 = panda2.ikine_GN(Tep, pinv=True, tol=tol) + + self.assertEqual(sol.success, True) + self.assertEqual(sol2.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_GN2(self): + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_GN() + + sol = panda.ikine_GN(Tep, pinv=True, tol=tol) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep.A, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_QP1(self): + + q0 = np.array( + [ + -1.66441371, + -1.20998727, + 1.04248366, + -2.10222463, + 1.05097407, + 1.41173279, + 0.0053529, + ] + ) + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + + Tep = panda.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_QP() + + sol = panda.ikine_QP(Tep, tol=tol, q0=q0) + + self.assertEqual(sol.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep, Tq) + + self.assertGreater(test_tol, E) + + def test_ets_ikine_QP2(self): + + q0 = np.array( + [ + -1.66441371, + -1.20998727, + 1.04248366, + -2.10222463, + 1.05097407, + 1.41173279, + 0.0053529, + ] + ) + + tol = 1e-6 + + panda = rtb.models.Panda().ets() + panda2 = rtb.models.Panda() + + Tep = panda.fkine([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + solver = rtb.IK_QP() + + sol = panda.ikine_QP(Tep, tol=tol, q0=q0) + sol2 = panda2.ikine_QP(Tep, tol=tol, q0=q0) + + self.assertEqual(sol.success, True) + self.assertEqual(sol2.success, True) + + Tq = panda.eval(sol.q) + + _, E = solver.error(Tep.A, Tq) + + self.assertGreater(test_tol, E) + + def test_ik_nr(self): + + tol = 1e-6 + + solver = rtb.IK_LM() + + r = rtb.models.Panda().ets() + r2 = rtb.models.Panda() + + Tep = r.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + sol = r.ik_NR(Tep, tol=tol) + sol2 = r2.ik_NR(Tep, tol=tol) + + self.assertEqual(sol[1], True) + self.assertEqual(sol2[1], True) + + Tq = r.eval(sol[0]) + Tq2 = r.eval(sol2[0]) + + _, E = solver.error(Tep, Tq) + _, E2 = solver.error(Tep, Tq2) + + self.assertGreater(test_tol, E) + self.assertGreater(test_tol, E2) + + def test_ik_lm_chan(self): + + tol = 1e-6 + + solver = rtb.IK_LM() + + r = rtb.models.Panda().ets() + r2 = rtb.models.Panda() + + Tep = r.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + sol = r.ik_LM(Tep, tol=tol, method="chan") + sol2 = r2.ik_LM(Tep, tol=tol, method="chan") + + self.assertEqual(sol[1], True) + self.assertEqual(sol2[1], True) + + Tq = r.eval(sol[0]) + Tq2 = r.eval(sol2[0]) + + _, E = solver.error(Tep, Tq) + _, E2 = solver.error(Tep, Tq2) + + self.assertGreater(test_tol, E) + self.assertGreater(test_tol, E2) + + def test_ik_lm_wampler(self): + + tol = 1e-6 + + solver = rtb.IK_LM() + + r = rtb.models.Panda().ets() + r2 = rtb.models.Panda() + + Tep = r.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + sol = r.ik_LM(Tep, tol=tol, method="wampler", k=0.01) + sol2 = r2.ik_LM(Tep, tol=tol, method="wampler", k=0.01) + + self.assertEqual(sol[1], True) + self.assertEqual(sol2[1], True) + + Tq = r.eval(sol[0]) + Tq2 = r.eval(sol2[0]) + + _, E = solver.error(Tep, Tq) + _, E2 = solver.error(Tep, Tq2) + + self.assertGreater(test_tol, E) + self.assertGreater(test_tol, E2) + + def test_ik_lm_sugihara(self): + + tol = 1e-6 + + solver = rtb.IK_LM() + + r = rtb.models.Panda().ets() + r2 = rtb.models.Panda() + + Tep = r.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + sol = r.ik_LM(Tep, tol=tol, k=0.01, method="sugihara") + sol2 = r2.ik_LM(Tep, tol=tol, k=0.01, method="sugihara") + + self.assertEqual(sol[1], True) + self.assertEqual(sol2[1], True) + + Tq = r.eval(sol[0]) + Tq2 = r.eval(sol2[0]) + + _, E = solver.error(Tep, Tq) + _, E2 = solver.error(Tep, Tq2) + + self.assertGreater(test_tol, E) + self.assertGreater(test_tol, E2) + + def test_ik_gn(self): + + tol = 1e-6 + + solver = rtb.IK_LM() + + r = rtb.models.Panda().ets() + r2 = rtb.models.Panda() + + Tep = r.eval([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) + + sol = r.ik_GN(Tep, tol=tol) + sol2 = r2.ik_GN(Tep, tol=tol) + + self.assertEqual(sol[1], True) + self.assertEqual(sol2[1], True) + + Tq = r.eval(sol[0]) + Tq2 = r.eval(sol2[0]) + + print(sol[4]) + print(Tep) + print(Tq) + + _, E = solver.error(Tep, Tq) + _, E2 = solver.error(Tep, Tq2) + + self.assertGreater(test_tol, E) + self.assertGreater(test_tol, E2) + + def test_sol_print1(self): + + sol = rtb.IKSolution( + q=np.zeros(3), + success=True, + iterations=1, + searches=2, + residual=3.0, + reason="no", + ) + + s = sol.__str__() + + ans = ( + "IKSolution: q=[0, 0, 0], success=True, iterations=1, searches=2," + " residual=3" + ) + + self.assertEqual(s, ans) + + def test_sol_print2(self): + + sol = rtb.IKSolution( + q=None, # type: ignore + success=True, + iterations=1, + searches=2, + residual=3.0, + reason="no", + ) + + s = sol.__str__() + + ans = "IKSolution: q=None, success=True, iterations=1, searches=2, residual=3" + + self.assertEqual(s, ans) + + def test_sol_print3(self): + + sol = rtb.IKSolution( + q=np.zeros(3), + success=False, + iterations=0, + searches=0, + residual=3.0, + reason="no", + ) + + s = sol.__str__() + + ans = "IKSolution: q=[0, 0, 0], success=False, reason=no" + + self.assertEqual(s, ans) + + def test_sol_print4(self): + + sol = rtb.IKSolution( + q=np.zeros(3), + success=True, + iterations=0, + searches=0, + residual=3.0, + reason="no", + ) + + s = sol.__str__() + + ans = "IKSolution: q=[0, 0, 0], success=True" + + self.assertEqual(s, ans) + + def test_sol_print5(self): + + sol = rtb.IKSolution( + q=np.zeros(3), + success=False, + iterations=1, + searches=2, + residual=3.0, + reason="no", + ) + + s = sol.__str__() + + ans = ( + "IKSolution: q=[0, 0, 0], success=False, reason=no, iterations=1," + " searches=2, residual=3" + ) + + self.assertEqual(s, ans) + + def test_iter_iksol(self): + sol = rtb.IKSolution( + np.array([1.0, 2.0, 3.0]), + success=True, + iterations=10, + searches=100, + residual=0.1, + ) + + a, b, c, d, e, f = sol + + nt.assert_almost_equal(a, np.array([1.0, 2.0, 3.0])) # type: ignore + self.assertEqual(b, True) + self.assertEqual(c, 10) + self.assertEqual(d, 100) + self.assertEqual(e, 0.1) + self.assertEqual(f, "") + + +if __name__ == "__main__": + + unittest.main() diff --git a/tests/test_Link.py b/tests/test_Link.py index 12c979539..546ed9d50 100644 --- a/tests/test_Link.py +++ b/tests/test_Link.py @@ -142,8 +142,8 @@ def test_dyn(self): B = 0.0015 Tc = 0.4(+) -0.43(-) G = -63 -qlim = -2.8 to 2.8""", - ) # noqa +qlim = -2.8 to 2.8""", # noqa + ) puma.links[0].dyn(indent=2) diff --git a/tests/test_Robot.py b/tests/test_Robot.py index b237f53af..f5d43d708 100644 --- a/tests/test_Robot.py +++ b/tests/test_Robot.py @@ -4,225 +4,1457 @@ import numpy.testing as nt import numpy as np -import roboticstoolbox as rp -import spatialmath as sm +import roboticstoolbox as rtb import unittest +import os +import spatialgeometry as sg +from spatialmath.base import tr2jac + +# from spatialmath import SE3 class TestRobot(unittest.TestCase): - def test_init(self): - l0 = rp.PrismaticDH() - l1 = rp.RevoluteDH() + def test_fkine(self): + panda = rtb.models.ETS.Panda() + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + # q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + # q3 = np.expand_dims(q1, 0) - with self.assertRaises(TypeError): - rp.DHRobot([l0, l1], keywords=1) + ans = np.array( + [ + [-0.50827907, -0.57904589, 0.63746234, 0.44682295], + [0.83014553, -0.52639462, 0.18375824, 0.16168396], + [0.22915229, 0.62258699, 0.74824773, 0.96798113], + [0.0, 0.0, 0.0, 1.0], + ] + ) - with self.assertRaises(TypeError): - rp.Robot(l0) + nt.assert_array_almost_equal(panda.fkine(q1).A, ans) - with self.assertRaises(TypeError): - rp.Robot([l0, 1]) + def test_jacob0(self): + panda = rtb.models.ETS.Panda() + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + q3 = np.expand_dims(q1, 0) + q4 = np.expand_dims(q1, 1) - def test_configurations_str(self): - r = rp.models.DH.Puma560() - r.configurations_str() + ans = np.array( + [ + [ + -1.61683957e-01, + 1.07925929e-01, + -3.41453006e-02, + 3.35029257e-01, + -1.07195463e-02, + 1.03187865e-01, + 0.00000000e00, + ], + [ + 4.46822947e-01, + 6.25741987e-01, + 4.16474664e-01, + -8.04745724e-02, + 7.78257566e-02, + -1.17720983e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + -2.35276631e-01, + -8.20187641e-02, + -5.14076923e-01, + -9.98040745e-03, + -2.02626953e-01, + 0.00000000e00, + ], + [ + 1.29458954e-16, + -9.85449730e-01, + 3.37672585e-02, + -6.16735653e-02, + 6.68449878e-01, + -1.35361558e-01, + 6.37462344e-01, + ], + [ + 9.07021273e-18, + 1.69967143e-01, + 1.95778638e-01, + 9.79165111e-01, + 1.84470262e-01, + 9.82748279e-01, + 1.83758244e-01, + ], + [ + 1.00000000e00, + -2.26036604e-17, + 9.80066578e-01, + -1.93473657e-01, + 7.20517510e-01, + -1.26028049e-01, + 7.48247732e-01, + ], + ] + ) - r2 = rp.models.ETS.Frankie() - r2.configurations_str() + panda.q = q1 + # nt.assert_array_almost_equal(panda.jacob0(), ans) + nt.assert_array_almost_equal(panda.jacob0(q2), ans) + nt.assert_array_almost_equal(panda.jacob0(q3), ans) + nt.assert_array_almost_equal(panda.jacob0(q4), ans) + self.assertRaises(TypeError, panda.jacob0, "Wfgsrth") - def test_dyntable(self): - r = rp.models.DH.Puma560() - r.dynamics() + def test_hessian0(self): + panda = rtb.models.ETS.Panda() + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + q3 = np.expand_dims(q1, 0) + q4 = np.expand_dims(q1, 1) - def test_linkcolormap(self): - r = rp.models.DH.Puma560() - r.linkcolormap() + ans = np.array( + [ + [ + [ + -4.46822947e-01, + -6.25741987e-01, + -4.16474664e-01, + 8.04745724e-02, + -7.78257566e-02, + 1.17720983e-02, + 0.00000000e00, + ], + [ + -6.25741987e-01, + -3.99892968e-02, + -1.39404950e-02, + -8.73761859e-02, + -1.69634134e-03, + -3.44399243e-02, + 0.00000000e00, + ], + [ + -4.16474664e-01, + -1.39404950e-02, + -4.24230421e-01, + -2.17748413e-02, + -7.82283735e-02, + -2.81325889e-02, + 0.00000000e00, + ], + [ + 8.04745724e-02, + -8.73761859e-02, + -2.17748413e-02, + -5.18935898e-01, + 5.28476698e-03, + -2.00682834e-01, + 0.00000000e00, + ], + [ + -7.78257566e-02, + -1.69634134e-03, + -7.82283735e-02, + 5.28476698e-03, + -5.79159088e-02, + -2.88966443e-02, + 0.00000000e00, + ], + [ + 1.17720983e-02, + -3.44399243e-02, + -2.81325889e-02, + -2.00682834e-01, + -2.88966443e-02, + -2.00614904e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + -1.61683957e-01, + 1.07925929e-01, + -3.41453006e-02, + 3.35029257e-01, + -1.07195463e-02, + 1.03187865e-01, + 0.00000000e00, + ], + [ + 1.07925929e-01, + -2.31853293e-01, + -8.08253690e-02, + -5.06596965e-01, + -9.83518983e-03, + -1.99678676e-01, + 0.00000000e00, + ], + [ + -3.41453006e-02, + -8.08253690e-02, + -3.06951191e-02, + 3.45709946e-01, + -1.01688580e-02, + 1.07973135e-01, + 0.00000000e00, + ], + [ + 3.35029257e-01, + -5.06596965e-01, + 3.45709946e-01, + -9.65242924e-02, + 1.45842251e-03, + -3.24608603e-02, + 0.00000000e00, + ], + [ + -1.07195463e-02, + -9.83518983e-03, + -1.01688580e-02, + 1.45842251e-03, + -1.05221866e-03, + 2.09794626e-01, + 0.00000000e00, + ], + [ + 1.03187865e-01, + -1.99678676e-01, + 1.07973135e-01, + -3.24608603e-02, + 2.09794626e-01, + -4.04324654e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + -6.34981134e-01, + -4.04611266e-01, + 2.23596800e-02, + -7.48714002e-02, + -5.93773551e-03, + 0.00000000e00, + ], + [ + 0.00000000e00, + -4.04611266e-01, + 2.07481281e-02, + -6.83089775e-02, + 4.72662062e-03, + -2.05994912e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 2.23596800e-02, + -6.83089775e-02, + -3.23085806e-01, + 5.69641385e-03, + -1.00311930e-01, + 0.00000000e00, + ], + [ + 0.00000000e00, + -7.48714002e-02, + 4.72662062e-03, + 5.69641385e-03, + 5.40000550e-02, + -2.69041502e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + -5.93773551e-03, + -2.05994912e-02, + -1.00311930e-01, + -2.69041502e-02, + -9.98142073e-02, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ], + [ + [ + -9.07021273e-18, + -2.77555756e-17, + -2.77555756e-17, + -1.11022302e-16, + -2.77555756e-17, + 0.00000000e00, + -2.77555756e-17, + ], + [ + -1.69967143e-01, + -1.97756387e-17, + 4.11786040e-17, + -1.48932398e-16, + -5.07612940e-17, + -8.38219650e-17, + -4.90138154e-17, + ], + [ + -1.95778638e-01, + 1.66579116e-01, + -1.38777878e-17, + 1.04083409e-17, + -1.38777878e-17, + 3.46944695e-18, + 0.00000000e00, + ], + [ + -9.79165111e-01, + -3.28841647e-02, + -9.97525009e-01, + -4.16333634e-17, + -1.14491749e-16, + 1.38777878e-17, + -6.24500451e-17, + ], + [ + -1.84470262e-01, + 1.22464303e-01, + -3.97312016e-02, + 7.41195745e-01, + -2.77555756e-17, + 1.12757026e-16, + 2.77555756e-17, + ], + [ + -9.82748279e-01, + -2.14206274e-02, + -9.87832342e-01, + 6.67336352e-02, + -7.31335770e-01, + 2.08166817e-17, + -6.07153217e-17, + ], + [ + -1.83758244e-01, + 1.27177529e-01, + -3.36043908e-02, + 7.68210453e-01, + 5.62842325e-03, + 7.58497864e-01, + 0.00000000e00, + ], + ], + [ + [ + 1.29458954e-16, + -1.11022302e-16, + 8.67361738e-17, + -4.16333634e-17, + 5.55111512e-17, + 2.77555756e-17, + 5.55111512e-17, + ], + [ + -9.85449730e-01, + -6.36381327e-17, + -1.02735399e-16, + -1.83043043e-17, + -5.63484308e-17, + 8.08886307e-18, + 1.07112702e-18, + ], + [ + 3.37672585e-02, + 9.65806345e-01, + 8.32667268e-17, + -2.55871713e-17, + 1.07552856e-16, + 2.08166817e-17, + -5.20417043e-18, + ], + [ + -6.16735653e-02, + -1.90658563e-01, + -5.39111251e-02, + -6.59194921e-17, + -2.77555756e-17, + 2.38524478e-17, + -4.16333634e-17, + ], + [ + 6.68449878e-01, + 7.10033786e-01, + 6.30795483e-01, + -8.48905588e-02, + 0.00000000e00, + 3.46944695e-17, + 2.77555756e-17, + ], + [ + -1.35361558e-01, + -1.24194307e-01, + -1.28407717e-01, + 1.84162966e-02, + -1.32869389e-02, + 2.77555756e-17, + -2.08166817e-17, + ], + [ + 6.37462344e-01, + 7.37360525e-01, + 5.99489263e-01, + -7.71850655e-02, + -4.08633244e-02, + 2.09458434e-02, + 0.00000000e00, + ], + ], + [ + [ + 0.00000000e00, + -6.59521910e-17, + -1.31033786e-16, + -1.92457571e-16, + 1.54134782e-17, + -7.69804929e-17, + 1.11140361e-17, + ], + [ + 0.00000000e00, + -2.77555756e-17, + 7.15573434e-17, + 1.65666092e-16, + 1.38777878e-17, + -8.67361738e-18, + 3.46944695e-17, + ], + [ + 0.00000000e00, + -1.98669331e-01, + 8.67361738e-18, + -1.46584134e-16, + 6.02816408e-17, + -3.12250226e-17, + 6.11490025e-17, + ], + [ + 0.00000000e00, + -9.54435515e-01, + 4.51380881e-02, + 1.38777878e-17, + 1.08420217e-16, + 3.46944695e-18, + 6.24500451e-17, + ], + [ + 0.00000000e00, + -2.95400686e-01, + -1.24639152e-01, + -6.65899738e-01, + -4.85722573e-17, + -5.20417043e-18, + -5.55111512e-17, + ], + [ + 0.00000000e00, + -9.45442009e-01, + 5.96856167e-02, + 7.19317248e-02, + 6.81888149e-01, + -2.77555756e-17, + 1.04083409e-17, + ], + [ + 0.00000000e00, + -2.89432165e-01, + -1.18596498e-01, + -6.35513913e-01, + 5.24032975e-03, + -6.51338823e-01, + 0.00000000e00, + ], + ], + ] + ) - r.linkcolormap(["r", "r", "r", "r", "r", "r"]) + ans_new = np.empty((7, 6, 7)) - # def test_tool_error(self): - # r = rp.models.DH.Puma560() + for i in range(7): + ans_new[i, :, :] = ans[:, :, i] - # with self.assertRaises(ValueError): - # r.tool = 2 + nt.assert_array_almost_equal(panda.hessian0(q1), ans_new) + nt.assert_array_almost_equal(panda.hessian0(q2), ans_new) + nt.assert_array_almost_equal(panda.hessian0(q3), ans_new) + nt.assert_array_almost_equal(panda.hessian0(q4), ans_new) + nt.assert_array_almost_equal(panda.hessian0(J0=panda.jacob0(q1)), ans_new) + nt.assert_array_almost_equal( + panda.hessian0(q=None, J0=panda.jacob0(q1)), ans_new + ) - def test_links(self): + def test_manipulability(self): + panda = rtb.models.ETS.Panda() + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] - l0 = rp.PrismaticDH() - l1 = rp.RevoluteDH() - l2 = rp.PrismaticDH() - l3 = rp.RevoluteDH() + ans = 0.006559178039088341 - r0 = rp.DHRobot([l0, l1, l2, l3]) + panda.q = q1 + nt.assert_array_almost_equal(panda.manipulability(q2), ans) + # self.assertRaises(ValueError, panda.manipulability) + self.assertRaises(TypeError, panda.manipulability, "Wfgsrth") + self.assertRaises(ValueError, panda.manipulability, [1, 3]) - self.assertIs(r0[0], l0) - self.assertIs(r0[1], l1) - self.assertIs(r0[2], l2) - self.assertIs(r0[3], l3) + def test_qlim(self): + panda = rtb.models.ETS.Panda() - # def test_ikine_LM(self): - # panda = rp.models.DH.Panda() - # q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) - # T = panda.fkine(q) - # Tt = sm.SE3([T, T]) + self.assertEqual(panda.qlim.shape[0], 2) + self.assertEqual(panda.qlim.shape[1], panda.n) - # qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] + def test_manuf(self): + panda = rtb.models.ETS.Panda() - # sol1 = panda.ikine_LM(T) - # sol2 = panda.ikine_LM(Tt) - # sol3 = panda.ikine_LM(T, q0=[0, 1.4, 0, 1, 0, 0.5, 1]) + self.assertIsInstance(panda.manufacturer, str) - # self.assertTrue(sol1.success) - # self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) + def test_complex(self): + l0 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.Rx()) + l1 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.Ry(), parent=l0) + l2 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.Rz(), parent=l1) + l3 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.tx(), parent=l2) + l4 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.ty(), parent=l3) + l5 = rtb.Link(rtb.ET.tx(0.1) * rtb.ET.tz(), parent=l4) - # self.assertTrue(sol2.success[0]) - # self.assertAlmostEqual( - # np.linalg.norm(T - panda.fkine(sol2.q[0, :])), 0, places=4 - # ) - # self.assertTrue(sol2.success[0]) - # self.assertAlmostEqual( - # np.linalg.norm(T - panda.fkine(sol2.q[1, :])), 0, places=4 - # ) + r = rtb.Robot([l0, l1, l2, l3, l4, l5]) + q = [1.0, 2, 3, 1, 2, 3] - # self.assertTrue(sol3.success) - # self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) + ans = np.array( + [ + [-0.0, 0.08752679, -0.74761985, 0.41198225, 0.05872664, 0.90929743], + [ + 1.46443609, + 2.80993063, + 0.52675075, + -0.68124272, + -0.64287284, + 0.35017549, + ], + [ + -1.04432, + -1.80423571, + -2.20308833, + 0.60512725, + -0.76371834, + -0.2248451, + ], + [1.0, 0.0, 0.90929743, 0.0, 0.0, 0.0], + [0.0, 0.54030231, 0.35017549, 0.0, 0.0, 0.0], + [0.0, 0.84147098, -0.2248451, 0.0, 0.0, 0.0], + ] + ) - # with self.assertRaises(ValueError): - # panda.ikine_LM(T, q0=[1, 2]) + nt.assert_array_almost_equal(r.jacob0(q), ans) - # def test_ikine_LM_mask(self): + def test_copy_init(self): + r = rtb.models.Panda() - # # simple RR manipulator, solve with mask + r2 = rtb.Robot(r) - # l0 = rp.RevoluteDH(a=2.0) - # l1 = rp.RevoluteDH(a=1) + r2.jacob0(r.q) - # r = rp.DHRobot([l0, l1]) # RR manipulator - # T = sm.SE3(-1, 2, 0) - # sol = r.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0]) + self.assertEqual(r.n, r2.n) - # self.assertTrue(sol.success) - # self.assertAlmostEqual( - # np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 - # ) + def test_init2(self): + r = rtb.Robot(rtb.ETS(rtb.ET.Ry(qlim=[-1, 1]))) - # # test initial condition search, drop iteration limit so it has to do - # # some searching - # sol = r.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0], ilimit=8, search=True) + self.assertEqual(r.n, 1) - # self.assertTrue(sol.success) - # self.assertAlmostEqual( - # np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 - # ) + def test_to_dict(self): + r = rtb.models.Panda() - # def test_ikine_LM_transpose(self): - # # need to test this on a robot with squarish Jacobian, choose Puma + rdict = r._to_dict(collision_alpha=0.5) + rdict2 = r._to_dict() - # r = rp.models.DH.Puma560() - # T = r.fkine(r.qn) + self.assertTrue(len(rdict) > len(rdict2)) - # sol = r.ikine_LM(T, transpose=0.5, ilimit=1000, tol=1e-6) + self.assertIsInstance(rdict, list) - # self.assertTrue(sol.success) - # self.assertAlmostEqual(np.linalg.norm(T - r.fkine(sol.q)), 0, places=4) + def test_fk_dict(self): + r = rtb.models.Panda() - # def test_ikine_con(self): - # panda = rp.models.DH.Panda() - # q = np.array([0, -0.3, 0, -2.2, 0, 2.0, np.pi / 4]) - # T = panda.fkine(q) - # Tt = sm.SE3([T, T, T]) + rdict = r._fk_dict(collision_alpha=0.5) + rdict2 = r._fk_dict() - # # qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02, - # # -6.98000000e-02, 1.38978915e-02, 9.62104811e-01, - # # 7.84926515e-01] + self.assertTrue(len(rdict) > len(rdict2)) - # sol1 = panda.ikine_min(T, qlim=True, q0=np.zeros(7)) - # sol2 = panda.ikine_min(Tt, qlim=True) + def test_URDF(self): + r = rtb.Robot.URDF("fetch_description/robots/fetch.urdf", gripper=6) - # self.assertTrue(sol1.success) - # self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) - # nt.assert_array_almost_equal( - # T.A - panda.fkine(sol1.q).A, np.zeros((4, 4)), decimal=4 - # ) + self.assertEqual(r.n, 5) - # self.assertTrue(sol2[0].success) - # nt.assert_array_almost_equal( - # T.A - panda.fkine(sol2[0].q).A, np.zeros((4, 4)), decimal=4 - # ) - # self.assertTrue(sol2[1].success) - # nt.assert_array_almost_equal( - # T.A - panda.fkine(sol2[1].q).A, np.zeros((4, 4)), decimal=4 - # ) - - # def test_ikine_unc(self): - # puma = rp.models.DH.Puma560() - # q = puma.qn - # T = puma.fkine(q) - # Tt = sm.SE3([T, T]) + def test_URDF2(self): + r = rtb.Robot.URDF( + "fetch_description/robots/fetch.urdf", gripper="forearm_roll_link" + ) - # sol1 = puma.ikine_min(Tt) - # sol2 = puma.ikine_min(T) - # sol3 = puma.ikine_min(T) + self.assertEqual(r.n, 7) - # self.assertTrue(sol1[0].success) - # nt.assert_array_almost_equal( - # T.A - puma.fkine(sol1[0].q).A, np.zeros((4, 4)), decimal=4 - # ) - # self.assertTrue(sol1[1].success) - # nt.assert_array_almost_equal( - # T.A - puma.fkine(sol1[1].q).A, np.zeros((4, 4)), decimal=4 - # ) - - # self.assertTrue(sol2.success) - # nt.assert_array_almost_equal( - # T.A - puma.fkine(sol2.q).A, np.zeros((4, 4)), decimal=4 - # ) - - # self.assertTrue(sol3.success) - # nt.assert_array_almost_equal( - # T.A - puma.fkine(sol3.q).A, np.zeros((4, 4)), decimal=4 - # ) + def test_showgraph(self): + r = rtb.models.Panda() - # def test_plot_swift(self): - # r = rp.models.Panda() + file = r.showgraph(display_graph=False) - # env = r.plot(r.q, block=False) - # env.close() + self.assertIsNotNone(file) + self.assertTrue(file[-4:] == ".pdf") # type: ignore -if __name__ == "__main__": # pragma nocover - unittest.main() - # pytest.main(['tests/test_SerialLink.py']) + def test_dotfile(self): + r = rtb.models.Panda() + + r.dotfile("test.dot") + try: + os.remove("test.dot") + except PermissionError: + pass + + def test_dotfile2(self): + r = rtb.models.Frankie() + + r.dotfile("test.dot", jtype=True, etsbox=True) + try: + os.remove("test.dot") + except PermissionError: + pass + + def test_dotfile3(self): + r = rtb.models.Panda() + + r.dotfile("test.dot", ets="brief") + try: + os.remove("test.dot") + except PermissionError: + pass + + def test_dotfile4(self): + r = rtb.models.Panda() + + r.dotfile("test.dot", ets="None") # type: ignore + try: + os.remove("test.dot") + except PermissionError: + pass + + def test_fkine_all(self): + r = rtb.models.ETS.Panda() + + r.fkine_all(r.q) + + def test_fkine_all2(self): + r = rtb.models.YuMi() + + r.fkine_all(r.q) + + def test_yoshi(self): + puma = rtb.models.Puma560() + q = puma.qn # type: ignore + + m1 = puma.manipulability(q) + m2 = puma.manipulability(np.c_[q, q].T) + m3 = puma.manipulability(q, axes="trans") + m4 = puma.manipulability(q, axes="rot") + + a0 = 0.0805 + a2 = 0.1354 + a3 = 2.44949 + + nt.assert_almost_equal(m1, a0, decimal=4) + nt.assert_almost_equal(m2[0], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m2[1], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m3, a2, decimal=4) + nt.assert_almost_equal(m4, a3, decimal=4) + + with self.assertRaises(ValueError): + puma.manipulability(axes="abcdef") # type: ignore + + def test_asada(self): + l1 = rtb.Link(ets=rtb.ETS(rtb.ET.Ry()), m=1, r=[0.5, 0, 0], name="l1") + l2 = rtb.Link( + ets=rtb.ETS(rtb.ET.tx(1)) * rtb.ET.Ry(), + m=1, + r=[0.5, 0, 0], + parent=l1, + name="l2", + ) + r = rtb.Robot([l1, l2], name="simple 2 link") + q = np.array([1.0, 1.5]) + + m1 = r.manipulability(q, method="asada") + m2 = r.manipulability(np.c_[q, q].T, method="asada") + m3 = r.manipulability(q, axes="trans", method="asada") + m4 = r.manipulability(q, axes="rot", method="asada") + + a0 = 0.0 + a2 = 0.0 + a3 = 0.0 + + nt.assert_almost_equal(m1, a0, decimal=4) + nt.assert_almost_equal(m2[0], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m2[1], a0, decimal=4) # type: ignore + nt.assert_almost_equal(m3, a2, decimal=4) + nt.assert_almost_equal(m4, a3, decimal=4) + + def test_cond(self): + r = rtb.models.Panda() + + m = r.manipulability(r.qr, method="invcondition") + + self.assertAlmostEqual(m, 0.11222, places=4) # type: ignore + + def test_minsingular(self): + r = rtb.models.Panda() + + m = r.manipulability(r.qr, method="minsingular") + + self.assertAlmostEqual(m, 0.209013, places=4) # type: ignore + + def test_jtraj(self): + r = rtb.models.Panda() + + q1 = r.q + 0.2 + + q = r.jtraj(r.fkine(q1), r.fkine(r.qr), 5) + + self.assertEqual(q.s.shape, (5, 7)) + + def test_jtraj2(self): + r = rtb.models.DH.Puma560() + + q1 = r.q + 0.2 + + q = r.jtraj(r.fkine(q1), r.fkine(r.qr), 5) + + self.assertEqual(q.s.shape, (5, 6)) + + def test_manip(self): + r = rtb.models.Panda() + q = r.qr + + m1 = r.manipulability(q) + m2 = r.manipulability(q, axes="trans") + m3 = r.manipulability(q, axes="rot") + + nt.assert_almost_equal(m1, 0.0837, decimal=4) + nt.assert_almost_equal(m2, 0.1438, decimal=4) + nt.assert_almost_equal(m3, 2.7455, decimal=4) + + with self.assertRaises(ValueError): + r.manipulability(axes="abcdef") # type: ignore - # import roboticstoolbox as rtb - # from spatialmath import SE3 + def test_jacobm(self): + r = rtb.models.Panda() + q = r.qr - # l0 = rtb.RevoluteDH(a=2.0) - # l1 = rtb.RevoluteDH(a=1) - # # l1 = rp.PrismaticDH(theta=1.0) - # # l2 = rp.PrismaticDH(theta=1, qlim=[0, 2]) + m1 = r.jacobm(q) + m2 = r.jacobm(q, axes="trans") + m3 = r.jacobm(q, axes="rot") - # # r0 = rtb.DHRobot([l0, l1]) # RP manipulator - # r1 = rtb.DHRobot([l0, l1]) # RR manipulator - # print(r1) - # T = SE3(-1, 2, 0) - # print(T) - # sol = r1.ikine_LM(T, mask=[1, 1, 0, 0, 0, 0]) - # print(sol) - # print(r1.fkine(sol.q)) + a1 = np.array( + [ + [0.00000000e00], + [-2.62678438e-03], + [1.18662211e-19], + [4.06398364e-02], + [1.21226717e-19], + [-2.73383661e-02], + [0.00000000e00], + ] + ) - # # sol = r1.ikine_LM(T, transpose=0.05, tol=1e-2, ilimit=1000, mask=[1, 1, 0, 0, 0, 0]) - # # print(sol) - # # print(r1.fkine(sol.q)) + a2 = np.array( + [ + [-4.03109907e-32], + [2.14997718e-02], + [2.57991732e-18], + [9.51555140e-02], + [1.09447194e-18], + [3.78529920e-02], + [0.00000000e00], + ] + ) - # sol = r1.ikine_LM( - # T, mask=[1, 1, 0, 0, 0, 0], - # ilimit=8, search=True) - # print(sol) + a3 = np.array( + [ + [-1.22157098e-31], + [-7.51299508e-01], + [9.51556025e-17], + [7.49956218e-01], + [8.40346012e-18], + [-5.17677915e-01], + [0.00000000e00], + ] + ) - # r = rtb.models.DH.Puma560() + nt.assert_almost_equal(m1, a1, decimal=4) + nt.assert_almost_equal(m2, a2, decimal=4) + nt.assert_almost_equal(m3, a3, decimal=4) - # T = r.fkine(r.qn) + with self.assertRaises(ValueError): + r.jacobm(axes="abcdef") # type: ignore - # sol = r.ikine_LM(T, transpose=0.5, ilimit=1000, tol=1e-6) - # print(sol) + def test_collided2(self): + p = rtb.models.Panda() + s0 = sg.Cuboid([0.01, 0.01, 0.01], pose=p.fkine(p.q)) + + c0 = p.collided(p.q, s0) + + self.assertTrue(c0) + + def test_velocity_damper(self): + r = rtb.models.Panda() + + Ain, Bin = r.joint_velocity_damper() + + a1 = np.array( + [ + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, -1.0, 0.0], + [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ] + ) + + a2 = np.array([0.0, 0.0, 0.0, -2.396, 0.0, -0.65, 0.0]) + + nt.assert_almost_equal(Ain, a1, decimal=4) + nt.assert_almost_equal(Bin, a2, decimal=4) + + def test_link_collision_damper(self): + r = rtb.models.Panda() + + s = sg.Cuboid([0.01, 0.01, 0.01]) + + Ain, Bin = r.link_collision_damper(s, r.q) + + a1 = np.array( + [ + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -1.93649378e-34, + -1.71137143e-18, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 5.05166785e-18, + -8.25000000e-02, + 5.05166785e-18, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 8.25000000e-02, + 1.01033361e-17, + 8.25000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -8.25000000e-02, + -1.01033361e-17, + -8.25000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -2.75268296e-35, + 2.90883510e-18, + -1.55445626e-34, + -8.25000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -8.25000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -8.25000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -1.57039486e-11, + 1.11890634e-10, + -1.57039486e-11, + -6.09481971e-02, + -1.57039486e-11, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -4.89389206e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + -7.69198080e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 1.20407156e-14, + -2.80950032e-13, + 1.20407156e-14, + 9.05269566e-14, + 1.20407156e-14, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -1.13663976e-10, + 6.90000000e-01, + -1.13663996e-10, + -3.74000000e-01, + -1.13663972e-10, + 1.00000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 1.07768918e-17, + -8.80000000e-02, + 1.07768918e-17, + 5.50000000e-03, + 1.07768918e-17, + 8.80000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -1.07768918e-17, + 8.80000000e-02, + -1.07768918e-17, + -5.50000000e-03, + -1.07768918e-17, + -8.80000000e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -1.42800820e-02, + -5.22756456e-01, + -1.42800820e-02, + 2.60933208e-01, + -1.42800820e-02, + -1.20480747e-01, + 3.09364536e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 5.96886490e-02, + -3.78932484e-01, + 5.96886490e-02, + 1.87911850e-01, + 5.96886490e-02, + -9.58635879e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 5.61245282e-02, + -3.41025601e-01, + 5.61245282e-02, + 1.75113444e-01, + 5.61245282e-02, + -1.05419509e-01, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 6.22253967e-02, + -3.91030050e-01, + 6.22253967e-02, + 1.67584307e-01, + 6.22253967e-02, + -1.03944697e-01, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -4.90028111e-02, + 2.75492780e-01, + -4.90028111e-02, + -1.50373572e-01, + -4.90028111e-02, + 1.14302308e-01, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 4.90028111e-02, + -3.83963034e-01, + 4.90028111e-02, + 1.57152963e-01, + 4.90028111e-02, + -5.83205400e-03, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 6.22246462e-02, + -3.48609107e-01, + 6.22246462e-02, + 1.25159489e-01, + 6.22246462e-02, + -1.46371610e-01, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + -2.68888497e-02, + 1.03162870e-01, + -2.68888497e-02, + -8.10072778e-02, + -2.68888497e-02, + 1.10725707e-01, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + [ + 2.68888497e-02, + -2.61882491e-01, + 2.68888497e-02, + 9.09272541e-02, + 2.68888497e-02, + 4.79939142e-02, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + 0.00000000e00, + ], + ] + ) + a2 = np.array( + [ + 2.00000000e-02, + -1.27216162e-01, + -3.34978685e-02, + -2.00000000e-02, + 8.72000000e-01, + -2.60000000e-01, + -4.60000000e-01, + -2.20000000e-01, + -2.20000000e-01, + 6.00000000e-02, + 4.20000000e-01, + -1.80000000e-01, + -4.60000000e-01, + -2.20000000e-01, + -2.20000000e-01, + 6.20000000e-01, + 7.80000000e-01, + 3.80000000e-01, + 1.00622747e-01, + 7.36135591e-02, + 5.36875734e-01, + -2.60000000e-01, + -3.80000000e-01, + -1.40000000e-01, + -3.80000000e-01, + 2.77555756e-17, + -1.40000000e-01, + -5.82574174e-02, + -9.75448830e-02, + -8.31641223e-02, + -2.20000000e-01, + -1.30350690e-01, + -1.30350690e-01, + 1.00004623e-01, + 1.41807468e-01, + 1.41807468e-01, + ] + ) + + nt.assert_almost_equal(Ain, a1, decimal=4) # type: ignore + nt.assert_almost_equal(Bin, a2, decimal=4) # type: ignore + + def test_hessiane(self): + deg = np.pi / 180 + mm = 1e-3 + tool_offset = (103) * mm + + l0 = rtb.ET.tz(0.333) * rtb.ET.Rz(jindex=0) + + l1 = rtb.ET.Rx(-90 * deg) * rtb.ET.Rz(jindex=1) + + l2 = rtb.ET.Rx(90 * deg) * rtb.ET.tz(0.316) * rtb.ET.Rz(jindex=2) + + l3 = rtb.ET.tx(0.0825) * rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=3) + + l4 = ( + rtb.ET.tx(-0.0825) + * rtb.ET.Rx(-90 * deg) + * rtb.ET.tz(0.384) + * rtb.ET.Rz(jindex=4) + ) + + l5 = rtb.ET.Rx(90 * deg) * rtb.ET.Rz(jindex=5) + + l6 = ( + rtb.ET.tx(0.088) + * rtb.ET.Rx(90 * deg) + * rtb.ET.tz(0.107) + * rtb.ET.Rz(jindex=6) + ) + + ee = rtb.ET.tz(tool_offset) * rtb.ET.Rz(-np.pi / 4) + + r = rtb.Robot(l0 + l1 + l2 + l3 + l4 + l5 + l6 + ee) + + q1 = np.array([1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9]) + q2 = [1.4, 0.2, 1.8, 0.7, 0.1, 3.1, 2.9] + q3 = np.expand_dims(q1, 0) + q4 = np.expand_dims(q1, 1) + + H0 = r.hessian0(q1) + He = np.empty((r.n, 6, r.n)) + T = r.fkine(q1, include_base=False).A + + for i in range(r.n): + He[i, :, :] = tr2jac(T.T) @ H0[i, :, :] + + J = r.jacobe(q1) + + nt.assert_array_almost_equal(r.hessiane(q1), He) + nt.assert_array_almost_equal(r.hessiane(q2), He) + nt.assert_array_almost_equal(r.hessiane(q3), He) + nt.assert_array_almost_equal(r.hessiane(q4), He) + + nt.assert_array_almost_equal(r.hessiane(Je=J), He) + nt.assert_array_almost_equal(r.hessiane(Je=J), He) + nt.assert_array_almost_equal(r.hessiane(Je=J), He) + nt.assert_array_almost_equal(r.hessiane(Je=J), He) + + def test_erobot(self): + ets = rtb.ETS(rtb.ET.Rz()) + robot = rtb.ERobot( + ets, name="myname", manufacturer="I made it", comment="other stuff" + ) + self.assertEqual(robot.name, "myname") + self.assertEqual(robot.manufacturer, "I made it") + self.assertEqual(robot.comment, "other stuff") + + def test_erobot2(self): + ets = rtb.ETS2(rtb.ET2.R()) + robot = rtb.ERobot2( + ets, name="myname", manufacturer="I made it", comment="other stuff" + ) + self.assertEqual(robot.name, "myname") + self.assertEqual(robot.manufacturer, "I made it") + self.assertEqual(robot.comment, "other stuff") + + def test_qlim_setters(self): + et = rtb.ET.Rz(qlim=[-1, 1]) + ets = rtb.ETS([et]) + l = rtb.Link(ets) + r = rtb.Robot([l]) + + nt.assert_almost_equal(et.qlim, np.array([-1, 1])) + nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T) + nt.assert_almost_equal(l.qlim, np.array([-1, 1])) + nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T) + + et.qlim = [-2, 2] + nt.assert_almost_equal(et.qlim, np.array([-2, 2])) + nt.assert_almost_equal(ets.qlim, np.array([[-1, 1]]).T) + nt.assert_almost_equal(l.qlim, np.array([-1, 1])) + nt.assert_almost_equal(r.qlim, np.array([[-1, 1]]).T) + + ets.qlim = np.array([[-2, 2]]).T + nt.assert_almost_equal(et.qlim, np.array([-2, 2])) + nt.assert_almost_equal(ets.qlim, np.array([[-2, 2]]).T) + nt.assert_almost_equal(l.qlim, np.array([-2, 2])) + nt.assert_almost_equal(r.qlim, np.array([[-2, 2]]).T) + + l.qlim = [-3, 3] + nt.assert_almost_equal(et.qlim, np.array([-2, 2])) + nt.assert_almost_equal(ets.qlim, np.array([[-3, 3]]).T) + nt.assert_almost_equal(l.qlim, np.array([-3, 3])) + nt.assert_almost_equal(r.qlim, np.array([[-3, 3]]).T) + + r.qlim = np.array([[-4, 4]]).T + nt.assert_almost_equal(et.qlim, np.array([-2, 2])) + nt.assert_almost_equal(ets.qlim, np.array([[-4, 4]]).T) + nt.assert_almost_equal(l.qlim, np.array([-4, 4])) + nt.assert_almost_equal(r.qlim, np.array([[-4, 4]]).T) + + +if __name__ == "__main__": # pragma nocover + unittest.main() diff --git a/tests/test_blocks.py b/tests/test_blocks.py new file mode 100644 index 000000000..357867d24 --- /dev/null +++ b/tests/test_blocks.py @@ -0,0 +1,417 @@ +#!/usr/bin/env python3 + +import unittest + +try: + from bdsim import BDSim +except ModuleNotFoundError: + raise unittest.SkipTest("bdsim not found, skipping all tests in test_blocks.py") from None + +from spatialmath import SE3 +from spatialmath.base import tr2x + +import numpy.testing as nt + +import roboticstoolbox as rtb +from roboticstoolbox.blocks import * +from roboticstoolbox.blocks.quad_model import quadrotor + +class State: + T = 5 + + class Opt: + def __init__(self): + self.graphics = True + self.animation = False + + def __init__(self): + self.options = self.Opt() + +class RobotBlockTest(unittest.TestCase): + def test_fkine(self): + + robot = rtb.models.ETS.Panda() + q = robot.configs["qr"] + T = robot.fkine(q) + + block = FKine(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], T) + + def test_ikine(self): + + robot = rtb.models.ETS.Panda() + q = robot.configs["qr"] + T = robot.fkine(q) + sol = robot.ikine_LM(T) + + block = IKine(robot, seed=0) + + q_ik = block.T_output(T)[0] # get IK from block + pass + nt.assert_array_almost_equal(robot.fkine(q_ik), T) # test it's FK is correct + + def test_jacobian(self): + + robot = rtb.models.ETS.Panda() + q = robot.configs["qr"] + + J = robot.jacob0(q) + block = Jacobian(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], J) + + J = robot.jacobe(q) + block = Jacobian(robot, frame="e") + nt.assert_array_almost_equal(block.T_output(q)[0], J) + + J = robot.jacob0(q) + block = Jacobian(robot, pinv=True) + nt.assert_array_almost_equal(block.T_output(q)[0], np.linalg.pinv(J)) + + def test_gravload(self): + + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = Gravload(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], robot.gravload(q)) + + def test_gravload_x(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = Gravload_X(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], robot.gravload_x(q)) + + def test_inertia(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = Inertia(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], robot.inertia(q)) + + def test_inertia_x(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = Inertia_X(robot) + nt.assert_array_almost_equal(block.T_output(q)[0], robot.inertia_x(q)) + + def test_idyn(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = IDyn(robot) + qd = [0.1, 0.1, 0.1, 0.2, 0.2, 0.3] + qdd = [0.1, 0.1, 0.1, 0.2, 0.2, 0.3] + nt.assert_array_almost_equal( + block.T_output(q, qd, qdd)[0], robot.rne(q, qd, qdd) + ) + + block = IDyn(robot, gravity=[0, 0, 0]) + qd = [0.1, 0.1, 0.1, 0.2, 0.2, 0.3] + qdd = [0.1, 0.1, 0.1, 0.2, 0.2, 0.3] + nt.assert_array_almost_equal( + block.T_output(q, qd, qdd)[0], robot.rne(q, qd, qdd, gravity=[0, 0, 0]) + ) + + def test_fdyn(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = FDyn(robot, q) + qd = np.zeros((6,)) + tau = [6, 5, 4, 3, 2, 1] + + x = np.r_[q, np.zeros((6,))] + xd = np.r_[np.zeros((6,)), robot.accel(q, qd, tau)] + nt.assert_equal(block.T_deriv(tau, x=x), xd) + nt.assert_equal(block.T_output(tau, x=x)[0], q) + nt.assert_equal(block.getstate0(), x) + + @unittest.skip + def test_fdyn_x(self): + robot = rtb.models.DH.Puma560() + q = robot.configs["qn"] + + block = FDyn_X(robot, q) + qd = np.zeros((6,)) + tau = [6, 5, 4, 3, 2, 1] + + x = np.r_[tr2x(robot.fkine(q).A), np.zeros((6,))] + xd = np.r_[np.zeros((6,)), robot.accel_x(q, qd, tau)] + block.test_inputs = [tau] # set inputs + block._x = x # set state [x, xd] + nt.assert_equal(block.deriv(), xd) + nt.assert_equal(block.output()[0], tr2x(robot.fkine(q).A)) + nt.assert_equal(block.getstate0(), x) + + @unittest.skip("cant test bdsim plot blocks") + def test_armplot(self): + + robot = rtb.models.ETS.Panda() + q = robot.configs["qr"] + + block = ArmPlot(robot) + + +class SpatialBlockTest(unittest.TestCase): + def test_delta(self): + + block = Tr2Delta() + + T1 = SE3() + T2 = SE3.Trans(0.01, 0.02, 0.03) * SE3.RPY(0.01, 0.02, 0.03) + nt.assert_array_almost_equal(block.T_output(T1, T2)[0], T1.delta(T2)) + + block = Delta2Tr() + + delta = [0.01, 0.02, 0.03, 0.04, 0.05, 0.06] + nt.assert_array_almost_equal(block.T_output(delta)[0], SE3.Delta(delta)) + + def test_tr2t(self): + + T = SE3.Trans(1, 2, 3) * SE3.RPY(0.3, 0.4, 0.5) + + block = TR2T() + out = block.T_output(T) + self.assertEqual(len(out), 3) + self.assertAlmostEqual(out[0], 1) + self.assertAlmostEqual(out[1], 2) + self.assertAlmostEqual(out[2], 3) + + def test_point2tr(self): + + T = SE3.Trans(1, 2, 3) * SE3.RPY(0.3, 0.4, 0.5) + + block = Point2Tr(T) + + t = np.r_[3, 4, 5] + nt.assert_array_almost_equal( + block.T_output(t)[0], SE3.Trans(t) * SE3.RPY(0.3, 0.4, 0.5) + ) + + @unittest.skip + def test_jtraj(self): + robot = rtb.models.DH.Puma560() + q1 = robot.configs["qz"] + q2 = robot.configs["qr"] + + block = JTraj(q1, q2) + s = State() + block.start(s) + nt.assert_array_almost_equal(block.T_output(t=0)[0], q1) + nt.assert_array_almost_equal(block.T_output(t=5)[0], q2) + + def test_ctraj(self): + + T1 = SE3.Trans(1, 2, 3) * SE3.RPY(0.3, 0.4, 0.5) + T2 = SE3.Trans(-1, -2, -3) * SE3.RPY(-0.3, -0.4, -0.5) + + block = CTraj(T1, T2, T=5) + + s = State() + block.start(s) + + nt.assert_array_almost_equal(block.T_output(t=0)[0], T1) + nt.assert_array_almost_equal(block.T_output(t=5)[0], T2) + + def test_trapezoidal(self): + + block = Trapezoidal(2, 3, T=5) + + s = State() + block.start(s) + + out = block.T_output(t=0) + nt.assert_array_almost_equal(out[0], 2) + nt.assert_array_almost_equal(out[1], 0) + + out = block.T_output(t=5) + nt.assert_array_almost_equal(out[0], 3) + nt.assert_array_almost_equal(out[1], 0) + + def test_circlepath(self): + + block = CirclePath( + radius=2, centre=[1, 2, 3], frequency=0.25, phase=0, unit="rps" + ) + + nt.assert_array_almost_equal(block.T_output(t=0)[0], (1 + 2, 2, 3)) + nt.assert_array_almost_equal(block.T_output(t=1)[0], (1, 2 + 2, 3)) + nt.assert_array_almost_equal(block.T_output(t=2)[0], (1 - 2, 2, 3)) + + def test_traj(self): + + block = Traj([1, 2], [3, 4], time=True, traj="trapezoidal", T=5) + s = State() + block.start(s) + + nt.assert_array_almost_equal(block.T_output(t=0)[0], [1, 2]) + nt.assert_array_almost_equal(block.T_output(t=0)[1], [0, 0]) + + nt.assert_array_almost_equal(block.T_output(t=5)[0], [3, 4]) + nt.assert_array_almost_equal(block.T_output(t=5)[1], [0, 0]) + + nt.assert_array_almost_equal(block.T_output(t=2.5)[0], [2, 3]) + + block = Traj([1, 2], [3, 4], time=True, traj="quintic", T=5) + block.start(s) + + nt.assert_array_almost_equal(block.T_output(t=0)[0], [1, 2]) + nt.assert_array_almost_equal(block.T_output(t=0)[1], [0, 0]) + nt.assert_array_almost_equal(block.T_output(t=0)[2], [0, 0]) + + nt.assert_array_almost_equal(block.T_output(t=5)[0], [3, 4]) + nt.assert_array_almost_equal(block.T_output(t=5)[1], [0, 0]) + nt.assert_array_almost_equal(block.T_output(t=5)[2], [0, 0]) + + nt.assert_array_almost_equal(block.T_output(t=2.5)[0], [2, 3]) + + +class MobileBlockTest(unittest.TestCase): + def test_bicycle(self): + + x = [2, 3, np.pi / 2] + block = Bicycle(x0=x, L=3) + + nt.assert_array_almost_equal(block.T_output(0, 0, x=x, t=0)[0], x) + nt.assert_array_almost_equal(block.T_deriv(0, 0, x=x), [0, 0, 0]) + + nt.assert_array_almost_equal(block.T_output(10, 0.3, x=x, t=0)[0], x) + nt.assert_array_almost_equal( + block.T_deriv(10, 0.3, x=x), [10 * np.cos(x[2]), 10 * np.sin(x[2]), 10 / 3 * np.tan(0.3)] + ) + + def test_unicycle(self): + + x = [2, 3, np.pi / 2] + block = Unicycle(x0=x, W=3) + + nt.assert_array_almost_equal(block.T_output(0, 0, x=x)[0], x) + nt.assert_array_almost_equal(block.T_deriv(0, 0, x=x), [0, 0, 0]) + + nt.assert_array_almost_equal(block.T_output(10, 0.3, x=x)[0], x) + nt.assert_array_almost_equal( + block.T_deriv(10, 0.3, x=x), [10 * np.cos(x[2]), 10 * np.sin(x[2]), 0.3] + ) + + def test_diffsteer(self): + + x = [2, 3, np.pi / 2] + block = DiffSteer(x0=x, W=3, R=1 / np.pi) + + nt.assert_array_almost_equal(block.T_output(0, 0, x=x)[0], x) + nt.assert_array_almost_equal(block.T_deriv(0, 0, x=x), [0, 0, 0]) + + nt.assert_array_almost_equal(block.T_output(5, -5, x=x)[0], x) + nt.assert_array_almost_equal(block.T_deriv(5, -5, x=x), [0, 0, -10]) + + @unittest.skip("cant test bdsim plot blocks") + def test_vehicleplot(self): + + bike = Bicycle() + block = VehiclePlot() + + s = State() + block.T_start(s) + block.T_step(np.array([0, 0, 0])) + + +class MultirotorBlockTest(unittest.TestCase): + def test_multirotor(self): + + x = np.r_[[1, 2, 3, 0, 0, 0], np.zeros((6,))] + block = MultiRotor(model=quadrotor) + + out = block.T_output( + np.r_[614.675223, -614.675223, 614.675223, -614.675223], t=0, x=x + )[0] + self.assertIsInstance(out, dict) + + out = block.T_deriv(100*np.r_[1, 1, 1, 1], x=x) + self.assertIsInstance(out, np.ndarray) + self.assertEqual(out.shape, (12,)) + + def test_multirotormixer(self): + + block = MultiRotorMixer(model=quadrotor) + nt.assert_array_almost_equal( + block.T_output(0, 0, 0, -20, t=0)[0], + [614.675223, -614.675223, 614.675223, -614.675223], + ) + + @unittest.skip("cant test bdsim plot blocks") + def test_multirotorplot(self): + + block = MultiRotorPlot(model=quadrotor) + + class State: + pass + + s = State() + + block.start(state=s) + block.step(state=s) + + def test_quadrotor(self): + + block = MultiRotor(quadrotor) + print(block.D) + z = np.r_[0, 0, 0, 0] + block.test_inputs = [z] + nt.assert_equal(block.getstate0(), np.zeros((12,))) + block.setstate(block.getstate0()) + + x = block.getstate0() + x[2] = -100 # set altitude + u = 100 * np.r_[1, -1, 1, -1] + + # check outputs + out = block.T_output(u, x=x) + self.assertIsInstance(out, list) + self.assertEqual(len(out), 1) + + self.assertIsInstance(out[0], dict) + + # check deriv, checked against MATLAB version 20200621 + u = 800 * np.r_[1, -1, 1, -1] # too little thrust, falling + d = block.T_deriv(u, x=x) + self.assertIsInstance(d, np.ndarray) + self.assertEqual(d.shape, (12,)) + self.assertGreater(d[8], 0) + nt.assert_array_almost_equal( + np.delete(d, 8), np.zeros((11,)) + ) # other derivs are zero + + u = 900 * np.r_[1, -1, 1, -1] # too much thrust, rising + self.assertLess(block.T_deriv(u, x=x)[8], 0) + + u = 800 * np.r_[1.2, -1, 0.8, -1] # + pitch + self.assertGreater(block.T_deriv(u, x=x)[10], 20) + + u = 800 * np.r_[0.8, -1, 1.2, -1] # - pitch + self.assertLess(block.T_deriv(u, x=x)[10], -20) + + u = 800 * np.r_[1, -0.8, 1, -1.2] # + roll + self.assertGreater(block.T_deriv(u, x=x)[9], 20) + + u = 800 * np.r_[1, -1.2, 1, -0.8] # - roll + self.assertLess(block.T_deriv(u, x=x)[9], -20) + + @unittest.skip("cant test bdsim plot blocks") + def test_quadrotorplot(self): + + block = MultiRotor(quadrotor) + u = [100 * np.r_[1, -1, 1, -1]] + x = block.getstate0() + out = block.T_output(u, x=x)[0] + + # block = MultiRotorPlot(quadrotor) + # s = block.T_start() + # block.T_step(out, s) + + +# ---------------------------------------------------------------------------------------# +if __name__ == "__main__": + + unittest.main() diff --git a/tests/test_jacob.py b/tests/test_jacob.py index bd94b5f66..84b3ea092 100644 --- a/tests/test_jacob.py +++ b/tests/test_jacob.py @@ -96,6 +96,224 @@ def test_jacob_dot_analytical_eul(self): print(np.round(Jd, 2)) nt.assert_array_almost_equal(j0, Jd, decimal=4) + # ------ This section tests various ets' with flipped joints ------ # + + def test_jacob0_flipped0(self): + robot = rtb.ETS( + [ + rtb.ET.Rz(jindex=0, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.SE3( + T=np.array( + [ + [1.0000e00, 0.0000e00, 0.0000e00, -4.3624e-04], + [0.0000e00, 1.0000e00, 0.0000e00, 0.0000e00], + [0.0000e00, 0.0000e00, 1.0000e00, 3.6000e-01], + [0.0000e00, 0.0000e00, 0.0000e00, 1.0000e00], + ] + ) + ), + rtb.ET.Ry(jindex=1, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=2, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.SE3( + T=np.array( + [ + [1.0000e00, 0.0000e00, 0.0000e00, 4.3624e-04], + [0.0000e00, 1.0000e00, 0.0000e00, 0.0000e00], + [0.0000e00, 0.0000e00, 1.0000e00, 4.2000e-01], + [0.0000e00, 0.0000e00, 0.0000e00, 1.0000e00], + ] + ) + ), + rtb.ET.Ry(jindex=3, flip=True, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=4, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.tz(0.4), + rtb.ET.Ry(jindex=5, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=6, qlim=np.array([-3.0541, 3.0541])), + rtb.ET.tz(0.126), + ] + ) + + q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79]) + + nt.assert_array_almost_equal( + robot.jacob0(q), numjac(lambda q: robot.fkine(q).A, q, SE=3) + ) + + def test_jacob0_flipped1(self): + ets_list = [ + rtb.ET.Rx(flip=True) + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry(flip=True) + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx(flip=True) + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty(flip=True) + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(flip=True), + rtb.ET.Rx(flip=True) + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty(flip=True) + * rtb.ET.tz(), + rtb.ET.Rx(flip=True) + * rtb.ET.Ry(flip=True) + * rtb.ET.Rz(flip=True) + * rtb.ET.tx(flip=True) + * rtb.ET.ty(flip=True) + * rtb.ET.tz(flip=True), + ] + + for ets in ets_list: + q = np.array([-0.3, 0, -2.2, 0, 2, 0.79]) + + nt.assert_array_almost_equal( + ets.jacob0(q), numjac(lambda q: ets.fkine(q).A, q, SE=3) + ) + + def test_jacobe_flipped0(self): + robot = rtb.ETS( + [ + rtb.ET.Rz(jindex=0, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.SE3( + T=np.array( + [ + [1.0000e00, 0.0000e00, 0.0000e00, -4.3624e-04], + [0.0000e00, 1.0000e00, 0.0000e00, 0.0000e00], + [0.0000e00, 0.0000e00, 1.0000e00, 3.6000e-01], + [0.0000e00, 0.0000e00, 0.0000e00, 1.0000e00], + ] + ) + ), + rtb.ET.Ry(jindex=1, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=2, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.SE3( + T=np.array( + [ + [1.0000e00, 0.0000e00, 0.0000e00, 4.3624e-04], + [0.0000e00, 1.0000e00, 0.0000e00, 0.0000e00], + [0.0000e00, 0.0000e00, 1.0000e00, 4.2000e-01], + [0.0000e00, 0.0000e00, 0.0000e00, 1.0000e00], + ] + ) + ), + rtb.ET.Ry(jindex=3, flip=True, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=4, qlim=np.array([-2.9668, 2.9668])), + rtb.ET.tz(0.4), + rtb.ET.Ry(jindex=5, qlim=np.array([-2.0942, 2.0942])), + rtb.ET.Rz(jindex=6, qlim=np.array([-3.0541, 3.0541])), + rtb.ET.tz(0.126), + ] + ) + + q = np.array([0, -0.3, 0, -2.2, 0, 2, 0.79]) + + J0 = numjac(lambda q: robot.fkine(q).A, q, SE=3) + TE = robot.fkine(q) + Je = block_diag(TE.R.T, TE.R.T) @ J0 + + nt.assert_array_almost_equal(robot.jacobe(q), Je) + + def test_jacobe_flipped1(self): + ets_list = [ + rtb.ET.Rx(flip=True) + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry(flip=True) + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx(flip=True) + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty(flip=True) + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz() + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(flip=True), + rtb.ET.Rx(flip=True) + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty() + * rtb.ET.tz(), + rtb.ET.Rx() + * rtb.ET.Ry() + * rtb.ET.Rz(flip=True) + * rtb.ET.tx() + * rtb.ET.ty(flip=True) + * rtb.ET.tz(), + rtb.ET.Rx(flip=True) + * rtb.ET.Ry(flip=True) + * rtb.ET.Rz(flip=True) + * rtb.ET.tx(flip=True) + * rtb.ET.ty(flip=True) + * rtb.ET.tz(flip=True), + ] + + for ets in ets_list: + q = np.array([-0.3, 0, -2.2, 0, 2, 0.79]) + + J0 = numjac(lambda q: ets.fkine(q).A, q, SE=3) + TE = ets.fkine(q) + Je = block_diag(TE.R.T, TE.R.T) @ J0 + + nt.assert_array_almost_equal(ets.jacobe(q), Je) + # def test_jacob_dot_analytical_rpy_xyz(self): # rep = 'rpy/xyz' # j0 = self.robot.jacob_dot(self.q, self.qd, analytical=rep) @@ -143,5 +361,4 @@ def test_jacob_dot_analytical_eul(self): if __name__ == "__main__": - unittest.main() diff --git a/tests/test_mobile.py b/tests/test_mobile.py index 02b4bcb6c..7143f6bd8 100644 --- a/tests/test_mobile.py +++ b/tests/test_mobile.py @@ -293,6 +293,42 @@ def test_jacobians(self): ) +class TestUnicycle(unittest.TestCase): + def test_str(self): + """ + check the string representation of the unicycle + """ + uni = Unicycle() + self.assertEqual( + str(uni), + """Unicycle: x = [ 0, 0, 0 ] + W=1, steer_max=inf, vel_max=inf, accel_max=inf""", + ) + + uni = Unicycle(steer_max=0.7) + self.assertEqual( + str(uni), + """Unicycle: x = [ 0, 0, 0 ] + W=1, steer_max=0.7, vel_max=inf, accel_max=inf""", + ) + + def test_deriv(self): + """ + test the derivative function + """ + uni = Unicycle() + + state = np.r_[0, 0, 0] + input = [1, 0] # no rotation + nt.assert_almost_equal(uni.deriv(state, input), np.r_[1.0, 0, 0]) + + input = [0, 1] # only rotate + nt.assert_almost_equal(uni.deriv(state, input), np.r_[0, 0, 1]) + + input = [1, 1] # turn and rotate + nt.assert_almost_equal(uni.deriv(state, input), np.r_[1, 0, 1]) + + # function setupOnce(testCase) # testCase.TestData.Duration = 50; # end diff --git a/tests/test_mobile_planner.py b/tests/test_mobile_planner.py new file mode 100644 index 000000000..67ad4f8d2 --- /dev/null +++ b/tests/test_mobile_planner.py @@ -0,0 +1,177 @@ +from collections import namedtuple +from math import pi +import numpy.testing as nt +import numpy as np +import unittest +import spatialmath.base as sm + +from roboticstoolbox.mobile import * + + +class TestPlanners(unittest.TestCase): + + def test_occgrid(self): + g = np.zeros((100, 120)) + g[20:30, 50:80] = 1 + + og = BinaryOccupancyGrid(g, name='my grid') + + self.assertEqual(og.shape, g.shape) + + s = str(og) + self.assertIsInstance(s, str) + self.assertEqual(s, "BinaryOccupancyGrid[my grid]: 120 x 100, cell size=1, x = [0.0, 119.0], y = [0.0, 99.0], 2.5% occupied") + + self.assertEqual(og.xmin, 0) + self.assertEqual(og.xmax, 119) + self.assertEqual(og.ymin, 0) + self.assertEqual(og.ymax, 99) + + self.assertTrue(og.isoccupied((50, 20))) + self.assertTrue(og.isoccupied((60, 25))) + self.assertTrue(og.isoccupied((200, 200))) + self.assertFalse(og.isoccupied((0, 0))) + self.assertFalse(og.isoccupied((80, 30))) + + og.plot(block=False) + + og2 = og.copy() + + self.assertEqual(og2.xmin, 0) + self.assertEqual(og2.xmax, 119) + self.assertEqual(og2.ymin, 0) + self.assertEqual(og2.ymax, 99) + + self.assertTrue(og2.isoccupied((50, 20))) + self.assertTrue(og2.isoccupied((60, 25))) + self.assertTrue(og2.isoccupied((200, 200))) + self.assertFalse(og2.isoccupied((0, 0))) + self.assertFalse(og2.isoccupied((80, 30))) + self.assertFalse(og2.isoccupied((45, 20))) + + og2.inflate(5) + + self.assertTrue(og2.isoccupied((50, 20))) + self.assertTrue(og2.isoccupied((60, 25))) + self.assertTrue(og2.isoccupied((200, 200))) + self.assertFalse(og2.isoccupied((0, 0))) + self.assertTrue(og2.isoccupied((80, 30))) + self.assertTrue(og2.isoccupied((45, 20))) + + self.assertEqual(str(og2), "BinaryOccupancyGrid[my grid]: 120 x 100, cell size=1, x = [0.0, 119.0], y = [0.0, 99.0], 6.3% occupied") + + # check no change to original + self.assertFalse(og.isoccupied((80, 30))) + + og = BinaryOccupancyGrid(g, cellsize=0.1, origin=(2,4), name='foo') + + self.assertEqual(og.xmin, 2) + self.assertEqual(og.xmax, 13.9) + self.assertEqual(og.ymin, 4) + self.assertEqual(og.ymax, 13.9) + self.assertTrue(og.isoccupied((8.5, 6.5))) + self.assertTrue(og.isoccupied((500, 500))) + self.assertFalse(og.isoccupied((3, 5))) + + og.inflate(0.5) + self.assertEqual(str(og), "BinaryOccupancyGrid[foo]: 120 x 100, cell size=0.1, x = [2.0, 13.9], y = [4.0, 13.9], 6.3% occupied") + + def test_bug2(self): + pass + + def test_dubins(self): + + start = (0, 0, pi/2) + goal = (1, 0, pi/2) + + dubins = DubinsPlanner(curvature=1.0) + path, status = dubins.query(start, goal) + + self.assertIsInstance(path, np.ndarray) + self.assertEqual(path.shape, (74,3)) + self.assertEqual(status.__class__.__name__, "DubinsStatus") + self.assertTrue(hasattr(status, 'segments')) + self.assertTrue(hasattr(status, 'length')) + + def test_reedsshepp(self): + + start = (0, 0, pi/2) + goal = (1, 0, pi/2) + + rs = ReedsSheppPlanner(curvature=1.0) + path, status = rs.query(start, goal) + + self.assertIsInstance(path, np.ndarray) + self.assertEqual(path.shape, (65,3)) + self.assertEqual(status.__class__.__name__, "ReedsSheppStatus") + self.assertTrue(hasattr(status, 'segments')) + self.assertTrue(hasattr(status, 'length')) + self.assertTrue(hasattr(status, 'direction')) + + # def test_bug2(self): + + # vars = loadmat("data/map1.mat") + # map = vars['map'] + + # bug = Bug2Planner(map) + # # bug.plan() + # path = bug.query([20, 10], [50, 35]) + + # # valid path + # self.assertTrue(path is not None) + + # # valid Nx2 array + # self.assertIsInstance(path, np.ndarray) + # self.assertEqual(path.shape[1], 2) + + # # includes start and goal + # self.assertTrue(all(path[0,:] == [20,10])) + # self.assertTrue(all(path[-1,:] == [50,35])) + + # # path doesn't include obstacles + # for p in path: + # self.assertFalse(bug.is_occupied(p)) + + # # there are no gaps + # for k in range(len(path)-1): + # d = np.linalg.norm(path[k] - path[k+1]) + # self.assertTrue(d < 1.5) + + # bug.plot() + # bug.plot(path=path) + + # def test_dxform(self): + + # vars = loadmat("data/map1.mat") + # map = vars['map'] + + # dx = DXform(map) + # dx.plan([50, 35]) + # path = dx.query([20, 10]) + + # # valid path + # self.assertTrue(path is not None) + + # # valid Nx2 array + # self.assertIsInstance(path, np.ndarray) + # self.assertEqual(path.shape[1], 2) + + # # includes start and goal + # self.assertTrue(all(path[0,:] == [20,10])) + # self.assertTrue(all(path[-1,:] == [50,35])) + + # # path doesn't include obstacles + # for p in path: + # self.assertFalse(dx.is_occupied(p)) + + # # there are no gaps + # for k in range(len(path)-1): + # d = np.linalg.norm(path[k] - path[k+1]) + # self.assertTrue(d < 1.5) + + # dx.plot() + # dx.plot(path=path) +if __name__ == '__main__': # pragma nocover + + unittest.main() + # pytest.main(['tests/test_SerialLink.py']) diff --git a/tests/test_tools.py b/tests/test_tools.py index d6ed8b972..6d7d6d39b 100644 --- a/tests/test_tools.py +++ b/tests/test_tools.py @@ -5,7 +5,7 @@ import numpy.testing as nt import numpy as np -import roboticstoolbox as rp +import roboticstoolbox as rtb import spatialmath as sm import unittest @@ -25,9 +25,9 @@ def test_null(self): ans2 = np.array([[-0.4082], [0.8165], [-0.4082]]) - r0 = rp.null(a0) - r1 = rp.null(a1) - r2 = rp.null(a2) + r0 = rtb.null(a0) + r1 = rtb.null(a1) + r2 = rtb.null(a2) nt.assert_array_almost_equal(np.abs(r0), np.abs(ans0), decimal=4) nt.assert_array_almost_equal(np.abs(r1), np.abs(ans1), decimal=4) @@ -38,9 +38,9 @@ def test_p_servo_rpy(self): b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1) c = sm.SE3.Tz(0.59) - v0, arrived0 = rp.p_servo(a, b, method="rpy") - v1, _ = rp.p_servo(a.A, b.A, method="rpy") - _, arrived1 = rp.p_servo(a, c, threshold=0.6, method="rpy") + v0, arrived0 = rtb.p_servo(a, b, method="rpy") + v1, _ = rtb.p_servo(a.A, b.A, method="rpy") + _, arrived1 = rtb.p_servo(a, c, threshold=0.6, method="rpy") ans = np.array([1, 0, 0, 0.7, -0, 0]) @@ -55,9 +55,9 @@ def test_p_servo_angle_axis(self): b = sm.SE3.Rx(0.7) * sm.SE3.Tx(1) c = sm.SE3.Tz(0.59) - v0, arrived0 = rp.p_servo(a, b, method="angle-axis") - v1, _ = rp.p_servo(a.A, b.A) - _, arrived1 = rp.p_servo(a, c, threshold=0.6) + v0, arrived0 = rtb.p_servo(a, b, method="angle-axis") + v1, _ = rtb.p_servo(a.A, b.A) + _, arrived1 = rtb.p_servo(a, c, threshold=0.6) ans = np.array([1, 0, 0, 0.7, -0, 0]) @@ -68,10 +68,35 @@ def test_p_servo_angle_axis(self): self.assertTrue(arrived1) def test_jsingu(self): - r = rp.models.Puma560() + r = rtb.models.Puma560() J = r.jacob0(r.qz) - rp.jsingu(J) + rtb.jsingu(J) + + def test_c_angle_axis(self): + n = 100 + + coord = (np.random.random((n, 6)) - 1.0) * 3.0 + coord2 = (np.random.random((n, 6)) - 1.0) * 3.0 + + for co, co2 in zip(coord, coord2): + Te = ( + sm.SE3.Trans(co[:3]) + * sm.SE3.Rx(co[3]) + * sm.SE3.Ry(co[4]) + * sm.SE3.Rz(co[5]) + ).A + Tep = ( + sm.SE3.Trans(co2[:3]) + * sm.SE3.Rx(co2[3]) + * sm.SE3.Ry(co2[4]) + * sm.SE3.Rz(co2[5]) + ).A + + e1 = rtb.angle_axis(Te, Tep) + e2 = rtb.angle_axis_python(Te, Tep) + + nt.assert_allclose(e1, e2) if __name__ == "__main__": # pragma nocover