diff --git a/.github/workflows/cibuildwheel.yml b/.github/workflows/cibuildwheel.yml new file mode 100644 index 000000000..ebe1a8ca6 --- /dev/null +++ b/.github/workflows/cibuildwheel.yml @@ -0,0 +1,65 @@ +name: Build all Wheels + +on: + release: + types: [created] + workflow_dispatch: + +jobs: + build_wheels: + name: Build wheels on ${{ matrix.os }} + runs-on: ${{ matrix.os }} + + strategy: + matrix: + os: [ubuntu-20.04, windows-2019, macos-11] + include: + - os: ubuntu-20.04 + cibw_archs: "aarch64" + + steps: + + # Set up emulation for arm on linux + - name: Set up QEMU + if: matrix.cibw_archs == 'aarch64' + uses: docker/setup-qemu-action@v2 + with: + platforms: arm64 + - uses: actions/checkout@v3 + + - name: Build wheels + uses: pypa/cibuildwheel@v2.11.3 + + - uses: actions/upload-artifact@v3 + with: + path: ./wheelhouse/*.whl + + build_sdist: + name: Build source distribution + runs-on: ubuntu-latest + steps: + - uses: actions/checkout@v3 + + - name: Build sdist + run: pipx run build --sdist + + - uses: actions/upload-artifact@v3 + with: + path: dist/*.tar.gz + + upload_pypi: + needs: [build_wheels, build_sdist] + runs-on: ubuntu-latest + steps: + - uses: actions/download-artifact@v3 + with: + # unpacks default artifact into dist/ + name: artifact + path: dist + + - uses: pypa/gh-action-pypi-publish@v1.5.0 + with: + user: __token__ + password: ${{ secrets.PYPI_PASSWORD }} + + diff --git a/.github/workflows/codeql_future.yml b/.github/workflows/codeql_future.yml index 8be7bdad9..0d18b779f 100644 --- a/.github/workflows/codeql_future.yml +++ b/.github/workflows/codeql_future.yml @@ -9,68 +9,67 @@ # the `language` matrix defined below to confirm you have the correct set of # supported CodeQL languages. # -name: "CodeQL - Future" +# name: "CodeQL - Future" -on: - push: - branches: [ "future" ] - pull_request: - # The branches below must be a subset of the branches above - branches: [ "master" ] - schedule: - - cron: '25 23 * * 4' +# on: +# push: +# branches: [ "future" ] +# pull_request: +# # The branches below must be a subset of the branches above +# branches: [ "master" ] +# schedule: +# - cron: '25 23 * * 4' -jobs: - analyze: - name: Analyze - runs-on: ubuntu-latest - permissions: - actions: read - contents: read - security-events: write +# jobs: +# analyze: +# name: Analyze +# runs-on: ubuntu-latest +# permissions: +# actions: read +# contents: read +# security-events: write - strategy: - fail-fast: false - matrix: - language: [ 'cpp', 'python' ] - # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python', 'ruby' ] - # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support +# strategy: +# fail-fast: false +# matrix: +# language: [ 'cpp', 'python' ] +# # CodeQL supports [ 'cpp', 'csharp', 'go', 'java', 'javascript', 'python', 'ruby' ] +# # Learn more about CodeQL language support at https://aka.ms/codeql-docs/language-support - steps: - - name: Checkout repository - uses: actions/checkout@v3 - with: - ref: future +# steps: +# - name: Checkout repository +# uses: actions/checkout@v3 +# with: +# ref: future - # Initializes the CodeQL tools for scanning. - - name: Initialize CodeQL - uses: github/codeql-action/init@v2 - with: - languages: ${{ matrix.language }} - # If you wish to specify custom queries, you can do so here or in a config file. - # By default, queries listed here will override any specified in a config file. - # Prefix the list here with "+" to use these queries and those in the config file. - - # Details on CodeQL's query packs refer to : https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs - # queries: security-extended,security-and-quality +# # Initializes the CodeQL tools for scanning. +# - name: Initialize CodeQL +# uses: github/codeql-action/init@v2 +# with: +# languages: ${{ matrix.language }} +# # If you wish to specify custom queries, you can do so here or in a config file. +# # By default, queries listed here will override any specified in a config file. +# # Prefix the list here with "+" to use these queries and those in the config file. - - # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). - # If this step fails, then you should remove it and run the build manually (see below) - - name: Autobuild - uses: github/codeql-action/autobuild@v2 +# # Details on CodeQL's query packs refer to : https://docs.github.com/en/code-security/code-scanning/automatically-scanning-your-code-for-vulnerabilities-and-errors/configuring-code-scanning#using-queries-in-ql-packs +# # queries: security-extended,security-and-quality - # ℹī¸ Command-line programs to run using the OS shell. - # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun +# # Autobuild attempts to build any compiled languages (C/C++, C#, or Java). +# # If this step fails, then you should remove it and run the build manually (see below) +# - name: Autobuild +# uses: github/codeql-action/autobuild@v2 - # If the Autobuild fails above, remove it and uncomment the following three lines. - # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. +# # ℹī¸ Command-line programs to run using the OS shell. +# # 📚 See https://docs.github.com/en/actions/using-workflows/workflow-syntax-for-github-actions#jobsjob_idstepsrun - # - run: | - # echo "Run, Build Application using script" - # ./location_of_script_within_repo/buildscript.sh +# # If the Autobuild fails above, remove it and uncomment the following three lines. +# # modify them (or add more) to build your code if your project, please refer to the EXAMPLE below for guidance. - - name: Perform CodeQL Analysis - uses: github/codeql-action/analyze@v2 - with: - category: "/language:${{matrix.language}}" +# # - run: | +# # echo "Run, Build Application using script" +# # ./location_of_script_within_repo/buildscript.sh + +# - name: Perform CodeQL Analysis +# uses: github/codeql-action/analyze@v2 +# with: +# category: "/language:${{matrix.language}}" diff --git a/.github/workflows/python-publish.yml b/.github/workflows/python-publish.yml deleted file mode 100644 index d746f226b..000000000 --- a/.github/workflows/python-publish.yml +++ /dev/null @@ -1,37 +0,0 @@ -# This workflows will upload a Python Package using Twine when a release is created -# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries - -name: Upload Python Package - -on: - release: - types: [created] - workflow_dispatch: - -jobs: - deploy: - runs-on: ${{ matrix.os }} - strategy: - max-parallel: 2 - matrix: - os: [macos-latest, windows-latest] - python-version: [3.7, 3.8, 3.9, "3.10"] - - steps: - - uses: actions/checkout@v2 - - name: Set up Python - uses: actions/setup-python@v2 - with: - python-version: ${{ matrix.python-version }} - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install -U setuptools wheel twine - - name: Build and publish - env: - TWINE_USERNAME: __token__ - TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }} - run: | - python setup.py sdist bdist_wheel - ls ./dist/*.whl - twine upload dist/*.whl diff --git a/.github/workflows/python_publish_linux.yml b/.github/workflows/python_publish_linux.yml deleted file mode 100644 index 945e9d732..000000000 --- a/.github/workflows/python_publish_linux.yml +++ /dev/null @@ -1,39 +0,0 @@ -# This is a basic workflow that is manually triggered - -name: Linux Build and Publish - -on: - release: - types: [created] - workflow_dispatch: - - -# A workflow run is made up of one or more jobs that can run sequentially or in parallel -jobs: - # This workflow contains a single job called "greet" - deploy: - # The type of runner that the job will run on - runs-on: ubuntu-latest - - # Steps represent a sequence of tasks that will be executed as part of the job - steps: - - name: Set up Python 3.7 - uses: actions/setup-python@v1 - with: - python-version: 3.7 - # Runs a single command using the runners shell - - uses: actions/checkout@v2 - - name: Make wheels - run: | - sudo docker run -e PLAT=manylinux2010_x86_64 -v ${{ github.workspace }}:/io quay.io/pypa/manylinux2010_x86_64 /io/.github/build-wheels.sh - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install -U setuptools wheel twine - - name: Build and publish - env: - TWINE_USERNAME: ${{ secrets.PYPI_USERNAME }} - TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }} - run: | - twine upload wheelhouse/*manylinux* - diff --git a/.github/workflows/upload_tar.yml b/.github/workflows/upload_tar.yml deleted file mode 100644 index e7a8b983a..000000000 --- a/.github/workflows/upload_tar.yml +++ /dev/null @@ -1,36 +0,0 @@ -# This workflows will upload a Python Package using Twine when a release is created -# For more information see: https://help.github.com/en/actions/language-and-framework-guides/using-python-with-github-actions#publishing-to-package-registries - -# name: Upload tar to PyPI - -on: - release: - types: [created] - workflow_dispatch: - -jobs: - deploy: - runs-on: ${{ matrix.os }} - strategy: - max-parallel: 2 - matrix: - os: [ubuntu-latest] - python-version: [3.9] - - steps: - - uses: actions/checkout@v2 - - name: Set up Python - uses: actions/setup-python@v2 - with: - python-version: ${{ matrix.python-version }} - - name: Install dependencies - run: | - python -m pip install --upgrade pip - pip install -U setuptools wheel twine - - name: Build and publish - env: - TWINE_USERNAME: __token__ - TWINE_PASSWORD: ${{ secrets.PYPI_PASSWORD }} - run: | - python setup.py sdist bdist_wheel - twine upload dist/*.gz diff --git a/pyproject.toml b/pyproject.toml new file mode 100644 index 000000000..4a380dc19 --- /dev/null +++ b/pyproject.toml @@ -0,0 +1,190 @@ +[project] + +name = "roboticstoolbox-python" + +description = "A Python library for robotics education and research" + +version = "1.0.3" + +authors = [ + { name = "Jesse Haviland", email = "j.haviland@qut.edu.au" }, + { name = "Peter Corke", email = "rvc@petercorke.com" }, +] + +dependencies = [ + "numpy>=1.17.4", + "spatialmath-python~=1.0.0", + "spatialgeometry~=1.0.0", + "pgraph-python", + "scipy", + "matplotlib", + "ansitable", + "swift-sim~=1.0.0", + "rtb-data", + "progress", +] + +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"] + +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", +] + + +[project.urls] +homepage = "https://github.com/petercorke/robotics-toolbox-python" +documentation = "https://petercorke.github.io/robotics-toolbox-python/" +repository = "https://github.com/petercorke/robotics-toolbox-python" + + + +[project.optional-dependencies] + +collision = [ + "pybullet" +] + +dev = [ + "black", + "pytest", + "pytest-cov", + "flake8", + "pyyaml", + "sympy", +] + +docs = [ + "sphinx", + "sphinx_rtd_theme", + "sphinx-autorun", +] + + + +[project.scripts] + +# rtbtool = "roboticstoolbox.bin.rtbtool" +eigdemo = "roboticstoolbox.examples.eigdemo:main" +tripleangledemo = "roboticstoolbox.examples.tripleangledemo:main" +twistdemo = "roboticstoolbox.examples.twistdemo:main" + + + +[build-system] + +requires = [ + "setuptools", + "oldest-supported-numpy" +] +build-backend = "setuptools.build_meta" + + + +[tool.setuptools] + +packages = [ + "roboticstoolbox", + "roboticstoolbox.backends", + "roboticstoolbox.backends.PyPlot", + "roboticstoolbox.backends.swift", + "roboticstoolbox.backends.VPython", + "roboticstoolbox.bin", + "roboticstoolbox.blocks", + "roboticstoolbox.examples", + "roboticstoolbox.robot", + "roboticstoolbox.mobile", + "roboticstoolbox.models", + "roboticstoolbox.models.DH", + "roboticstoolbox.models.ETS", + "roboticstoolbox.models.URDF", + "roboticstoolbox.tools", + "roboticstoolbox.tools.urdf", + "roboticstoolbox.tools.xacro", +] + + + +[tool.black] + +line-length = 88 +target_version = ['py37'] +include = '\.pyi?$' +exclude = ''' + +( + /( + \.eggs # exclude a few common directories in the + | \.git # root of the project + | \.hg + | \.mypy_cache + | \.tox + | \.venv + | \.github + | _build + | buck-out + | build + | dist + | docs + )/ +) +''' + + + +[tool.flake8] + +max-line-length = 88 +extend-ignore = 'E203' + + +[tool.cibuildwheel] + +# Will cause the wheel to be installed with `pip install [dev,collision]` +# test-extras = ["dev", "collision"] +# test-requires = "pytest" +# test-command = "pytest {project}/tests" + +manylinux-x86_64-image = "manylinux2014" +manylinux-aarch64-image = "manylinux2014" + +# Build CPython 3.7 - 3.11 +build = ["cp37-*", "cp38-*", "cp39-*", "cp310-*", "cp311-*"] + +# Disable building musllinux wheels on all platforms +skip = ["pp*", "*musllinux*"] + + +[tool.cibuildwheel.macos] + +# Build `x86_64` and `arm64` wheels on an Intel runner. +# Note that the `arm64` wheel cannot be tested in this configuration. +archs = ["x86_64", "arm64"] + +[tool.cibuildwheel.linux] + +# On an Linux Intel runner with qemu installed, build Intel and ARM wheels +archs = ["x86_64", "aarch64"] + +[tool.cibuildwheel.windows] + +# On an Windows Intel runner build wheels +archs = ["AMD64", "x86"] + + + diff --git a/roboticstoolbox/robot/DHRobot.py b/roboticstoolbox/robot/DHRobot.py index b3309005b..c6ee8adce 100644 --- a/roboticstoolbox/robot/DHRobot.py +++ b/roboticstoolbox/robot/DHRobot.py @@ -31,7 +31,7 @@ from scipy.linalg import block_diag from roboticstoolbox.robot.DHLink import _check_rne, DHLink from roboticstoolbox import rtb_get_param -from frne import init, frne, delete +from roboticstoolbox.frne import init, frne, delete from numpy import any from typing import Union, Tuple @@ -2318,7 +2318,9 @@ def ik_nr( TODO """ - return self.ets().ik_nr(Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping) + return self.ets().ik_nr( + Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping + ) def ik_gn( self, @@ -2424,11 +2426,9 @@ def ik_gn( TODO """ - return self.ets().ik_gn(Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping) - - - - + return self.ets().ik_gn( + Tep, q0, ilimit, slimit, tol, reject_jl, we, use_pinv, pinv_damping + ) class SerialLink(DHRobot): diff --git a/roboticstoolbox/robot/ET.py b/roboticstoolbox/robot/ET.py index 84434a95e..6fe953ca9 100644 --- a/roboticstoolbox/robot/ET.py +++ b/roboticstoolbox/robot/ET.py @@ -18,7 +18,7 @@ tr2xyt, ) from copy import deepcopy -from fknm import ET_T, ET_init, ET_update +from roboticstoolbox.fknm import ET_T, ET_init, ET_update from spatialmath.base import getvector from spatialmath import SE3, SE2 from typing import Optional, Callable, Union diff --git a/roboticstoolbox/robot/ETS.py b/roboticstoolbox/robot/ETS.py index b4b0b0a5a..e08fa46ec 100644 --- a/roboticstoolbox/robot/ETS.py +++ b/roboticstoolbox/robot/ETS.py @@ -40,7 +40,7 @@ from collections import UserList from spatialmath.base import issymbol, getmatrix -from fknm import ( +from roboticstoolbox.fknm import ( ETS_init, ETS_fkine, ETS_jacob0, diff --git a/roboticstoolbox/robot/Gripper.py b/roboticstoolbox/robot/Gripper.py index 4f0730e00..781cca3f9 100644 --- a/roboticstoolbox/robot/Gripper.py +++ b/roboticstoolbox/robot/Gripper.py @@ -10,7 +10,7 @@ from typing import List from functools import lru_cache from typing import Union -from fknm import Robot_link_T +from roboticstoolbox.fknm import Robot_link_T ArrayLike = Union[list, np.ndarray, tuple, set] diff --git a/roboticstoolbox/robot/Robot.py b/roboticstoolbox/robot/Robot.py index cba305350..ca3755f73 100644 --- a/roboticstoolbox/robot/Robot.py +++ b/roboticstoolbox/robot/Robot.py @@ -20,7 +20,7 @@ from roboticstoolbox.robot.IK import IKMixin from typing import Union, Dict, Tuple from spatialgeometry import Shape -from fknm import Robot_link_T +from roboticstoolbox.fknm import Robot_link_T from functools import lru_cache from spatialgeometry import SceneNode from roboticstoolbox.robot.Link import BaseLink, Link diff --git a/setup.py b/setup.py index eb65d04bb..3de45fce7 100644 --- a/setup.py +++ b/setup.py @@ -1,49 +1,8 @@ -from setuptools import setup, find_packages, Extension +from setuptools import setup, Extension import os - -# fmt: off -import pip -pip.main(['install', 'numpy>=1.17.4']) import numpy -# fmt: on - -here = os.path.abspath(os.path.dirname(__file__)) - -req = [ - "numpy>=1.17.4", - "spatialmath-python~=1.0.0", - "spatialgeometry~=1.0.0", - "pgraph-python", - "scipy", - "matplotlib", - "ansitable", - "swift-sim~=1.0.0", - "rtb-data", - "progress", -] - -collision_req = ["pybullet"] - -vp_req = ["vpython", "numpy-stl", "imageio", "imageio-ffmpeg"] - -dev_req = ["pytest", "pytest-cov", "flake8", "pyyaml", "sympy"] - -docs_req = [ - "sphinx", - "sphinx_rtd_theme", - "sphinx-autorun", -] - -# Get the long description from the README file -with open(os.path.join(here, "README.md"), encoding="utf-8") as f: - long_description = f.read() - -# list all data folders here, to ensure they get packaged extra_folders = [ - # 'roboticstoolbox/models/URDF/xacro', - # 'roboticstoolbox/models/DH/meshes', - # 'roboticstoolbox/data', "roboticstoolbox/core", ] @@ -61,7 +20,7 @@ def package_files(directory): extra_files += package_files(extra_folder) frne = Extension( - "frne", + "roboticstoolbox.frne", sources=[ "./roboticstoolbox/core/vmath.c", "./roboticstoolbox/core/ne.c", @@ -70,9 +29,8 @@ def package_files(directory): include_dirs=["./roboticstoolbox/core/"], ) -# eig = "./roboticstoolbox/core/Eigen" fknm = Extension( - "fknm", + "roboticstoolbox.fknm", sources=[ "./roboticstoolbox/core/methods.cpp", "./roboticstoolbox/core/ik.cpp", @@ -80,72 +38,9 @@ def package_files(directory): "./roboticstoolbox/core/fknm.cpp", ], include_dirs=["./roboticstoolbox/core/", numpy.get_include()], - # define_macros=[("EIGEN_USE_MKL_ALL", "1")], - # extra_compile_args=["-Werror"], - # extra_compile_args=["-fopenmp"], - # extra_compile_args=["-Ofast"], - # extra_link_args=["-lgomp"], - # extra_compile_args=["-I/opt/intel/oneapi/mkl/2022.0.2/include"], - # extra_link_args=[ - # "-L/opt/intel/oneapi/mkl/2022.0.2/lib/intel64 -lmkl_rt -Wl,--no-as-needed -lpthread -lm -ldl" - # ], ) setup( - name="roboticstoolbox-python", - version="1.0.2", - description="A Python library for robotic education and research", - long_description=long_description, - long_description_content_type="text/markdown", - url="https://github.com/petercorke/robotics-toolbox-python", - author="Jesse Haviland and Peter Corke", - license="MIT", - classifiers=[ - # 3 - Alpha - # 4 - Beta - # 5 - Production/Stable - "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. In particular, ensure - # that you indicate whether you support Python 2, Python 3 or both. - "Programming Language :: Python :: 3.6", - "Programming Language :: Python :: 3.7", - "Programming Language :: Python :: 3.8", - "Programming Language :: Python :: 3.9", - "Programming Language :: Python :: 3.10", - ], - python_requires=">=3.6", - project_urls={ - "Documentation": "https://petercorke.github.io/roboticstoolbox-python", - "Source": "https://github.com/petercorke/roboticstoolbox-python", - "Tracker": "https://github.com/petercorke/roboticstoolbox-python/issues", - "Coverage": "https://codecov.io/gh/petercorke/roboticstoolbox-python", - }, - # cmdclass={"build_ext": build_ext_subclass}, ext_modules=[frne, fknm], - keywords="python robotics robotics-toolbox kinematics dynamics" - " motion-planning trajectory-generation jacobian hessian" - " control simulation robot-manipulator mobile-robot", - packages=find_packages(exclude=["tests", "notebooks"]), package_data={"roboticstoolbox": extra_files}, - scripts=[ - "roboticstoolbox/bin/rtbtool", - ], - entry_points={ - "console_scripts": [ - "eigdemo=roboticstoolbox.examples.eigdemo:main", - "tripleangledemo=roboticstoolbox.examples.tripleangledemo:main", - "twistdemo=roboticstoolbox.examples.twistdemo:main", - ] - }, - install_requires=req, - extras_require={ - "collision": collision_req, - "dev": dev_req, - "docs": docs_req, - "vpython": vp_req, - }, ) diff --git a/tests/test_DHRobot.py b/tests/test_DHRobot.py index 4228b0d9f..b954dd161 100644 --- a/tests/test_DHRobot.py +++ b/tests/test_DHRobot.py @@ -980,33 +980,33 @@ def test_ikine_LMS(self): 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() + # def test_ikine_unc(self): + # puma = rp.models.DH.Puma560() - T = puma.fkine(puma.qn) + # T = puma.fkine(puma.qn) - sol = puma.ikine_min(T) - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) + # sol = puma.ikine_min(T) + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) - q0 = np.r_[0.1, 0.1, 0.1, 0.2, 0.3, 0.4] - sol = puma.ikine_min(T, q0=q0) - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) + # q0 = np.r_[0.1, 0.1, 0.1, 0.2, 0.3, 0.4] + # sol = puma.ikine_min(T, q0=q0) + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) - def test_ikine_con(self): - puma = rp.models.DH.Puma560() + # def test_ikine_con(self): + # puma = rp.models.DH.Puma560() - T = puma.fkine(puma.qn) + # T = puma.fkine(puma.qn) - sol = puma.ikine_min(T, qlim=True) - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) + # sol = puma.ikine_min(T, qlim=True) + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) - q0 = np.r_[0.1, 0.1, 0.1, 0.2, 0.3, 0.4] - sol = puma.ikine_min(T, q0=q0, qlim=True) - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) + # q0 = np.r_[0.1, 0.1, 0.1, 0.2, 0.3, 0.4] + # sol = puma.ikine_min(T, q0=q0, qlim=True) + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - puma.fkine(sol.q)), 0, places=5) # def test_ikine_min(self): # puma = rp.models.DH.Puma560() diff --git a/tests/test_Robot.py b/tests/test_Robot.py index 1d6da252b..b237f53af 100644 --- a/tests/test_Robot.py +++ b/tests/test_Robot.py @@ -60,128 +60,128 @@ def test_links(self): self.assertIs(r0[2], l2) self.assertIs(r0[3], l3) - 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]) + # 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]) - qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] + # qr = [0.0342, 1.6482, 0.0312, 1.2658, -0.0734, 0.4836, 0.7489] - 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]) + # 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.assertTrue(sol1.success) - self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) + # self.assertTrue(sol1.success) + # self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol1.q)), 0, places=4) - 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 - ) + # 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 + # ) - self.assertTrue(sol3.success) - self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) + # self.assertTrue(sol3.success) + # self.assertAlmostEqual(np.linalg.norm(T - panda.fkine(sol3.q)), 0, places=4) - with self.assertRaises(ValueError): - panda.ikine_LM(T, q0=[1, 2]) + # with self.assertRaises(ValueError): + # panda.ikine_LM(T, q0=[1, 2]) - def test_ikine_LM_mask(self): + # def test_ikine_LM_mask(self): - # simple RR manipulator, solve with mask + # # simple RR manipulator, solve with mask - l0 = rp.RevoluteDH(a=2.0) - l1 = rp.RevoluteDH(a=1) + # l0 = rp.RevoluteDH(a=2.0) + # l1 = rp.RevoluteDH(a=1) - 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]) + # 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.assertTrue(sol.success) - self.assertAlmostEqual( - np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 - ) + # self.assertTrue(sol.success) + # self.assertAlmostEqual( + # np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 + # ) - # 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) + # # 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.assertTrue(sol.success) - self.assertAlmostEqual( - np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 - ) + # self.assertTrue(sol.success) + # self.assertAlmostEqual( + # np.linalg.norm(T.t[:2] - r.fkine(sol.q).t[:2]), 0, places=4 + # ) - def test_ikine_LM_transpose(self): - # need to test this on a robot with squarish Jacobian, choose Puma + # def test_ikine_LM_transpose(self): + # # need to test this on a robot with squarish Jacobian, choose Puma - r = rp.models.DH.Puma560() - T = r.fkine(r.qn) - - sol = r.ikine_LM(T, transpose=0.5, ilimit=1000, tol=1e-6) - - self.assertTrue(sol.success) - self.assertAlmostEqual(np.linalg.norm(T - r.fkine(sol.q)), 0, places=4) - - 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]) - - # qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02, - # -6.98000000e-02, 1.38978915e-02, 9.62104811e-01, - # 7.84926515e-01] - - sol1 = panda.ikine_min(T, qlim=True, q0=np.zeros(7)) - sol2 = panda.ikine_min(Tt, qlim=True) - - 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.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]) - - sol1 = puma.ikine_min(Tt) - sol2 = puma.ikine_min(T) - sol3 = puma.ikine_min(T) - - 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 - ) + # r = rp.models.DH.Puma560() + # T = r.fkine(r.qn) + + # sol = r.ikine_LM(T, transpose=0.5, ilimit=1000, tol=1e-6) + + # self.assertTrue(sol.success) + # self.assertAlmostEqual(np.linalg.norm(T - r.fkine(sol.q)), 0, places=4) + + # 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]) + + # # qr = [7.69161412e-04, 9.01409257e-01, -1.46372859e-02, + # # -6.98000000e-02, 1.38978915e-02, 9.62104811e-01, + # # 7.84926515e-01] + + # sol1 = panda.ikine_min(T, qlim=True, q0=np.zeros(7)) + # sol2 = panda.ikine_min(Tt, qlim=True) + + # 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.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]) + + # sol1 = puma.ikine_min(Tt) + # sol2 = puma.ikine_min(T) + # sol3 = puma.ikine_min(T) + + # 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_plot_swift(self): # r = rp.models.Panda()