├── .github └── workflows │ └── python_package.yml ├── .gitmodules ├── .local_build_and_test.sh ├── CMakeLists.txt ├── LICENSE ├── README.md ├── dqrobotics ├── __init__.py ├── interfaces │ ├── __init__.py │ ├── coppeliasim │ │ └── __init__.py │ ├── json11 │ │ └── __init__.py │ └── vrep │ │ ├── __init__.py │ │ └── robots │ │ └── __init__.py ├── robot_control │ └── __init__.py ├── robot_modeling │ └── __init__.py ├── robots │ └── __init__.py ├── solvers │ ├── __init__.py │ ├── _dq_cplex_solver.py │ └── _dq_quadprog_solver.py └── utils │ ├── DQ_LinearAlgebra │ └── __init__.py │ ├── DQ_Math │ └── __init__.py │ └── __init__.py ├── pyproject.toml ├── setup.py └── src ├── DQ_py.cpp ├── dqrobotics_module.cpp ├── dqrobotics_module.h ├── interfaces ├── json11 │ └── DQ_JsonReader_py.cpp └── vrep │ ├── DQ_SerialVrepRobot_py.cpp │ ├── DQ_VrepInterface_py.cpp │ └── DQ_VrepRobot_py.cpp ├── robot_control ├── DQ_ClassicQPController_py.cpp ├── DQ_KinematicConstrainedController_py.cpp ├── DQ_KinematicController_py.cpp ├── DQ_NumericalFilteredPseudoInverseController_py.cpp ├── DQ_PseudoinverseController_py.cpp └── DQ_QuadraticProgrammingController_py.cpp ├── robot_modeling ├── DQ_CooperativeDualTaskSpace_py.cpp ├── DQ_DifferentialDriveRobot_py.cpp ├── DQ_HolonomicBase_py.cpp ├── DQ_Kinematics_py.cpp ├── DQ_MobileBase_py.cpp ├── DQ_SerialManipulatorDH_py.cpp ├── DQ_SerialManipulatorDenso_py.cpp ├── DQ_SerialManipulatorMDH_py.cpp ├── DQ_SerialManipulator_py.cpp ├── DQ_SerialWholeBody_py.cpp └── DQ_WholeBody_py.cpp ├── solvers └── DQ_QuadraticProgrammingSolver_py.cpp └── utils ├── DQ_Geometry_py.cpp ├── DQ_LinearAlgebra_py.cpp └── DQ_Math_py.cpp /.github/workflows/python_package.yml: -------------------------------------------------------------------------------- 1 | name: Python Package 2 | 3 | on: [push,workflow_dispatch] 4 | 5 | jobs: 6 | build: 7 | 8 | permissions: 9 | id-token: write 10 | runs-on: ${{ matrix.os }} 11 | continue-on-error: ${{ matrix.experimental }} 12 | strategy: 13 | fail-fast: false 14 | matrix: 15 | python-version: ['3.10', '3.11', '3.12', '3.13'] 16 | os: ['ubuntu-22.04-arm', 'ubuntu-22.04', 'macos-latest', 'windows-latest'] 17 | experimental: [false] 18 | # exclude: 19 | # excludes python-versions not available in specific OSs. Usually needed when new versions are released. 20 | # - os: linux-arm64 21 | # python-version: '3.13' 22 | 23 | steps: 24 | - uses: actions/checkout@v2 25 | - name: Customize git 26 | run: | 27 | git submodule update --init --recursive 28 | git fetch --prune --unshallow 29 | - name: Set up Python ${{ matrix.python-version }} (Github Hosted VMs) 30 | uses: actions/setup-python@v2 31 | with: 32 | python-version: ${{ matrix.python-version }} 33 | - name: Install compilation dependencies [Ubuntu/C++] 34 | if: startsWith (matrix.os, 'ubuntu-22.04') 35 | run: | 36 | sudo apt install libeigen3-dev 37 | - name: Install compilation dependencies [MacOS/C++] 38 | if: matrix.os == 'macos-latest' 39 | run: | 40 | brew install eigen 41 | - name: Install compilation dependencies [Windows/C++] 42 | if: matrix.os == 'windows-latest' 43 | run: | 44 | pwd 45 | echo "Setting up vcpkg..." 46 | cd C:\vcpkg 47 | .\bootstrap-vcpkg.bat 48 | vcpkg integrate install 49 | echo "Installing dependencies..." 50 | .\vcpkg install eigen3:x64-windows 51 | echo "Adding symlink to vcpkg..." 52 | cmd /c mklink /d c:\Tools\vcpkg c:\vcpkg 53 | cd ~ 54 | - name: Install compilation dependencies [Python] 55 | run: | 56 | python -m pip install --upgrade pip 57 | pip install setuptools wheel setuptools-git-versioning 58 | - name: Compile 59 | run: | 60 | python setup.py bdist_wheel 61 | - name: Remove build folder for (not Windows) 62 | if: matrix.os != 'windows-latest' 63 | run: | 64 | rm -rf build 65 | - name: Remove build folder (Windows only) 66 | if: matrix.os == 'windows-latest' 67 | run: | 68 | rm -r -fo build 69 | - name: Rename wheel (MacOS only) 70 | if: matrix.os == 'macos-latest' 71 | run: | 72 | brew install rename 73 | cd dist 74 | sw_vers 75 | rename 's/11_0/10_14/' * 76 | rename 's/12_0/10_15/' * 77 | rename 's/universal2/arm64/' * 78 | cd .. 79 | - name: Install (not Windows) 80 | if: matrix.os != 'windows-latest' 81 | run: | 82 | python tests/print_platform_info.py 83 | python -m pip install dist/*.whl 84 | - name: Install (Windows) 85 | if: matrix.os == 'windows-latest' 86 | run: | 87 | python tests/print_platform_info.py 88 | python -m pip install @(join-path "dist" (get-childitem -path dist -name *.whl)) 89 | - name: Install Test Pre-requisites (quadprog always manual) 90 | run: | 91 | cd tests 92 | python -m pip install scipy 93 | git clone https://github.com/quadprog/quadprog.git 94 | cd quadprog 95 | pip install -r requirements_develop.txt && python -m cython quadprog/quadprog.pyx 96 | cd .. 97 | python -m pip install ./quadprog 98 | cd .. 99 | - name: Test 100 | run: | 101 | cd tests 102 | python DQ_test.py 103 | python DQ_Kinematics_test.py 104 | python DQ_SerialManipulatorMDH_test.py 105 | python cpp_issues.py 106 | python python_issues.py 107 | python DQ_Kinematics_pose_jacobian_derivative_tests.py 108 | python DQ_Kinematics_static_jacobian_derivative_tests.py 109 | cd .. 110 | - name: Rename wheel (Ubuntu Only) 111 | if: startsWith (matrix.os, 'ubuntu-22.04') 112 | run: | 113 | sudo apt install rename 114 | cd dist 115 | rename 's/linux/manylinux2014/' * 116 | cd .. 117 | - name: Publish to PyPi 118 | env: 119 | TWINE_USERNAME: '__token__' 120 | TWINE_PASSWORD: ${{ secrets.PYPI_TOKEN }} 121 | if: github.ref == 'refs/heads/master' 122 | run: | 123 | python -m pip install twine 124 | twine upload dist/* 125 | -------------------------------------------------------------------------------- /.gitmodules: -------------------------------------------------------------------------------- 1 | [submodule "pybind11"] 2 | path = pybind11 3 | url = https://github.com/pybind/pybind11 4 | [submodule "cpp"] 5 | path = cpp 6 | url = https://github.com/dqrobotics/cpp 7 | branch = master 8 | [submodule "interfaces/cpp-interface-json11"] 9 | path = interfaces/cpp-interface-json11 10 | url = https://github.com/dqrobotics/cpp-interface-json11.git 11 | branch = master 12 | [submodule "tests"] 13 | path = tests 14 | url = https://github.com/dqrobotics/python-tests.git 15 | branch = master 16 | -------------------------------------------------------------------------------- /.local_build_and_test.sh: -------------------------------------------------------------------------------- 1 | #!/bin/bash 2 | set -e 3 | sed -i'' -e 's/-j2/-j8/g' setup.py 4 | source venv/bin/activate 5 | python3 -m pip install wheel scipy quadprog 6 | cd .. 7 | python -m pip uninstall dqrobotics -y 8 | python -m pip install ./python 9 | cd python/tests 10 | python DQ_test.py 11 | python DQ_Kinematics_test.py 12 | python cpp_issues.py 13 | python python_issues.py 14 | cd .. 15 | sed -i'' -e 's/-j8/-j2/g' setup.py 16 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.11...4.0) 2 | 3 | if(WIN32) 4 | set(CMAKE_TOOLCHAIN_FILE C:/vcpkg/scripts/buildsystems/vcpkg.cmake) 5 | endif() 6 | 7 | project(dqroboticspython) 8 | 9 | set(CMAKE_CXX_STANDARD 17) 10 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 11 | 12 | include_directories( 13 | interfaces/cpp-interface-json11/include/ 14 | interfaces/cpp-interface-json11/dropbox/json11/ 15 | ) 16 | 17 | add_subdirectory(pybind11) 18 | 19 | pybind11_add_module(_dqrobotics 20 | 21 | src/dqrobotics_module.cpp 22 | 23 | src/DQ_py.cpp 24 | 25 | #Utils 26 | src/utils/DQ_LinearAlgebra_py.cpp 27 | src/utils/DQ_Geometry_py.cpp 28 | src/utils/DQ_Math_py.cpp 29 | 30 | #robot_modeling 31 | src/robot_modeling/DQ_CooperativeDualTaskSpace_py.cpp 32 | src/robot_modeling/DQ_Kinematics_py.cpp 33 | src/robot_modeling/DQ_SerialManipulator_py.cpp 34 | src/robot_modeling/DQ_SerialManipulatorDH_py.cpp 35 | src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp 36 | src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp 37 | src/robot_modeling/DQ_MobileBase_py.cpp 38 | src/robot_modeling/DQ_HolonomicBase_py.cpp 39 | src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp 40 | src/robot_modeling/DQ_WholeBody_py.cpp 41 | src/robot_modeling/DQ_SerialWholeBody_py.cpp 42 | 43 | #robot_control 44 | src/robot_control/DQ_KinematicController_py.cpp 45 | src/robot_control/DQ_PseudoinverseController_py.cpp 46 | src/robot_control/DQ_NumericalFilteredPseudoInverseController_py.cpp 47 | src/robot_control/DQ_KinematicConstrainedController_py.cpp 48 | src/robot_control/DQ_QuadraticProgrammingController_py.cpp 49 | src/robot_control/DQ_ClassicQPController_py.cpp 50 | 51 | #solvers 52 | #DQ_QuadraticProgrammingSolver.h 53 | src/solvers/DQ_QuadraticProgrammingSolver_py.cpp 54 | 55 | #interfaces/json11 56 | interfaces/cpp-interface-json11/src/dqrobotics/interfaces/json11/DQ_JsonReader.cpp 57 | src/interfaces/json11/DQ_JsonReader_py.cpp 58 | interfaces/cpp-interface-json11/dropbox/json11/json11.cpp 59 | 60 | ) 61 | 62 | # CMAKE was not passing BUILD_SHARED_LIBS nicely to the add_subdirectory. 63 | # https://thatonegamedev.com/cpp/cmake/how-to-manage-dependencies-with-cmake/ 64 | set(BUILD_SHARED_LIBS FALSE CACHE BOOL "x" FORCE) 65 | add_subdirectory(cpp) 66 | 67 | target_link_libraries(_dqrobotics PRIVATE dqrobotics) -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | GNU LESSER GENERAL PUBLIC LICENSE 2 | Version 3, 29 June 2007 3 | 4 | Copyright (C) 2007 Free Software Foundation, Inc. 5 | Everyone is permitted to copy and distribute verbatim copies 6 | of this license document, but changing it is not allowed. 7 | 8 | 9 | This version of the GNU Lesser General Public License incorporates 10 | the terms and conditions of version 3 of the GNU General Public 11 | License, supplemented by the additional permissions listed below. 12 | 13 | 0. Additional Definitions. 14 | 15 | As used herein, "this License" refers to version 3 of the GNU Lesser 16 | General Public License, and the "GNU GPL" refers to version 3 of the GNU 17 | General Public License. 18 | 19 | "The Library" refers to a covered work governed by this License, 20 | other than an Application or a Combined Work as defined below. 21 | 22 | An "Application" is any work that makes use of an interface provided 23 | by the Library, but which is not otherwise based on the Library. 24 | Defining a subclass of a class defined by the Library is deemed a mode 25 | of using an interface provided by the Library. 26 | 27 | A "Combined Work" is a work produced by combining or linking an 28 | Application with the Library. The particular version of the Library 29 | with which the Combined Work was made is also called the "Linked 30 | Version". 31 | 32 | The "Minimal Corresponding Source" for a Combined Work means the 33 | Corresponding Source for the Combined Work, excluding any source code 34 | for portions of the Combined Work that, considered in isolation, are 35 | based on the Application, and not on the Linked Version. 36 | 37 | The "Corresponding Application Code" for a Combined Work means the 38 | object code and/or source code for the Application, including any data 39 | and utility programs needed for reproducing the Combined Work from the 40 | Application, but excluding the System Libraries of the Combined Work. 41 | 42 | 1. Exception to Section 3 of the GNU GPL. 43 | 44 | You may convey a covered work under sections 3 and 4 of this License 45 | without being bound by section 3 of the GNU GPL. 46 | 47 | 2. Conveying Modified Versions. 48 | 49 | If you modify a copy of the Library, and, in your modifications, a 50 | facility refers to a function or data to be supplied by an Application 51 | that uses the facility (other than as an argument passed when the 52 | facility is invoked), then you may convey a copy of the modified 53 | version: 54 | 55 | a) under this License, provided that you make a good faith effort to 56 | ensure that, in the event an Application does not supply the 57 | function or data, the facility still operates, and performs 58 | whatever part of its purpose remains meaningful, or 59 | 60 | b) under the GNU GPL, with none of the additional permissions of 61 | this License applicable to that copy. 62 | 63 | 3. Object Code Incorporating Material from Library Header Files. 64 | 65 | The object code form of an Application may incorporate material from 66 | a header file that is part of the Library. You may convey such object 67 | code under terms of your choice, provided that, if the incorporated 68 | material is not limited to numerical parameters, data structure 69 | layouts and accessors, or small macros, inline functions and templates 70 | (ten or fewer lines in length), you do both of the following: 71 | 72 | a) Give prominent notice with each copy of the object code that the 73 | Library is used in it and that the Library and its use are 74 | covered by this License. 75 | 76 | b) Accompany the object code with a copy of the GNU GPL and this license 77 | document. 78 | 79 | 4. Combined Works. 80 | 81 | You may convey a Combined Work under terms of your choice that, 82 | taken together, effectively do not restrict modification of the 83 | portions of the Library contained in the Combined Work and reverse 84 | engineering for debugging such modifications, if you also do each of 85 | the following: 86 | 87 | a) Give prominent notice with each copy of the Combined Work that 88 | the Library is used in it and that the Library and its use are 89 | covered by this License. 90 | 91 | b) Accompany the Combined Work with a copy of the GNU GPL and this license 92 | document. 93 | 94 | c) For a Combined Work that displays copyright notices during 95 | execution, include the copyright notice for the Library among 96 | these notices, as well as a reference directing the user to the 97 | copies of the GNU GPL and this license document. 98 | 99 | d) Do one of the following: 100 | 101 | 0) Convey the Minimal Corresponding Source under the terms of this 102 | License, and the Corresponding Application Code in a form 103 | suitable for, and under terms that permit, the user to 104 | recombine or relink the Application with a modified version of 105 | the Linked Version to produce a modified Combined Work, in the 106 | manner specified by section 6 of the GNU GPL for conveying 107 | Corresponding Source. 108 | 109 | 1) Use a suitable shared library mechanism for linking with the 110 | Library. A suitable mechanism is one that (a) uses at run time 111 | a copy of the Library already present on the user's computer 112 | system, and (b) will operate properly with a modified version 113 | of the Library that is interface-compatible with the Linked 114 | Version. 115 | 116 | e) Provide Installation Information, but only if you would otherwise 117 | be required to provide such information under section 6 of the 118 | GNU GPL, and only to the extent that such information is 119 | necessary to install and execute a modified version of the 120 | Combined Work produced by recombining or relinking the 121 | Application with a modified version of the Linked Version. (If 122 | you use option 4d0, the Installation Information must accompany 123 | the Minimal Corresponding Source and Corresponding Application 124 | Code. If you use option 4d1, you must provide the Installation 125 | Information in the manner specified by section 6 of the GNU GPL 126 | for conveying Corresponding Source.) 127 | 128 | 5. Combined Libraries. 129 | 130 | You may place library facilities that are a work based on the 131 | Library side by side in a single library together with other library 132 | facilities that are not Applications and are not covered by this 133 | License, and convey such a combined library under terms of your 134 | choice, if you do both of the following: 135 | 136 | a) Accompany the combined library with a copy of the same work based 137 | on the Library, uncombined with any other library facilities, 138 | conveyed under the terms of this License. 139 | 140 | b) Give prominent notice with the combined library that part of it 141 | is a work based on the Library, and explaining where to find the 142 | accompanying uncombined form of the same work. 143 | 144 | 6. Revised Versions of the GNU Lesser General Public License. 145 | 146 | The Free Software Foundation may publish revised and/or new versions 147 | of the GNU Lesser General Public License from time to time. Such new 148 | versions will be similar in spirit to the present version, but may 149 | differ in detail to address new problems or concerns. 150 | 151 | Each version is given a distinguishing version number. If the 152 | Library as you received it specifies that a certain numbered version 153 | of the GNU Lesser General Public License "or any later version" 154 | applies to it, you have the option of following the terms and 155 | conditions either of that published version or of any later version 156 | published by the Free Software Foundation. If the Library as you 157 | received it does not specify a version number of the GNU Lesser 158 | General Public License, you may choose any version of the GNU Lesser 159 | General Public License ever published by the Free Software Foundation. 160 | 161 | If the Library as you received it specifies that a proxy can decide 162 | whether future versions of the GNU Lesser General Public License shall 163 | apply, that proxy's public statement of acceptance of any version is 164 | permanent authorization for you to choose that version for the 165 | Library. 166 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dqrobotics-python [![Python package](https://github.com/dqrobotics/python/actions/workflows/python_package.yml/badge.svg)](https://github.com/dqrobotics/python/actions/workflows/python_package.yml) [![PyPI version](https://badge.fury.io/py/dqrobotics.svg)](https://badge.fury.io/py/dqrobotics) 2 | The DQ Robotics library in Python 3 | 4 | Refer to the [docs](https://dqroboticsgithubio.readthedocs.io/en/latest/installation/python.html) 5 | 6 | 7 | ## Dev Stuff 8 | 9 | With the initial settings described below for each system, the `actions_runner` will pick up the correct version of Python and run the CI accordingly. 10 | 11 | ### Setting up the self-hosted CI environment for Apple Silicon 12 | 13 | _From [363627c](https://github.com/dqrobotics/python/commit/363627cdbd3d9207cd22a9ad618f57af29f26bd0), a self-hosted machine for `arm64` MacOS is no longer needed. The Github-hosted image is currently `arm64` by default._ 14 | 15 | 1. Install `brew` 16 | 2. Install the Python versions currently supported by DQRobotics e.g. 17 | ``` 18 | brew install python@3.XX 19 | ``` 20 | 3. Add an alias for each Python version in your `.zshrc`, e.g. `alias python3.XX='/opt/homebrew/bin/python3.XX'` 21 | 22 | 23 | ### Setting up the self-hosted CI environment for Ubuntu arm64 24 | 25 | _From [1198a5b](https://github.com/dqrobotics/python/commit/1198a5bf703d0e5acbed426761804b642eb7dc9c), a self-hosted machine for Ubuntu `arm64` is no longer needed. Tags such as `ubuntu-22.04-arm` enable the `arm64` version of Ubuntu_ 26 | 27 | 0. Make sure you have the basic compilation enviroment 28 | ``` 29 | sudo apt update && sudo apt upgrade -y 30 | sudo apt install git g++ cmake 31 | ``` 32 | 33 | 1. Add the deadsnakes PPA 34 | ``` 35 | sudo add-apt-repository ppa:deadsnakes/ppa 36 | ``` 37 | 2. Run 38 | ``` 39 | sudo apt update 40 | ``` 41 | 3. Install, for each Python version, the following 42 | ``` 43 | sudo apt install python3.XX-dev python3.XX-venv python3.XX-distutils 44 | ``` 45 | -------------------------------------------------------------------------------- /dqrobotics/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics import * 27 | 28 | -------------------------------------------------------------------------------- /dqrobotics/interfaces/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._interfaces import * 27 | -------------------------------------------------------------------------------- /dqrobotics/interfaces/coppeliasim/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._interfaces._coppeliasim import * 27 | -------------------------------------------------------------------------------- /dqrobotics/interfaces/json11/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._interfaces._json11 import * 27 | -------------------------------------------------------------------------------- /dqrobotics/interfaces/vrep/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._interfaces._vrep import * 27 | -------------------------------------------------------------------------------- /dqrobotics/interfaces/vrep/robots/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._interfaces._vrep._robots import * 27 | -------------------------------------------------------------------------------- /dqrobotics/robot_control/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._robot_control import * 27 | -------------------------------------------------------------------------------- /dqrobotics/robot_modeling/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._robot_modeling import * 27 | -------------------------------------------------------------------------------- /dqrobotics/robots/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._robots import * 27 | -------------------------------------------------------------------------------- /dqrobotics/solvers/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._solvers import * 27 | 28 | try: 29 | from dqrobotics.solvers._dq_quadprog_solver import DQ_QuadprogSolver 30 | except: 31 | pass 32 | 33 | try: 34 | from dqrobotics.solvers._dq_cplex_solver import DQ_CPLEXSolver 35 | except: 36 | pass 37 | -------------------------------------------------------------------------------- /dqrobotics/solvers/_dq_cplex_solver.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._solvers import DQ_QuadraticProgrammingSolver 27 | import numpy as np 28 | import cplex 29 | 30 | # https://github.com/dqrobotics/python/issues/24 31 | class DQ_CPLEXSolver(DQ_QuadraticProgrammingSolver): 32 | def __init__(self): 33 | DQ_QuadraticProgrammingSolver.__init__(self) 34 | # Make and set a solver instance 35 | self.P = cplex.Cplex() 36 | self.P.objective.set_sense(self.P.objective.sense.minimize) 37 | self.P.set_problem_type(self.P.problem_type.QP) 38 | self.P.set_results_stream(results_file=None) 39 | 40 | def solve_quadratic_program(self, H, f, A, b, Aeq, beq): 41 | problem_size = H.shape[0] 42 | inequality_constraint_size = b.shape[0] 43 | equality_constraint_size = beq.shape[0] 44 | 45 | # Define the number of variables by naming them 46 | q_dot = ["q{}".format(i) for i in range(problem_size)] 47 | if self.P.variables.get_num() != problem_size: 48 | self.P.variables.delete() 49 | self.P.variables.add(names=q_dot) 50 | 51 | # Decide the range of variables 52 | for i in range(problem_size): 53 | self.P.variables.set_lower_bounds(i, -1 * cplex.infinity) 54 | self.P.variables.set_upper_bounds(i, cplex.infinity) 55 | 56 | # Set f'x 57 | f = f.tolist() 58 | for i in range(problem_size): 59 | self.P.objective.set_linear(i, f[i][0]) 60 | 61 | # Set x^T H x 62 | H = H.tolist() 63 | for i in range(problem_size): 64 | for j in range(i, problem_size): 65 | self.P.objective.set_quadratic_coefficients(i, j, H[i][j]) 66 | 67 | # Set linear inequality constraints and equality constraints 68 | # Define the number of total constraints by naming them 69 | c = ["c{}".format(i) for i in range(inequality_constraint_size + equality_constraint_size)] 70 | if self.P.linear_constraints.get_num() != inequality_constraint_size + equality_constraint_size: 71 | self.P.linear_constraints.delete() 72 | self.P.linear_constraints.add(names=c) 73 | 74 | # Inequalities 75 | A = A.tolist() 76 | b = b.tolist() 77 | for i in range(inequality_constraint_size): 78 | self.P.linear_constraints.set_linear_components(c[i], [q_dot, A[i]]) 79 | self.P.linear_constraints.set_senses(c[i], 'L') 80 | self.P.linear_constraints.set_rhs(c[i], b[i][0]) 81 | 82 | # Equalities 83 | Aeq = Aeq.tolist() 84 | beq = beq.tolist() 85 | for i in range(equality_constraint_size): 86 | self.P.linear_constraints.set_linear_components(c[inequality_constraint_size + i], [q_dot, Aeq[i]]) 87 | self.P.linear_constraints.set_senses(c[inequality_constraint_size + i], 'E') 88 | self.P.linear_constraints.set_rhs(c[inequality_constraint_size + i], beq[i][0]) 89 | 90 | # Solve the problem 91 | self.P.solve() 92 | delta_thetas = np.array(self.P.solution.get_values()) 93 | 94 | return delta_thetas 95 | -------------------------------------------------------------------------------- /dqrobotics/solvers/_dq_quadprog_solver.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._solvers import DQ_QuadraticProgrammingSolver 27 | import numpy as np 28 | import quadprog 29 | 30 | class DQ_QuadprogSolver(DQ_QuadraticProgrammingSolver): 31 | def __init__(self): 32 | DQ_QuadraticProgrammingSolver.__init__(self) 33 | self.equality_constraints_tolerance = 0 # default of np.finfo(np.float64).eps is already included in the solver 34 | pass 35 | 36 | def set_equality_constraints_tolerance(self, tolerance): 37 | """ 38 | Set allowed tolerance for the equality constraints 39 | :param tolerance: Tolerance allowed for equality constraints 40 | """ 41 | self.equality_constraints_tolerance = tolerance 42 | 43 | def get_equality_constraints_tolerance(self): 44 | """ 45 | Get allowed tolerance for the equality constraints 46 | :return: Current tolerance 47 | """ 48 | return self.equality_constraints_tolerance 49 | 50 | def solve_quadratic_program(self, H, f, A, b, Aeq, beq): 51 | """ 52 | Solves the following quadratic program 53 | min(x) 0.5*x'Hx + f'x 54 | s.t. Ax <= b 55 | Aeqx = beq. 56 | Method signature is compatible with MATLAB's 'quadprog'. 57 | :param H: the n x n matrix of the quadratic coeficitients of the decision variables. 58 | :param f: the n x 1 vector of the linear coeficients of the decision variables. 59 | :param A: the m x n matrix of inequality constraints. 60 | :param b: the m x 1 value for the inequality constraints. 61 | :param Aeq: the m x n matrix of equality constraints. 62 | :param beq: the m x 1 value for the inequality constraints. 63 | :return: the optimal x 64 | """ 65 | A_internal = A 66 | b_internal = b 67 | if Aeq is not None and beq is not None: 68 | if Aeq.shape == (0, 0) or beq.shape == 0: 69 | pass 70 | else: 71 | A_internal = np.vstack([A, Aeq, -Aeq]) 72 | beq = beq.reshape(-1) 73 | b_internal = np.concatenate([b.reshape(-1), beq + self.equality_constraints_tolerance, 74 | -beq + self.equality_constraints_tolerance]) 75 | if A_internal.shape == (0, 0) or b_internal.shape == 0: 76 | # Calls from DQRobotics CPP will trigger this condition 77 | A_internal = np.zeros((1, H.shape[0])) 78 | b_internal = np.zeros(1) 79 | 80 | (x, f, xu, iterations, lagrangian, iact) = quadprog.solve_qp(G=H, 81 | a=-f, 82 | C=-np.transpose(A_internal), 83 | b=-b_internal) 84 | return x 85 | -------------------------------------------------------------------------------- /dqrobotics/utils/DQ_LinearAlgebra/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._utils._DQ_LinearAlgebra import * 27 | -------------------------------------------------------------------------------- /dqrobotics/utils/DQ_Math/__init__.py: -------------------------------------------------------------------------------- 1 | from dqrobotics._dqrobotics._utils._DQ_Math import * 2 | -------------------------------------------------------------------------------- /dqrobotics/utils/__init__.py: -------------------------------------------------------------------------------- 1 | """ 2 | # Copyright (c) 2019-2022 DQ Robotics Developers 3 | # 4 | # This file is part of DQ Robotics. 5 | # 6 | # DQ Robotics is free software: you can redistribute it and/or modify 7 | # it under the terms of the GNU Lesser General Public License as published by 8 | # the Free Software Foundation, either version 3 of the License, or 9 | # (at your option) any later version. 10 | # 11 | # DQ Robotics is distributed in the hope that it will be useful, 12 | # but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | # MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | # GNU Lesser General Public License for more details. 15 | # 16 | # You should have received a copy of the GNU Lesser General Public License 17 | # along with DQ Robotics. If not, see . 18 | # 19 | # ################################################################ 20 | # 21 | # Contributors: 22 | # - Murilo M. Marinho, email: murilo@g.u-tokyo.ac.jp 23 | # 24 | # ################################################################ 25 | """ 26 | from dqrobotics._dqrobotics._utils import * 27 | -------------------------------------------------------------------------------- /pyproject.toml: -------------------------------------------------------------------------------- 1 | [build-system] 2 | requires = [ 3 | "setuptools>=42", 4 | "wheel", 5 | "ninja", 6 | "cmake>=3.12", 7 | "setuptools-git-versioning>=2.0,<3" 8 | ] 9 | build-backend = "setuptools.build_meta" 10 | 11 | [project] 12 | name = 'dqrobotics' 13 | authors = [ 14 | {name = "Murilo M. Marinho", email = "murilomarinho@ieee.org"} 15 | ] 16 | maintainers = [ 17 | {name = "Murilo M. Marinho", email = "murilomarinho@ieee.org"} 18 | ] 19 | dynamic = ["version", "classifiers"] 20 | dependencies=[ 21 | 'numpy', 22 | ] 23 | description='dqrobotics Python' 24 | readme = "README.md" 25 | requires-python = ">= 3.9" 26 | 27 | [project.urls] 28 | Homepage = "https://dqrobotics.github.io" 29 | Documentation = "https://dqroboticsgithubio.readthedocs.io/en/latest/installation/python.html" 30 | Repository = "https://github.com/dqrobotics/python" 31 | Issues = "https://github.com/dqrobotics/python/issues" 32 | 33 | # https://pypi.org/project/setuptools-git-versioning/ 34 | [tool.setuptools-git-versioning] 35 | enabled = true 36 | # https://stackoverflow.com/questions/73605607/how-to-use-setuptools-scm 37 | dev_template = "{tag}.a{ccount}" 38 | dirty_template = "{tag}.a{ccount}" -------------------------------------------------------------------------------- /setup.py: -------------------------------------------------------------------------------- 1 | import os 2 | import re 3 | import sys 4 | import platform 5 | import subprocess 6 | 7 | from setuptools import setup, Extension 8 | from setuptools.command.build_ext import build_ext 9 | from distutils.version import LooseVersion 10 | 11 | 12 | 13 | class CMakeExtension(Extension): 14 | def __init__(self, name, sourcedir=''): 15 | Extension.__init__(self, name, sources=[]) 16 | self.sourcedir = os.path.abspath(sourcedir) 17 | 18 | 19 | class CMakeBuild(build_ext): 20 | def run(self): 21 | try: 22 | out = subprocess.check_output(['cmake', '--version']) 23 | except OSError: 24 | raise RuntimeError("CMake must be installed to build the following extensions: " + 25 | ", ".join(e.name for e in self.extensions)) 26 | 27 | if platform.system() == "Windows": 28 | cmake_version = LooseVersion(re.search(r'version\s*([\d.]+)', out.decode()).group(1)) 29 | if cmake_version < '3.1.0': 30 | raise RuntimeError("CMake >= 3.1.0 is required on Windows") 31 | 32 | for ext in self.extensions: 33 | self.build_extension(ext) 34 | 35 | def build_extension(self, ext): 36 | extdir = os.path.abspath(os.path.dirname(self.get_ext_fullpath(ext.name))) 37 | cmake_args = ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY=' + extdir, 38 | '-DPYTHON_EXECUTABLE=' + sys.executable] 39 | 40 | cfg = 'Debug' if self.debug else 'Release' 41 | build_args = ['--config', cfg] 42 | 43 | # Without this, the results of some calculations will differ from the MATLAB version and cause the tests to 44 | # fail. 45 | if platform.machine() == "aarch64" or platform.machine() == "arm64": 46 | cmake_args += ['-DCMAKE_CXX_FLAGS="-ffp-contract=off"'] 47 | # https://stackoverflow.com/questions/64036879/differing-floating-point-calculation-results-between-x86-64-and-armv8-2-a 48 | 49 | if platform.system() == "Windows": 50 | cmake_args += ['-DCMAKE_LIBRARY_OUTPUT_DIRECTORY_{}={}'.format(cfg.upper(), extdir)] 51 | cmake_args += ['-DCMAKE_TOOLCHAIN_FILE=C:/Tools/vcpkg/scripts/buildsystems/vcpkg.cmake'] 52 | if sys.maxsize > 2 ** 32: 53 | cmake_args += ['-A', 'x64'] 54 | build_args += ['--', '/m'] 55 | else: 56 | cmake_args += ['-DCMAKE_BUILD_TYPE=' + cfg] 57 | build_args += ['--', '-j2'] 58 | 59 | env = os.environ.copy() 60 | env['CXXFLAGS'] = '{} -DVERSION_INFO=\\"{}\\"'.format(env.get('CXXFLAGS', ''), 61 | self.distribution.get_version()) 62 | if not os.path.exists(self.build_temp): 63 | os.makedirs(self.build_temp) 64 | subprocess.check_call(['cmake', ext.sourcedir] + cmake_args, cwd=self.build_temp, env=env) 65 | subprocess.check_call(['cmake', '--build', '.'] + build_args, cwd=self.build_temp) 66 | 67 | 68 | setup( 69 | ext_modules=[CMakeExtension('dqrobotics._dqrobotics')], 70 | cmdclass=dict(build_ext=CMakeBuild), 71 | zip_safe=False, 72 | packages=['dqrobotics', 73 | 'dqrobotics.robots', 74 | 'dqrobotics.robot_modeling', 75 | 'dqrobotics.utils', 76 | 'dqrobotics.utils.DQ_Math', 77 | 'dqrobotics.utils.DQ_LinearAlgebra', 78 | 'dqrobotics.interfaces', 79 | 'dqrobotics.interfaces.json11', 80 | 'dqrobotics.robot_control', 81 | 'dqrobotics.solvers'], 82 | classifiers=[ 83 | "Programming Language :: Python :: 3.10", 84 | "Programming Language :: Python :: 3.11", 85 | "Programming Language :: Python :: 3.12", 86 | "Programming Language :: Python :: 3.13", 87 | "Programming Language :: C++", 88 | "Development Status :: 5 - Production/Stable", 89 | "Operating System :: POSIX :: Linux", 90 | "License :: OSI Approved :: GNU Lesser General Public License v3 or later (LGPLv3+)", 91 | ], 92 | ) 93 | -------------------------------------------------------------------------------- /src/DQ_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "dqrobotics_module.h" 24 | 25 | void init_DQ_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ 29 | * **************************************************/ 30 | py::class_ dq(m, "DQ"); 31 | dq.def(py::init()); 32 | dq.def(py::init()); 33 | ///Members 34 | dq.def_readwrite("q", &DQ::q); 35 | ///Static Members 36 | dq.def_readonly_static("i",&DQ::i); 37 | dq.def_readonly_static("j",&DQ::j); 38 | dq.def_readonly_static("k",&DQ::k); 39 | dq.def_readonly_static("E",&DQ::E); 40 | ///Methods 41 | dq.def("P" ,&DQ::P, "Retrieves the primary part of a DQ."); 42 | dq.def("D" ,&DQ::D, "Retrieves the dual part of a DQ."); 43 | dq.def("Re" ,&DQ::Re, "Retrieves the real part of a DQ."); 44 | dq.def("Im" ,&DQ::Im, "Retrieves the imaginary part of a DQ."); 45 | dq.def("conj" ,&DQ::conj, "Retrieves the conjugate of a DQ."); 46 | dq.def("norm" ,&DQ::norm, "Retrieves the norm of a DQ."); 47 | dq.def("inv" ,&DQ::inv, "Retrieves the inverse of a DQ."); 48 | dq.def("translation" ,&DQ::translation, "Retrieves the translation represented by a unit DQ."); 49 | dq.def("rotation" ,&DQ::rotation, "Retrieves the rotation represented by a unit DQ."); 50 | dq.def("rotation_axis" ,&DQ::rotation_axis, "Retrieves the rotation axis represented by a unit DQ."); 51 | dq.def("rotation_angle" ,&DQ::rotation_angle, "Retrieves the rotation angle represented by a unit DQ."); 52 | dq.def("log" ,&DQ::log, "Retrieves the logarithm of a DQ."); 53 | dq.def("exp" ,&DQ::exp, "Retrieves the exp of a DQ."); 54 | dq.def("pow" ,&DQ::pow, "Retrieves the pow of a DQ."); 55 | dq.def("tplus" ,&DQ::tplus, "Retrieves the tplus operators for a DQ."); 56 | dq.def("pinv" ,&DQ::pinv , "Retrieves the pinv of a DQ."); 57 | dq.def("hamiplus4" ,&DQ::hamiplus4, "Retrieves the H+ operator for the primary part of a DQ."); 58 | dq.def("haminus4" ,&DQ::haminus4, "Retrieves the H- operator for the primary part of a DQ."); 59 | dq.def("hamiplus8" ,&DQ::hamiplus8, "Retrieves the H+ operator for a DQ."); 60 | dq.def("haminus8" ,&DQ::haminus8, "Retrieves the H- operator for a DQ."); 61 | dq.def("vec3" ,&DQ::vec3, "Retrieves the pure part of the primary part of a DQ as a vector."); 62 | dq.def("vec4" ,&DQ::vec4, "Retrieves the primary part of a DQ as a vector."); 63 | dq.def("vec6" ,&DQ::vec6, "Retrieves the pure part of a DQ as a vector."); 64 | dq.def("vec8" ,&DQ::vec8, "Retrieves the DQ as a vector."); 65 | dq.def("normalize" ,&DQ::normalize, "Returns a normalized DQ."); 66 | dq.def("__repr__" ,&DQ::to_string, "Used by python's print function."); 67 | dq.def("to_string" ,&DQ::to_string, "Returns the DQ as a string."); 68 | dq.def("generalized_jacobian",&DQ::generalized_jacobian, "Retrieves the generalized Jacobian of a DQ."); 69 | dq.def("sharp" ,&DQ::sharp, "Retrieves the sharp of a DQ"); 70 | dq.def("Ad" ,&DQ::Ad, "Retrieves the adjoint transformation of a DQ."); 71 | dq.def("Adsharp" ,&DQ::Adsharp, "Retrieves the adjoint sharp transformation of a DQ."); 72 | dq.def("Q4" ,&DQ::Q4, "Retrieves the partial derivative of a unit quaternion with respect to its logarithm."); 73 | dq.def("Q8" ,&DQ::Q8, "Retrieves the partial derivative of a unit DQ with respect to its logarithm."); 74 | 75 | ///Operators 76 | //Self 77 | dq.def(py::self + py::self); 78 | dq.def(py::self * py::self); 79 | dq.def(py::self - py::self); 80 | dq.def(py::self == py::self); 81 | dq.def(py::self != py::self); 82 | dq.def(- py::self); 83 | //Double 84 | dq.def(double() * py::self); 85 | dq.def(py::self * double()); 86 | dq.def(double() + py::self); 87 | dq.def(py::self + double()); 88 | dq.def(double() - py::self); 89 | dq.def(py::self - double()); 90 | dq.def(double() == py::self); 91 | dq.def(py::self == double()); 92 | dq.def(double() != py::self); 93 | dq.def(py::self != double()); 94 | 95 | ///Namespace Functions 96 | m.def("C8" ,&DQ_robotics::C8, "Returns the C8 matrix."); 97 | m.def("C4" ,&DQ_robotics::C4, "Returns the C4 matrix."); 98 | m.def("P" ,&DQ_robotics::P, "Retrieves the primary part of a DQ."); 99 | m.def("D" ,&DQ_robotics::D, "Retrieves the dual part of a DQ."); 100 | m.def("Re" ,&DQ_robotics::Re, "Retrieves the real part of a DQ."); 101 | m.def("Im" ,&DQ_robotics::Im, "Retrieves the imaginary part of a DQ."); 102 | m.def("conj" ,&DQ_robotics::conj, "Retrieves the conjugate of a DQ."); 103 | m.def("norm" ,&DQ_robotics::norm, "Retrieves the norm of a DQ."); 104 | m.def("inv" ,&DQ_robotics::inv, "Retrieves the inverse of a DQ."); 105 | m.def("translation" ,&DQ_robotics::translation, "Retrieves the translation represented by a unit DQ."); 106 | m.def("rotation" ,&DQ_robotics::rotation, "Retrieves the rotation represented by a unit DQ."); 107 | m.def("rotation_axis" ,&DQ_robotics::rotation_axis, "Retrieves the rotation axis represented by a unit DQ."); 108 | m.def("rotation_angle" ,&DQ_robotics::rotation_angle, "Retrieves the rotation angle represented by a unit DQ."); 109 | m.def("log" ,&DQ_robotics::log, "Retrieves the logarithm of a DQ."); 110 | m.def("exp" ,&DQ_robotics::exp, "Retrieves the exp of a DQ."); 111 | m.def("pow" ,&DQ_robotics::pow, "Retrieves the pow of a DQ."); 112 | m.def("tplus" ,&DQ_robotics::tplus, "Retrieves the tplus operators for a DQ."); 113 | m.def("pinv" ,(DQ (*) (const DQ&)) &DQ_robotics::pinv ,"Retrieves the pinv of a DQ."); 114 | m.def("dec_mult" ,&DQ_robotics::dec_mult, "Retrieves the dec mult of a DQ."); 115 | m.def("hamiplus4" ,&DQ_robotics::hamiplus4, "Retrieves the H+ operator for the primary part of a DQ."); 116 | m.def("haminus4" ,&DQ_robotics::haminus4, "Retrieves the H- operator for the primary part of a DQ."); 117 | m.def("hamiplus8" ,&DQ_robotics::hamiplus8, "Retrieves the H+ operator for a DQ."); 118 | m.def("haminus8" ,&DQ_robotics::haminus8, "Retrieves the H- operator for a DQ."); 119 | m.def("vec3" ,&DQ_robotics::vec3, "Retrieves the pure part of the primary part of a DQ as a vector."); 120 | m.def("vec4" ,&DQ_robotics::vec4, "Retrieves the primary part of a DQ as a vector."); 121 | m.def("vec6" ,&DQ_robotics::vec6, "Retrieves the pure part of a DQ as a vector."); 122 | m.def("vec8" ,&DQ_robotics::vec8, "Retrieves the DQ as a vector."); 123 | m.def("normalize" ,&DQ_robotics::normalize, "Returns a normalized DQ."); 124 | m.def("generalized_jacobian",&DQ_robotics::generalized_jacobian, "Retrieves the generalized Jacobian of a DQ."); 125 | m.def("sharp" ,&DQ_robotics::sharp, "Returns the sharp DQ"); 126 | m.def("crossmatrix4" ,&DQ_robotics::crossmatrix4, "Returns the crossmatrix4 operator."); 127 | m.def("Ad" ,&DQ_robotics::Ad, "Retrieves the adjoint transformation of a DQ."); 128 | m.def("Adsharp" ,&DQ_robotics::Adsharp, "Retrieves the adjoint sharp transformation of a DQ."); 129 | m.def("cross" ,&DQ_robotics::cross, "Returns the result of the cross product between two DQ."); 130 | m.def("dot" ,&DQ_robotics::dot, "Returns the result of the dot product between two DQ."); 131 | m.def("Q4" ,&DQ_robotics::Q4, "Retrieves the partial derivative of a unit quaternion with respect to its logarithm."); 132 | m.def("Q8" ,&DQ_robotics::Q8, "Retrieves the partial derivative of a unit DQ with respect to its logarithm."); 133 | 134 | m.def("is_unit" ,&DQ_robotics::is_unit, "Returns true if the DQ has unit norm, false otherwise."); 135 | m.def("is_pure" ,&DQ_robotics::is_pure, "Returns true if the DQ is pure, false otherwise."); 136 | m.def("is_real" ,&DQ_robotics::is_real, "Returns true if the DQ is a real dual number, false otherwise."); 137 | m.def("is_real_number" ,&DQ_robotics::is_real_number, "Returns true if the DQ is a real number, false otherwise."); 138 | m.def("is_quaternion" ,&DQ_robotics::is_quaternion, "Returns true if the DQ is a quaternion, false otherwise."); 139 | m.def("is_pure_quaternion" ,&DQ_robotics::is_pure_quaternion, "Returns true if the DQ is a pure quaternion, false otherwise."); 140 | m.def("is_line" ,&DQ_robotics::is_line, "Returns true if the DQ is a Plucker line, false otherwise."); 141 | m.def("is_plane" ,&DQ_robotics::is_plane, "Returns true if the DQ is a plane, false otherwise."); 142 | 143 | ///Namespace readonly 144 | m.attr("DQ_threshold") = DQ_threshold; 145 | m.attr("i_") = DQ_robotics::i_; 146 | m.attr("j_") = DQ_robotics::j_; 147 | m.attr("k_") = DQ_robotics::k_; 148 | m.attr("E_") = DQ_robotics::E_; 149 | } 150 | -------------------------------------------------------------------------------- /src/dqrobotics_module.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019-2023 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "dqrobotics_module.h" 24 | 25 | PYBIND11_MODULE(_dqrobotics, m) { 26 | 27 | //DQ Class 28 | init_DQ_py(m); 29 | 30 | /***************************************************** 31 | * Utils 32 | * **************************************************/ 33 | //dqrobotics/utils/ 34 | py::module utils_py = m.def_submodule("_utils","A submodule of dqrobotics"); 35 | 36 | //DQ_LinearAlgebra 37 | init_DQ_LinearAlgebra_py(utils_py); 38 | 39 | //DQ_Geometry 40 | init_DQ_Geometry_py(utils_py); 41 | 42 | //DQ_Math 43 | init_DQ_Math_py(utils_py); 44 | 45 | /***************************************************** 46 | * Robots Kinematic Models 47 | * **************************************************/ 48 | py::module robots_py = m.def_submodule("_robots", "A submodule of dqrobotics"); 49 | 50 | //#include 51 | py::class_ ax18manipulatorrobot_py(robots_py, "Ax18ManipulatorRobot"); 52 | ax18manipulatorrobot_py.def_static("kinematics",&Ax18ManipulatorRobot::kinematics,"Returns the kinematics of the Ax18ManipulatorRobot"); 53 | 54 | //#include 55 | py::class_ barrettwamarmrobot_py(robots_py, "BarrettWamArmRobot"); 56 | barrettwamarmrobot_py.def_static("kinematics",&BarrettWamArmRobot::kinematics,"Returns the kinematics of the BarrettWamArmRobot"); 57 | 58 | //#include 59 | py::class_ comausmartsixrobot_py(robots_py, "ComauSmartSixRobot"); 60 | comausmartsixrobot_py.def_static("kinematics",&ComauSmartSixRobot::kinematics,"Returns the kinematics of the ComauSmartSixRobot"); 61 | 62 | //#include 63 | py::class_ kukalw4robot_py(robots_py, "KukaLw4Robot"); 64 | kukalw4robot_py.def_static("kinematics",&KukaLw4Robot::kinematics,"Returns the kinematics of the KukaLw4Robot"); 65 | 66 | //#include 67 | py::class_ kukayoubotrobot_py(robots_py, "KukaYoubotRobot"); 68 | kukayoubotrobot_py.def_static("kinematics",&KukaYoubotRobot::kinematics,"Returns the kinematics of the KukaYoubotRobot"); 69 | 70 | //#include 71 | py::class_ frankaemikapandarobot_py(robots_py, "FrankaEmikaPandaRobot"); 72 | frankaemikapandarobot_py.def_static("kinematics",&FrankaEmikaPandaRobot::kinematics,"Returns the kinematics of the FrankaEmikaPandaRobot"); 73 | 74 | /***************************************************** 75 | * Robot Modeling 76 | * **************************************************/ 77 | py::module robot_modeling = m.def_submodule("_robot_modeling", "The robot_modeling submodule of dqrobotics"); 78 | 79 | //DQ_Kinematics 80 | init_DQ_Kinematics_py(robot_modeling); 81 | 82 | //DQ_SerialManipulator 83 | init_DQ_SerialManipulator_py(robot_modeling); 84 | 85 | //DQ_SerialManipulatorDH 86 | init_DQ_SerialManipulatorDH_py(robot_modeling); 87 | 88 | //DQ_SerialManipulatorMDH 89 | init_DQ_SerialManipulatorMDH_py(robot_modeling); 90 | 91 | //DQ_SerialManipulatorDenso 92 | init_DQ_SerialManipulatorDenso_py(robot_modeling); 93 | 94 | //DQ_CooperativeDualTaskSpace 95 | init_DQ_CooperativeDualTaskSpace_py(robot_modeling); 96 | 97 | //DQ_MobileBase 98 | init_DQ_MobileBase_py(robot_modeling); 99 | 100 | //DQ_HolonomicBase 101 | init_DQ_HolonomicBase_py(robot_modeling); 102 | 103 | //DQ_DifferentialDriveRobot 104 | init_DQ_DifferentialDriveRobot_py(robot_modeling); 105 | 106 | //DQ_WholeBody 107 | init_DQ_WholeBody_py(robot_modeling); 108 | 109 | //DQ_SerialWholeBody 110 | init_DQ_SerialWholeBody_py(robot_modeling); 111 | 112 | /***************************************************** 113 | * Solvers 114 | * **************************************************/ 115 | py::module solvers = m.def_submodule("_solvers", "The solvers submodule of dqrobotics"); 116 | 117 | //DQ_QuadraticProgrammingSolver 118 | init_DQ_QuadraticProgrammingSolver_py(solvers); 119 | 120 | /***************************************************** 121 | * Robot Control 122 | * **************************************************/ 123 | py::module robot_control = m.def_submodule("_robot_control", "The robot_control submodule of dqrobotics"); 124 | 125 | py::enum_(robot_control, "ControlObjective") 126 | .value("Line", ControlObjective::Line) 127 | .value("None", ControlObjective::None) 128 | .value("Pose", ControlObjective::Pose) 129 | .value("Plane", ControlObjective::Plane) 130 | .value("Distance", ControlObjective::Distance) 131 | .value("DistanceToPlane",ControlObjective::DistanceToPlane) 132 | .value("Rotation", ControlObjective::Rotation) 133 | .value("Translation", ControlObjective::Translation) 134 | .export_values(); 135 | 136 | //DQ_KinematicController 137 | init_DQ_KinematicController_py(robot_control); 138 | 139 | //DQ_TaskSpacePseudoInverseController 140 | init_DQ_PseudoinverseController_py(robot_control); 141 | 142 | //DQ_NumericalFilteredPseudoinverseController 143 | init_DQ_NumericalFilteredPseudoInverseController_py(robot_control); 144 | 145 | //DQ_KinematicConstrainedController 146 | init_DQ_KinematicConstrainedController_py(robot_control); 147 | 148 | //DQ_TaskspaceQuadraticProgrammingController 149 | init_DQ_QuadraticProgrammingController_py(robot_control); 150 | 151 | //DQ_ClassicQPController 152 | init_DQ_ClassicQPController_py(robot_control); 153 | 154 | /***************************************************** 155 | * Interfaces Submodule 156 | * **************************************************/ 157 | py::module interfaces_py = m.def_submodule("_interfaces", "A submodule of dqrobotics"); 158 | 159 | /***************************************************** 160 | * Json11 submodule 161 | * **************************************************/ 162 | py::module json11_py = interfaces_py.def_submodule("_json11", "A submodule of dqrobotics"); 163 | 164 | //DQ_JsonReader 165 | init_DQ_JsonReader_py(json11_py); 166 | 167 | } 168 | 169 | -------------------------------------------------------------------------------- /src/dqrobotics_module.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019 DQ Robotics Developers 4 | 5 | This file is part of DQ Robotics. 6 | 7 | DQ Robotics is free software: you can redistribute it and/or modify 8 | it under the terms of the GNU Lesser General Public License as published by 9 | the Free Software Foundation, either version 3 of the License, or 10 | (at your option) any later version. 11 | 12 | DQ Robotics is distributed in the hope that it will be useful, 13 | but WITHOUT ANY WARRANTY; without even the implied warranty of 14 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 15 | GNU Lesser General Public License for more details. 16 | 17 | You should have received a copy of the GNU Lesser General Public License 18 | along with DQ Robotics. If not, see . 19 | 20 | Contributors: 21 | - Murilo M. Marinho (murilomarinho@ieee.org) 22 | */ 23 | 24 | #include 25 | #include 26 | #include 27 | #include 28 | namespace py = pybind11; 29 | 30 | #include 31 | 32 | #include 33 | #include 34 | #include 35 | 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | #include 47 | 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | 55 | #include 56 | 57 | #include 58 | #include 59 | #include 60 | #include 61 | #include 62 | #include 63 | 64 | #include 65 | 66 | using namespace DQ_robotics; 67 | using namespace Eigen; 68 | 69 | //DQ 70 | void init_DQ_py(py::module& m); 71 | 72 | //dqrobotics/utils 73 | void init_DQ_LinearAlgebra_py(py::module& m); 74 | void init_DQ_Geometry_py(py::module& m); 75 | void init_DQ_Math_py(py::module& m); 76 | 77 | //dqrobotics/robot_modeling 78 | void init_DQ_Kinematics_py(py::module& m); 79 | void init_DQ_SerialManipulator_py(py::module& m); 80 | void init_DQ_SerialManipulatorDH_py(py::module& m); 81 | void init_DQ_SerialManipulatorMDH_py(py::module& m); 82 | void init_DQ_SerialManipulatorDenso_py(py::module& m); 83 | void init_DQ_MobileBase_py(py::module& m); 84 | void init_DQ_HolonomicBase_py(py::module& m); 85 | void init_DQ_DifferentialDriveRobot_py(py::module& m); 86 | void init_DQ_CooperativeDualTaskSpace_py(py::module& m); 87 | void init_DQ_WholeBody_py(py::module& m); 88 | void init_DQ_SerialWholeBody_py(py::module& m); 89 | 90 | //dqrobotics/robot_control 91 | void init_DQ_ClassicQPController_py(py::module& m); 92 | void init_DQ_KinematicConstrainedController_py(py::module& m); 93 | void init_DQ_KinematicController_py(py::module& m); 94 | void init_DQ_PseudoinverseController_py(py::module& m); 95 | void init_DQ_NumericalFilteredPseudoInverseController_py(py::module& m); 96 | void init_DQ_QuadraticProgrammingController_py(py::module& m); 97 | 98 | //dqrobotics/solvers 99 | void init_DQ_QuadraticProgrammingSolver_py(py::module& m); 100 | 101 | //dqrobotics/interfaces/coppeliasim 102 | void init_DQ_CoppeliaSimInterface_py(py::module& m); 103 | void init_DQ_CoppeliaSimInterfaceZMQ_py(py::module& m); 104 | 105 | //dqrobotics/interfaces/json11 106 | void init_DQ_JsonReader_py(py::module& m); 107 | 108 | -------------------------------------------------------------------------------- /src/interfaces/json11/DQ_JsonReader_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../../dqrobotics_module.h" 24 | 25 | void init_DQ_JsonReader_py(py::module& m) 26 | { 27 | py::class_ jsonreader_py(m,"DQ_JsonReader"); 28 | jsonreader_py.def(py::init<>()); 29 | 30 | jsonreader_py.def_static("get_serial_manipulator_dh_from_json",&DQ_JsonReader::get_from_json,"Gets a DQ_KinematicsDH instance from a .json file"); 31 | jsonreader_py.def_static("get_serial_manipulator_denso_from_json",&DQ_JsonReader::get_from_json,"Gets a DQ_KinematicsDenso instance from a .json file"); 32 | //This might be relevant in the future https://github.com/pybind/pybind11/issues/199 33 | } 34 | -------------------------------------------------------------------------------- /src/interfaces/vrep/DQ_SerialVrepRobot_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2023 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../../dqrobotics_module.h" 24 | 25 | void init_DQ_SerialVrepRobot_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * SerialVrepRobot 29 | * **************************************************/ 30 | py::class_< 31 | DQ_SerialVrepRobot, 32 | std::shared_ptr, 33 | DQ_VrepRobot 34 | > dqsv_robot(m,"DQ_SerialVrepRobot"); 35 | 36 | 37 | dqsv_robot.def("get_joint_names", &DQ_SerialVrepRobot::get_joint_names, "Gets the joint names used in CoppeliaSim."); 38 | 39 | dqsv_robot.def("set_target_configuration_space_positions", &DQ_SerialVrepRobot::set_target_configuration_space_positions, "Sets the target configuration space positions in CoppeliaSim."); 40 | 41 | dqsv_robot.def("get_configuration_space_velocities", &DQ_SerialVrepRobot::get_configuration_space_velocities, "Sets the target configuration space velocities in CoppeliaSim."); 42 | dqsv_robot.def("set_target_configuration_space_velocities", &DQ_SerialVrepRobot::set_target_configuration_space_velocities, "Gets the configuration space velocities in CoppeliaSim."); 43 | 44 | dqsv_robot.def("set_configuration_space_torques", &DQ_SerialVrepRobot::set_configuration_space_torques, "Sets the configuration space torques in CoppeliaSim."); 45 | dqsv_robot.def("get_configuration_space_torques", &DQ_SerialVrepRobot::get_configuration_space_torques, "Gets the configuration space torques in CoppeliaSim."); 46 | 47 | //Deprecated 48 | dqsv_robot.def("send_q_target_to_vrep", &DQ_SerialVrepRobot::send_q_target_to_vrep, "Send target joint values to CoppeliaSim."); 49 | } 50 | -------------------------------------------------------------------------------- /src/interfaces/vrep/DQ_VrepInterface_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019-2023 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 21 | - Initial implementation. 22 | 23 | 2. Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) 24 | -Added the method wait_for_simulation_step_to_end() 25 | */ 26 | 27 | #include "../../dqrobotics_module.h" 28 | 29 | //Default arguments added with: 30 | //https://pybind11.readthedocs.io/en/stable/basics.html#default-args 31 | 32 | void init_DQ_VrepInterface_py(py::module& m) 33 | { 34 | /***************************************************** 35 | * VrepInterface 36 | * **************************************************/ 37 | py::class_< 38 | DQ_VrepInterface, 39 | std::shared_ptr 40 | > dqvrepinterface_py(m,"DQ_VrepInterface"); 41 | dqvrepinterface_py.def(py::init<>()); 42 | dqvrepinterface_py.def(py::init()); 43 | 44 | py::enum_(dqvrepinterface_py, "OP_MODES") 45 | .value("OP_BUFFER", DQ_VrepInterface::OP_MODES::OP_BUFFER) 46 | .value("OP_ONESHOT", DQ_VrepInterface::OP_MODES::OP_ONESHOT) 47 | .value("OP_BLOCKING", DQ_VrepInterface::OP_MODES::OP_BLOCKING) 48 | .value("OP_STREAMING", DQ_VrepInterface::OP_MODES::OP_STREAMING) 49 | .value("OP_AUTOMATIC", DQ_VrepInterface::OP_MODES::OP_AUTOMATIC) 50 | .export_values(); 51 | 52 | py::enum_(dqvrepinterface_py, "SCRIPT_TYPES") 53 | .value("ST_CHILD", DQ_VrepInterface::SCRIPT_TYPES::ST_CHILD) 54 | .value("ST_MAIN", DQ_VrepInterface::SCRIPT_TYPES::ST_MAIN) 55 | .value("ST_CUSTOMIZATION", DQ_VrepInterface::SCRIPT_TYPES::ST_CUSTOMIZATION) 56 | .export_values(); 57 | 58 | py::enum_(dqvrepinterface_py, "REFERENCE_FRAMES") 59 | .value("BODY_FRAME", DQ_VrepInterface::REFERENCE_FRAMES::BODY_FRAME) 60 | .value("ABSOLUTE_FRAME", DQ_VrepInterface::REFERENCE_FRAMES::ABSOLUTE_FRAME) 61 | .export_values(); 62 | 63 | dqvrepinterface_py.def("connect",(bool (DQ_VrepInterface::*) (const int&, const int&, const int&))&DQ_VrepInterface::connect,"Connects to V-REP in local machine."); 64 | dqvrepinterface_py.def("connect",(bool (DQ_VrepInterface::*) (const std::string&, const int&, const int&, const int&))&DQ_VrepInterface::connect,"Connects to V-REP with a given ip."); 65 | 66 | dqvrepinterface_py.def("disconnect", &DQ_VrepInterface::disconnect,"Disconnects from V-REP."); 67 | dqvrepinterface_py.def("disconnect_all",&DQ_VrepInterface::disconnect_all,"Disconnect all from V-REP"); 68 | 69 | dqvrepinterface_py.def("start_simulation",&DQ_VrepInterface::start_simulation,"Start simulation"); 70 | dqvrepinterface_py.def("stop_simulation", &DQ_VrepInterface::stop_simulation,"Stops simulation"); 71 | 72 | dqvrepinterface_py.def("is_simulation_running",&DQ_VrepInterface::is_simulation_running,"Checks whether the simulation is running or not"); 73 | 74 | // void set_synchronous(const bool& flag); 75 | dqvrepinterface_py.def("set_synchronous", (void (DQ_VrepInterface::*) (const bool&))&DQ_VrepInterface::set_synchronous, "Sets synchronous mode"); 76 | 77 | //void trigger_next_simulation_step(); 78 | dqvrepinterface_py.def("trigger_next_simulation_step", &DQ_VrepInterface::trigger_next_simulation_step, "Sends a synchronization trigger signal to the server."); 79 | 80 | //void wait_for_simulation_step_to_end(); 81 | dqvrepinterface_py.def("wait_for_simulation_step_to_end", &DQ_VrepInterface::wait_for_simulation_step_to_end, "Waits until the simulation step is finished."); 82 | 83 | //dqvrepinterface_py.def("get_object_handle", &DQ_VrepInterface::get_object_handle,"Gets an object handle"); 84 | //dqvrepinterface_py.def("get_object_handles",&DQ_VrepInterface::get_object_handles,"Get object handles"); 85 | 86 | //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); 87 | //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); 88 | //dqvrepinterface_py.def("get_object_translation",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation,"Gets object translation."); 89 | dqvrepinterface_py.def("get_object_translation", 90 | (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_translation, 91 | "Gets object translation.", 92 | py::arg("objectname")=std::string(""), 93 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 94 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 95 | 96 | //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_translation,"Sets object translation."); 97 | //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation,"Sets object translation."); 98 | //dqvrepinterface_py.def("set_object_translation",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation,"Sets object translation."); 99 | dqvrepinterface_py.def("set_object_translation", 100 | (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_translation, 101 | "Sets object translation.", 102 | py::arg("objectname")=std::string(""), 103 | py::arg("t")=DQ(0), 104 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 105 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 106 | 107 | //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); 108 | //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); 109 | //dqvrepinterface_py.def("get_object_rotation",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation,"Gets object rotation."); 110 | dqvrepinterface_py.def("get_object_rotation", 111 | (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_rotation, 112 | "Gets object rotation.", 113 | py::arg("objectname")=std::string(""), 114 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 115 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 116 | 117 | //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); 118 | //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); 119 | //dqvrepinterface_py.def("set_object_rotation",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation,"Sets object rotation."); 120 | dqvrepinterface_py.def("set_object_rotation", 121 | (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_rotation, 122 | "Sets object rotation.", 123 | py::arg("objectname")=std::string(""), 124 | py::arg("r")=DQ(1), 125 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 126 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 127 | 128 | //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const int&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); 129 | //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const std::string&, const int&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); 130 | //dqvrepinterface_py.def("get_object_pose",(DQ (DQ_VrepInterface::*) (const int&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose,"Gets object pose."); 131 | dqvrepinterface_py.def("get_object_pose", 132 | (DQ (DQ_VrepInterface::*) (const std::string&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_object_pose, 133 | "Gets object pose.", 134 | py::arg("objectname")=std::string(""), 135 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 136 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 137 | 138 | //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const int&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&) const)&DQ_VrepInterface::set_object_pose,"Sets object pose."); 139 | //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const std::string&, const int&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose,"Sets object pose."); 140 | //dqvrepinterface_py.def("set_object_pose",(void (DQ_VrepInterface::*) (const int&, const std::string&, const DQ&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose,"Sets object pose."); 141 | dqvrepinterface_py.def("set_object_pose", 142 | (void (DQ_VrepInterface::*) (const std::string&, const DQ&, const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_object_pose, 143 | "Sets object pose.", 144 | py::arg("objectname")=std::string(""), 145 | py::arg("h")=DQ(1), 146 | py::arg("relative_to_objectname")=VREP_OBJECTNAME_ABSOLUTE, 147 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 148 | 149 | dqvrepinterface_py.def("get_object_poses",&DQ_VrepInterface::get_object_poses,"Get the poses of many objects"); 150 | dqvrepinterface_py.def("set_object_poses",&DQ_VrepInterface::set_object_poses,"Set object poses of many objects"); 151 | 152 | //dqvrepinterface_py.def("set_joint_position",(void (DQ_VrepInterface::*) (const int&, const double&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_position,"Set joint position"); 153 | dqvrepinterface_py.def("set_joint_position", 154 | (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_position, 155 | "Set joint position", 156 | py::arg("jointname")=std::string(""), 157 | py::arg("angle_rad")=0.0, 158 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 159 | 160 | //dqvrepinterface_py.def("set_joint_target_position",(void (DQ_VrepInterface::*) (const int&, const double&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_target_position,"Set joint position"); 161 | dqvrepinterface_py.def("set_joint_target_position", 162 | (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_position, 163 | "Set joint position", 164 | py::arg("jointname")=std::string(""), 165 | py::arg("angle_rad")=0.0, 166 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 167 | 168 | //dqvrepinterface_py.def("get_joint_position",(double (DQ_VrepInterface::*) (const int&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::get_joint_position,"Get joint position"); 169 | dqvrepinterface_py.def("get_joint_position", 170 | (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_position, 171 | "Get joint position", 172 | py::arg("jointname")=std::string(""), 173 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 174 | 175 | //dqvrepinterface_py.def("set_joint_positions",(void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_positions,"Set joint positions"); 176 | dqvrepinterface_py.def("set_joint_positions", 177 | (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_positions, 178 | "Set joint positions", 179 | py::arg("jointnames")=std::vector(), 180 | py::arg("angles_rad")=VectorXd::Zero(1), 181 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 182 | 183 | //dqvrepinterface_py.def("set_joint_target_positions",(void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::set_joint_target_positions,"Set joint positions"); 184 | dqvrepinterface_py.def("set_joint_target_positions", 185 | (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_positions, 186 | "Set joint positions", 187 | py::arg("jointnames")=std::vector(), 188 | py::arg("angles_rad")=VectorXd::Zero(1), 189 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 190 | 191 | //dqvrepinterface_py.def("get_joint_positions",(VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&) const) &DQ_VrepInterface::get_joint_positions,"Get joint positions"); 192 | dqvrepinterface_py.def("get_joint_positions", 193 | (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_positions, 194 | "Get joint positions", 195 | py::arg("jointnames")=std::vector(), 196 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 197 | 198 | 199 | //void set_joint_target_velocity(const std::string& jointname, const double& angle_dot_rad, const OP_MODES& opmode=OP_ONESHOT); 200 | dqvrepinterface_py.def("set_joint_target_velocity", 201 | (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_velocity, 202 | "Set joint velocity", 203 | py::arg("jointname")=std::string(""), 204 | py::arg("angle_dot_rad")=0.0, 205 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 206 | 207 | //void set_joint_target_velocities(const std::vector& jointnames, const VectorXd& angles_dot_rad, const OP_MODES& opmode=OP_ONESHOT); 208 | dqvrepinterface_py.def("set_joint_target_velocities", 209 | (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_target_velocities, 210 | "Set joint velcoties", 211 | py::arg("jointnames")=std::vector(), 212 | py::arg("angles_dot_rad")=VectorXd::Zero(1), 213 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 214 | 215 | //double get_joint_velocity(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC); 216 | dqvrepinterface_py.def("get_joint_velocity", 217 | (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_velocity, 218 | "Get joint velocity", 219 | py::arg("jointname")=std::string(""), 220 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 221 | 222 | //VectorXd get_joint_velocities(const std::vector& jointnames, const OP_MODES& opmode=OP_AUTOMATIC); 223 | dqvrepinterface_py.def("get_joint_velocities", 224 | (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_velocities, 225 | "Get joint velocities", 226 | py::arg("jointnames")=std::vector(), 227 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 228 | 229 | 230 | //void set_joint_torque(const std::string& jointname, const double& torque, const OP_MODES& opmode=OP_ONESHOT); 231 | dqvrepinterface_py.def("set_joint_torque", 232 | (void (DQ_VrepInterface::*) (const std::string&, const double&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_torque, 233 | "Set joint torque", 234 | py::arg("jointname")=std::string(""), 235 | py::arg("torque")=0.0, 236 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 237 | 238 | //void set_joint_torques(const std::vector& jointnames, const VectorXd& torques, const OP_MODES& opmode=OP_ONESHOT); 239 | dqvrepinterface_py.def("set_joint_torques", 240 | (void (DQ_VrepInterface::*) (const std::vector&, const VectorXd&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::set_joint_torques, 241 | "Set joint torques", 242 | py::arg("jointnames")=std::vector(), 243 | py::arg("torques")=VectorXd::Zero(1), 244 | py::arg("opmode")=DQ_VrepInterface::OP_ONESHOT); 245 | 246 | //double get_joint_torque(const std::string& jointname, const OP_MODES& opmode=OP_AUTOMATIC); 247 | dqvrepinterface_py.def("get_joint_torque", 248 | (double (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_torque, 249 | "Get joint torque", 250 | py::arg("jointname")=std::string(""), 251 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 252 | 253 | //VectorXd get_joint_torques(const std::vector& jointnames, const OP_MODES& opmode=OP_AUTOMATIC); 254 | dqvrepinterface_py.def("get_joint_torques", 255 | (VectorXd (DQ_VrepInterface::*) (const std::vector&, const DQ_VrepInterface::OP_MODES&))&DQ_VrepInterface::get_joint_torques, 256 | "Get joint torques", 257 | py::arg("jointnames")=std::vector(), 258 | py::arg("opmode")=DQ_VrepInterface::OP_AUTOMATIC); 259 | 260 | //double get_mass(const std::string& link_name, const std::string& function_name = "get_mass", const std::string& obj_name= "DQRoboticsApiCommandServer"); 261 | dqvrepinterface_py.def("get_mass", 262 | (double (DQ_VrepInterface::*) (const std::string&, const std::string&, const std::string&))&DQ_VrepInterface::get_mass, 263 | "Get the mass of an object from CoppeliaSim", 264 | py::arg("linkname")=std::string(""), 265 | py::arg("function_name")=std::string("get_mass"), 266 | py::arg("obj_name")=std::string("DQRoboticsApiCommandServer")); 267 | 268 | //DQ get_center_of_mass(const std::string& link_name, 269 | // const REFERENCE_FRAMES& reference_frame=BODY_FRAME, 270 | // const std::string& function_name = "get_center_of_mass", 271 | // const std::string& obj_name= "DQRoboticsApiCommandServer"); 272 | dqvrepinterface_py.def("get_center_of_mass", 273 | (DQ (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::REFERENCE_FRAMES&, 274 | const std::string&, const std::string&))&DQ_VrepInterface::get_center_of_mass, 275 | "Get the center of mass of an object from CoppeliaSim", 276 | py::arg("linkname")=std::string(""), 277 | py::arg("reference_frame")=DQ_VrepInterface::BODY_FRAME, 278 | py::arg("function_name")=std::string("get_center_of_mass"), 279 | py::arg("obj_name")=std::string("DQRoboticsApiCommandServer")); 280 | 281 | //MatrixXd get_inertia_matrix(const std::string& link_name, 282 | // const REFERENCE_FRAMES& reference_frame=BODY_FRAME, 283 | // const std::string& function_name = "get_inertia", 284 | // const std::string& obj_name= "DQRoboticsApiCommandServer"); 285 | dqvrepinterface_py.def("get_inertia_matrix", 286 | (MatrixXd (DQ_VrepInterface::*) (const std::string&, const DQ_VrepInterface::REFERENCE_FRAMES&, 287 | const std::string&, const std::string&))&DQ_VrepInterface::get_inertia_matrix, 288 | "Get the inertia matrix of an object from CoppeliaSim", 289 | py::arg("linkname")=std::string(""), 290 | py::arg("reference_frame")=DQ_VrepInterface::BODY_FRAME, 291 | py::arg("function_name")=std::string("get_inertia"), 292 | py::arg("obj_name")=std::string("DQRoboticsApiCommandServer")); 293 | 294 | } 295 | -------------------------------------------------------------------------------- /src/interfaces/vrep/DQ_VrepRobot_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../../dqrobotics_module.h" 24 | 25 | void init_DQ_VrepRobot_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * VrepRobot 29 | * **************************************************/ 30 | py::class_< 31 | DQ_VrepRobot, 32 | std::shared_ptr 33 | > dqvreprobot_py(m,"DQ_VrepRobot"); 34 | 35 | dqvreprobot_py.def("set_configuration_space_positions", &DQ_VrepRobot::set_configuration_space_positions, "Sets the configuration space positions in CoppeliaSim."); 36 | dqvreprobot_py.def("get_configuration_space_positions", &DQ_VrepRobot::get_configuration_space_positions, "Gets the configuration space positions in CoppeliaSim."); 37 | 38 | //Deprecated 39 | dqvreprobot_py.def("send_q_to_vrep", &DQ_VrepRobot::send_q_to_vrep, "Get joint values from vrep."); 40 | dqvreprobot_py.def("get_q_from_vrep", &DQ_VrepRobot::get_q_from_vrep, "Send joint values to vrep."); 41 | } 42 | -------------------------------------------------------------------------------- /src/robot_control/DQ_ClassicQPController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_ClassicQPController_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ ClassicQPController 29 | * **************************************************/ 30 | py::class_ dq_classicqpcontroller_py(m,"DQ_ClassicQPController"); 31 | dq_classicqpcontroller_py.def(py::init< 32 | const std::shared_ptr&, 33 | const std::shared_ptr& 34 | >()); 35 | dq_classicqpcontroller_py.def("compute_objective_function_symmetric_matrix", &DQ_ClassicQPController::compute_objective_function_symmetric_matrix, "Compute symmetric matrix."); 36 | dq_classicqpcontroller_py.def("compute_objective_function_linear_component", &DQ_ClassicQPController::compute_objective_function_linear_component, "Compute the objective function."); 37 | } 38 | -------------------------------------------------------------------------------- /src/robot_control/DQ_KinematicConstrainedController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_KinematicConstrainedController_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ KinematicConstrainedController 29 | * **************************************************/ 30 | py::class_ dqkinematicconstrainedcontroller_py(m,"DQ_KinematicConstrainedController"); 31 | dqkinematicconstrainedcontroller_py.def("set_equality_constraint", &DQ_KinematicConstrainedController::set_equality_constraint, "Sets equality constraints."); 32 | dqkinematicconstrainedcontroller_py.def("set_inequality_constraint", &DQ_KinematicConstrainedController::set_inequality_constraint, "Sets inequality constraints."); 33 | 34 | } 35 | -------------------------------------------------------------------------------- /src/robot_control/DQ_KinematicController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | class DQ_KinematicControllerPub : public DQ_KinematicController 26 | { 27 | public: 28 | using DQ_KinematicController::_get_robot; 29 | }; 30 | 31 | void init_DQ_KinematicController_py(py::module& m) 32 | { 33 | /***************************************************** 34 | * DQ KinematicController 35 | * **************************************************/ 36 | py::class_ kc_py(m,"DQ_KinematicController"); 37 | kc_py.def("get_control_objective" ,&DQ_KinematicController::get_control_objective,"Gets the control objective"); 38 | kc_py.def("get_jacobian" ,&DQ_KinematicController::get_jacobian,"Gets the Jacobian"); 39 | kc_py.def("get_last_error_signal" ,&DQ_KinematicController::get_last_error_signal, "Gets the last error signal"); 40 | kc_py.def("get_task_variable" ,&DQ_KinematicController::get_task_variable, "Gets the task variable"); 41 | kc_py.def("is_set" ,&DQ_KinematicController::is_set,"Checks if the controller's objective has been set"); 42 | kc_py.def("system_reached_stable_region",&DQ_KinematicController::system_reached_stable_region,"Checks if the controller has stabilized"); 43 | kc_py.def("set_control_objective" ,&DQ_KinematicController::set_control_objective,"Sets the control objective"); 44 | kc_py.def("set_gain" ,&DQ_KinematicController::set_gain,"Sets the controller gain"); 45 | kc_py.def("get_gain" ,&DQ_KinematicController::get_gain,"Gets the controller gain"); 46 | kc_py.def("set_stability_threshold" ,&DQ_KinematicController::set_stability_threshold,"Sets the stability threshold"); 47 | kc_py.def("set_damping" ,&DQ_KinematicController::set_damping, "Sets the damping."); 48 | kc_py.def("get_damping" ,&DQ_KinematicController::get_damping, "Gets the damping."); 49 | kc_py.def("set_primitive_to_effector" ,&DQ_KinematicController::set_primitive_to_effector, "Sets the effector primitive"); 50 | kc_py.def("set_target_primitive" ,&DQ_KinematicController::set_target_primitive, "Sets the target primitive"); 51 | kc_py.def("set_stability_counter_max" ,&DQ_KinematicController::set_stability_counter_max, "Sets the maximum of the stability counter"); 52 | kc_py.def("reset_stability_counter" ,&DQ_KinematicController::reset_stability_counter, "Resets the stability counter"); 53 | kc_py.def("_get_robot",&DQ_KinematicControllerPub::_get_robot , "Gets the robot"); 54 | 55 | 56 | } 57 | -------------------------------------------------------------------------------- /src/robot_control/DQ_NumericalFilteredPseudoInverseController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2022 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | class DQ_NumericalFilteredPseudoinverseControllerPy : public DQ_NumericalFilteredPseudoinverseController 26 | { 27 | public: 28 | using DQ_NumericalFilteredPseudoinverseController::DQ_NumericalFilteredPseudoinverseController; 29 | DQ_NumericalFilteredPseudoinverseControllerPy()=delete; 30 | DQ_NumericalFilteredPseudoinverseControllerPy(const std::shared_ptr& r): 31 | DQ_NumericalFilteredPseudoinverseController(r) 32 | { 33 | 34 | } 35 | 36 | /* Trampoline (need one for each virtual function) */ 37 | VectorXd compute_setpoint_control_signal(const VectorXd &q, const VectorXd &task_reference) override 38 | { 39 | PYBIND11_OVERRIDE( 40 | VectorXd, /* Return type */ 41 | DQ_NumericalFilteredPseudoinverseController, /* Parent class */ 42 | compute_setpoint_control_signal, /* Name of function in C++ (must match Python name) */ 43 | q,task_reference /* Argument(s) */ 44 | ); 45 | } 46 | 47 | VectorXd compute_tracking_control_signal(const VectorXd &q, const VectorXd &task_reference, const VectorXd &feed_forward) override 48 | { 49 | PYBIND11_OVERRIDE( 50 | VectorXd, /* Return type */ 51 | DQ_NumericalFilteredPseudoinverseController, /* Parent class */ 52 | compute_tracking_control_signal, /* Name of function in C++ (must match Python name) */ 53 | q,task_reference,feed_forward /* Argument(s) */ 54 | ); 55 | } 56 | }; 57 | 58 | 59 | void init_DQ_NumericalFilteredPseudoInverseController_py(py::module& m) 60 | { 61 | /***************************************************** 62 | * DQ TaskSpacePseudoInverseController 63 | * **************************************************/ 64 | py::class_< 65 | DQ_NumericalFilteredPseudoinverseController, 66 | DQ_NumericalFilteredPseudoinverseControllerPy, 67 | DQ_PseudoinverseController 68 | > nfpic(m,"DQ_NumericalFilteredPseudoinverseController"); 69 | nfpic.def(py::init< 70 | const std::shared_ptr& 71 | >()); 72 | nfpic.def("compute_setpoint_control_signal",&DQ_NumericalFilteredPseudoinverseController::compute_setpoint_control_signal,"Computes the setpoint control signal."); 73 | nfpic.def("compute_tracking_control_signal",&DQ_NumericalFilteredPseudoinverseController::compute_tracking_control_signal,"Computes the tracking control signal."); 74 | nfpic.def("set_maximum_numerical_filtered_damping",&DQ_NumericalFilteredPseudoinverseController::set_maximum_numerical_filtered_damping,"Sets the maximum numerical filtered damping."); 75 | nfpic.def("set_singular_region_size",&DQ_NumericalFilteredPseudoinverseController::set_singular_region_size,"Sets the singular region size."); 76 | nfpic.def("get_maximum_numerical_filtered_damping",&DQ_NumericalFilteredPseudoinverseController::get_maximum_numerical_filtered_damping,"Gets the maximum numerical filtered damping."); 77 | nfpic.def("get_singular_region_size",&DQ_NumericalFilteredPseudoinverseController::get_singular_region_size,"Gets the singular region size."); 78 | nfpic.def("get_last_filtered_damping",&DQ_NumericalFilteredPseudoinverseController::get_last_filtered_damping,"Gets the last filtered damping."); 79 | nfpic.def("get_last_jacobian_rank",&DQ_NumericalFilteredPseudoinverseController::get_last_jacobian_rank,"Gets the last Jacobian rank."); 80 | nfpic.def("get_last_jacobian_svd",&DQ_NumericalFilteredPseudoinverseController::get_last_jacobian_svd,"Gets the last Jacobian svd."); 81 | } 82 | -------------------------------------------------------------------------------- /src/robot_control/DQ_PseudoinverseController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_PseudoinverseController_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ TaskSpacePseudoInverseController 29 | * **************************************************/ 30 | py::class_ dqpseudoinversecontroller_py(m,"DQ_PseudoinverseController"); 31 | dqpseudoinversecontroller_py.def(py::init< 32 | const std::shared_ptr& 33 | >()); 34 | dqpseudoinversecontroller_py.def("compute_setpoint_control_signal",&DQ_PseudoinverseController::compute_setpoint_control_signal,"Computes the setpoint control signal."); 35 | dqpseudoinversecontroller_py.def("compute_tracking_control_signal",&DQ_PseudoinverseController::compute_tracking_control_signal,"Computes the tracking control signal."); 36 | } 37 | -------------------------------------------------------------------------------- /src/robot_control/DQ_QuadraticProgrammingController_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | class DQ_QuadraticProgrammingControllerPy : public DQ_QuadraticProgrammingController 26 | { 27 | protected: 28 | /* Inherit the constructors */ 29 | using DQ_QuadraticProgrammingController::DQ_QuadraticProgrammingController; 30 | public: 31 | DQ_QuadraticProgrammingControllerPy()=delete; 32 | DQ_QuadraticProgrammingControllerPy(const std::shared_ptr& r, 33 | const std::shared_ptr& s): 34 | DQ_QuadraticProgrammingController(r,s) 35 | { 36 | 37 | } 38 | 39 | //virtual VectorXd compute_setpoint_control_signal(const VectorXd&q, const VectorXd& task_reference) override; 40 | //virtual VectorXd compute_tracking_control_signal(const VectorXd&q, const VectorXd& task_reference, const VectorXd& feed_forward) override; 41 | 42 | /* Trampoline (need one for each virtual function) */ 43 | MatrixXd compute_objective_function_symmetric_matrix(const MatrixXd& J, const VectorXd& task_error) override 44 | { 45 | PYBIND11_OVERLOAD_PURE( 46 | MatrixXd, /* Return type */ 47 | DQ_QuadraticProgrammingController, /* Parent class */ 48 | compute_objective_function_symmetric_matrix, /* Name of function in C++ (must match Python name) */ 49 | J, task_error /* Argument(s) */ 50 | ) 51 | } 52 | 53 | VectorXd compute_objective_function_linear_component(const MatrixXd& J, const VectorXd& task_error) override 54 | { 55 | PYBIND11_OVERLOAD_PURE( 56 | VectorXd, /* Return type */ 57 | DQ_QuadraticProgrammingController, /* Parent class */ 58 | compute_objective_function_linear_component, /* Name of function in C++ (must match Python name) */ 59 | J, task_error /* Argument(s) */ 60 | ) 61 | } 62 | }; 63 | 64 | 65 | void init_DQ_QuadraticProgrammingController_py(py::module& m) 66 | { 67 | /***************************************************** 68 | * DQ TaskspaceQuadraticProgrammingController 69 | * **************************************************/ 70 | py::class_ qpcpy(m,"DQ_QuadraticProgrammingController"); 71 | qpcpy.def(py::init< 72 | const std::shared_ptr&, 73 | const std::shared_ptr& 74 | >()); 75 | qpcpy.def("compute_objective_function_symmetric_matrix", &DQ_QuadraticProgrammingController::compute_objective_function_symmetric_matrix, "Compute symmetric matrix."); 76 | qpcpy.def("compute_objective_function_linear_component", &DQ_QuadraticProgrammingController::compute_objective_function_linear_component, "Compute the objective function."); 77 | qpcpy.def("compute_setpoint_control_signal", &DQ_QuadraticProgrammingController::compute_setpoint_control_signal, "Compute the setpoint control signal."); 78 | qpcpy.def("compute_tracking_control_signal", &DQ_QuadraticProgrammingController::compute_tracking_control_signal, "Compute the tracking control signal."); 79 | } 80 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_CooperativeDualTaskSpace_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_CooperativeDualTaskSpace_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ CooperativeDualTaskSpace 29 | * **************************************************/ 30 | py::class_< 31 | DQ_CooperativeDualTaskSpace 32 | > dqcooperativedualtaskspace(m, "DQ_CooperativeDualTaskSpace"); 33 | dqcooperativedualtaskspace.def(py::init()); 34 | dqcooperativedualtaskspace.def("pose1", &DQ_CooperativeDualTaskSpace::pose1, "Returns the first robot's pose"); 35 | dqcooperativedualtaskspace.def("pose2", &DQ_CooperativeDualTaskSpace::pose2,"Returns the second robot's pose"); 36 | dqcooperativedualtaskspace.def("absolute_pose", &DQ_CooperativeDualTaskSpace::absolute_pose,"Returns the absolute pose"); 37 | dqcooperativedualtaskspace.def("relative_pose", &DQ_CooperativeDualTaskSpace::relative_pose,"Returns the relative pose"); 38 | dqcooperativedualtaskspace.def("pose_jacobian1", &DQ_CooperativeDualTaskSpace::pose_jacobian1,"Returns the pose Jacobian of the first robot"); 39 | dqcooperativedualtaskspace.def("pose_jacobian2", &DQ_CooperativeDualTaskSpace::pose_jacobian2,"Returns the pose Jacobian of the second robot"); 40 | dqcooperativedualtaskspace.def("absolute_pose_jacobian", &DQ_CooperativeDualTaskSpace::absolute_pose_jacobian,"Returns the absolute pose Jacobian"); 41 | dqcooperativedualtaskspace.def("relative_pose_jacobian", &DQ_CooperativeDualTaskSpace::relative_pose_jacobian,"Returns the relative pose Jacobian"); 42 | } 43 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_DifferentialDriveRobot_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_DifferentialDriveRobot_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ DifferentialDriveRobot 29 | * **************************************************/ 30 | py::class_< 31 | DQ_DifferentialDriveRobot, 32 | std::shared_ptr, 33 | DQ_HolonomicBase 34 | > dqdifferentialdriverobot_py(m,"DQ_DifferentialDriveRobot"); 35 | dqdifferentialdriverobot_py.def(py::init()); 36 | dqdifferentialdriverobot_py.def("constraint_jacobian", &DQ_DifferentialDriveRobot::constraint_jacobian, "Returns the constraint Jacobian"); 37 | dqdifferentialdriverobot_py.def("pose_jacobian", (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const int&) const)&DQ_DifferentialDriveRobot::pose_jacobian,"Returns the pose Jacobian"); 38 | dqdifferentialdriverobot_py.def("pose_jacobian", (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&) const)&DQ_DifferentialDriveRobot::pose_jacobian,"Returns the pose Jacobian"); 39 | dqdifferentialdriverobot_py.def("pose_jacobian_derivative", (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, 40 | "Returns the pose Jacobian derivative"); 41 | dqdifferentialdriverobot_py.def("pose_jacobian_derivative", (MatrixXd (DQ_DifferentialDriveRobot::*)(const VectorXd&, const VectorXd&) const)&DQ_DifferentialDriveRobot::pose_jacobian_derivative, 42 | "Returns the pose Jacobian derivative"); 43 | } 44 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_HolonomicBase_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_HolonomicBase_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ HolonomicBase 29 | * **************************************************/ 30 | py::class_< 31 | DQ_HolonomicBase, 32 | std::shared_ptr, 33 | DQ_MobileBase 34 | > dqholonomicbase_py(m,"DQ_HolonomicBase"); 35 | dqholonomicbase_py.def(py::init()); 36 | dqholonomicbase_py.def("fkm",(DQ (DQ_HolonomicBase::*)(const VectorXd&) const)&DQ_HolonomicBase::fkm,"Gets the fkm."); 37 | dqholonomicbase_py.def("fkm",(DQ (DQ_HolonomicBase::*)(const VectorXd&,const int&) const)&DQ_HolonomicBase::fkm,"Gets the fkm."); 38 | dqholonomicbase_py.def("pose_jacobian", (MatrixXd (DQ_HolonomicBase::*)(const VectorXd&, const int&) const)&DQ_HolonomicBase::pose_jacobian,"Returns the pose Jacobian"); 39 | dqholonomicbase_py.def("pose_jacobian", (MatrixXd (DQ_HolonomicBase::*)(const VectorXd&) const)&DQ_HolonomicBase::pose_jacobian,"Returns the pose Jacobian"); 40 | dqholonomicbase_py.def("pose_jacobian_derivative", (MatrixXd (DQ_HolonomicBase::*)(const VectorXd&, const VectorXd&) const)&DQ_HolonomicBase::pose_jacobian_derivative, 41 | "Returns the pose Jacobian derivative"); 42 | dqholonomicbase_py.def("pose_jacobian_derivative", (MatrixXd (DQ_HolonomicBase::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_HolonomicBase::pose_jacobian_derivative, 43 | "Returns the pose Jacobian derivative"); 44 | dqholonomicbase_py.def("get_dim_configuration_space",&DQ_HolonomicBase::get_dim_configuration_space,"Returns the size of the configuration space"); 45 | dqholonomicbase_py.def("raw_fkm", &DQ_HolonomicBase::raw_fkm,"Returns the raw fkm"); 46 | dqholonomicbase_py.def("raw_pose_jacobian", &DQ_HolonomicBase::raw_pose_jacobian,"Return the raw pose Jacobian"); 47 | dqholonomicbase_py.def("raw_pose_jacobian_derivative", &DQ_HolonomicBase::raw_pose_jacobian_derivative,"Return the raw pose Jacobian derivative"); 48 | } 49 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_Kinematics_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_Kinematics_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ Kinematics 29 | * **************************************************/ 30 | py::class_< 31 | DQ_Kinematics, 32 | std::shared_ptr 33 | > dqkinematics_py(m, "DQ_Kinematics"); 34 | //dqkinematics_py.def(py::init<>()); 35 | //dqkinematics_py.def("pose_jacobian", (MatrixXd (DQ_Kinematics::*)(const VectorXd&) const)&DQ_Kinematics::pose_jacobian, "Returns the pose Jacobian"); 36 | 37 | //Concrete 38 | dqkinematics_py.def("get_dim_configuration_space", &DQ_Kinematics::get_dim_configuration_space, "Returns the dimension of the configuration space"); 39 | dqkinematics_py.def("get_reference_frame", &DQ_Kinematics::get_reference_frame, "Returns the current reference frame"); 40 | dqkinematics_py.def("set_reference_frame", &DQ_Kinematics::set_reference_frame, "Sets the reference frame"); 41 | dqkinematics_py.def("get_base_frame", &DQ_Kinematics::get_base_frame, "Returns the current base frame"); 42 | dqkinematics_py.def("set_base_frame", &DQ_Kinematics::set_base_frame, "Sets the base frame"); 43 | 44 | //Static 45 | dqkinematics_py.def_static("distance_jacobian", &DQ_Kinematics::distance_jacobian, "Returns the distance Jacobian"); 46 | dqkinematics_py.def_static("translation_jacobian", &DQ_Kinematics::translation_jacobian, "Returns the translation Jacobian"); 47 | dqkinematics_py.def_static("rotation_jacobian", &DQ_Kinematics::rotation_jacobian, "Returns the rotation Jacobian"); 48 | dqkinematics_py.def_static("line_jacobian", &DQ_Kinematics::line_jacobian, "Returns the line Jacobian"); 49 | dqkinematics_py.def_static("plane_jacobian", &DQ_Kinematics::plane_jacobian, "Returns the plane Jacobian"); 50 | dqkinematics_py.def_static("distance_jacobian_derivative", &DQ_Kinematics::distance_jacobian_derivative, "Returns the distance Jacobian derivative"); 51 | dqkinematics_py.def_static("translation_jacobian_derivative", &DQ_Kinematics::translation_jacobian_derivative, "Returns the translation Jacobian derivative"); 52 | dqkinematics_py.def_static("rotation_jacobian_derivative", &DQ_Kinematics::rotation_jacobian_derivative, "Returns the rotation Jacobian derivative"); 53 | dqkinematics_py.def_static("line_jacobian_derivative", &DQ_Kinematics::line_jacobian_derivative, "Returns the line Jacobian derivative"); 54 | dqkinematics_py.def_static("plane_jacobian_derivative", &DQ_Kinematics::plane_jacobian_derivative, "Returns the plane Jacobian derivative"); 55 | dqkinematics_py.def_static("point_to_point_distance_jacobian", &DQ_Kinematics::point_to_point_distance_jacobian,"Returns the robot point to point distance Jacobian"); 56 | dqkinematics_py.def_static("point_to_point_residual", &DQ_Kinematics::point_to_point_residual,"Returns the robot point to point residual"); 57 | dqkinematics_py.def_static("point_to_line_distance_jacobian", &DQ_Kinematics::point_to_line_distance_jacobian,"Returns the robot point to line distance Jacobian"); 58 | dqkinematics_py.def_static("point_to_line_residual", &DQ_Kinematics::point_to_line_residual,"Returns the robot point to line residual"); 59 | dqkinematics_py.def_static("point_to_plane_distance_jacobian", &DQ_Kinematics::point_to_plane_distance_jacobian,"Returns the robot point to plane distance Jacobian"); 60 | dqkinematics_py.def_static("point_to_plane_residual", &DQ_Kinematics::point_to_plane_residual,"Returns the robot point to plane residual"); 61 | dqkinematics_py.def_static("line_to_point_distance_jacobian", &DQ_Kinematics::line_to_point_distance_jacobian,"Returns the robot line to point distance Jacobian"); 62 | dqkinematics_py.def_static("line_to_point_residual", &DQ_Kinematics::line_to_point_residual,"Returns the robot line to point residual"); 63 | dqkinematics_py.def_static("line_to_line_distance_jacobian", &DQ_Kinematics::line_to_line_distance_jacobian,"Returns the robot line to line distance Jacobian"); 64 | dqkinematics_py.def_static("line_to_line_residual", &DQ_Kinematics::line_to_line_residual, "Returns the robot line to line residual"); 65 | dqkinematics_py.def_static("plane_to_point_distance_jacobian", &DQ_Kinematics::plane_to_point_distance_jacobian,"Returns the robot plane to point distance Jacobian"); 66 | dqkinematics_py.def_static("plane_to_point_residual", &DQ_Kinematics::plane_to_point_residual,"Returns the robot plane to point residual"); 67 | dqkinematics_py.def_static("line_to_line_angle_jacobian", &DQ_Kinematics::line_to_line_angle_jacobian,"Returns the line to line angle Jacobian"); 68 | dqkinematics_py.def_static("line_to_line_angle_residual", &DQ_Kinematics::line_to_line_angle_residual,"Returns the line to line angle residual"); 69 | dqkinematics_py.def_static("line_segment_to_line_segment_distance_jacobian", &DQ_Kinematics::line_segment_to_line_segment_distance_jacobian, "Returns the line segment to line segment distance Jacobian"); 70 | } 71 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_MobileBase_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_MobileBase_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ MobileBase 29 | * **************************************************/ 30 | py::class_< 31 | DQ_MobileBase, 32 | std::shared_ptr, 33 | DQ_Kinematics> dqmobilebase_py(m,"DQ_MobileBase"); 34 | dqmobilebase_py.def("set_frame_displacement", &DQ_MobileBase::set_frame_displacement,"Set the frame displacement"); 35 | dqmobilebase_py.def("frame_displacement", &DQ_MobileBase::frame_displacement, "Get the frame displacement"); 36 | } 37 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulatorDH_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_SerialManipulatorDH_py(py::module& m) 26 | { 27 | /*************************************************** 28 | * DQ SerialManipulatorDH 29 | * **************************************************/ 30 | py::class_< 31 | DQ_SerialManipulatorDH, 32 | std::shared_ptr, 33 | DQ_SerialManipulator 34 | > dqserialmanipulatordh_py(m, "DQ_SerialManipulatorDH"); 35 | dqserialmanipulatordh_py.def(py::init()); 36 | 37 | ///Methods 38 | //Concrete 39 | dqserialmanipulatordh_py.def("get_thetas", &DQ_SerialManipulatorDH::get_thetas, "Retrieves the vector of thetas."); 40 | dqserialmanipulatordh_py.def("get_ds", &DQ_SerialManipulatorDH::get_ds, "Retrieves the vector of ds."); 41 | dqserialmanipulatordh_py.def("get_as", &DQ_SerialManipulatorDH::get_as, "Retrieves the vector of as."); 42 | dqserialmanipulatordh_py.def("get_alphas", &DQ_SerialManipulatorDH::get_alphas, "Retrieves the vector of alphas."); 43 | dqserialmanipulatordh_py.def("get_types", &DQ_SerialManipulatorDH::get_types, "Retrieves the vector of types."); 44 | 45 | //Overrides from DQ_SerialManipulator 46 | dqserialmanipulatordh_py.def("raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDH::raw_pose_jacobian, "Retrieves the raw pose Jacobian."); 47 | dqserialmanipulatordh_py.def("raw_fkm", (DQ (DQ_SerialManipulatorDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDH::raw_fkm, "Retrieves the raw FKM."); 48 | dqserialmanipulatordh_py.def("raw_pose_jacobian_derivative",(MatrixXd (DQ_SerialManipulatorDH::*)(const VectorXd&, const VectorXd&, const int&) const) 49 | &DQ_SerialManipulatorDH::raw_pose_jacobian_derivative, "Retrieves the raw pose Jacobian derivative."); 50 | } 51 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulatorDenso_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2021 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_SerialManipulatorDenso_py(py::module& m) 26 | { 27 | /*************************************************** 28 | * DQ SerialManipulatorDenso 29 | * **************************************************/ 30 | py::class_< 31 | DQ_SerialManipulatorDenso, 32 | std::shared_ptr, 33 | DQ_SerialManipulator> dqserialmanipulatordh_py(m, "DQ_SerialManipulatorDenso"); 34 | dqserialmanipulatordh_py.def(py::init()); 35 | 36 | ///Methods 37 | //Concrete 38 | dqserialmanipulatordh_py.def("get_as", &DQ_SerialManipulatorDenso::get_as, "Retrieves the vector of as."); 39 | dqserialmanipulatordh_py.def("get_bs", &DQ_SerialManipulatorDenso::get_bs, "Retrieves the vector of bs."); 40 | dqserialmanipulatordh_py.def("get_ds", &DQ_SerialManipulatorDenso::get_ds, "Retrieves the vector of ds."); 41 | 42 | dqserialmanipulatordh_py.def("get_alphas", &DQ_SerialManipulatorDenso::get_alphas, "Retrieves the vector of alphas."); 43 | dqserialmanipulatordh_py.def("get_betas", &DQ_SerialManipulatorDenso::get_betas, "Retrieves the vector of betas."); 44 | dqserialmanipulatordh_py.def("get_thetas", &DQ_SerialManipulatorDenso::get_gammas, "Retrieves the vector of gammas."); 45 | 46 | //Overrides from DQ_SerialManipulator 47 | dqserialmanipulatordh_py.def("raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDenso::raw_pose_jacobian, "Retrieves the raw pose Jacobian."); 48 | dqserialmanipulatordh_py.def("raw_fkm", (DQ (DQ_SerialManipulatorDenso::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorDenso::raw_fkm, "Retrieves the raw FKM."); 49 | dqserialmanipulatordh_py.def("raw_pose_jacobian_derivative",(MatrixXd (DQ_SerialManipulatorDenso::*)(const VectorXd&, const VectorXd&, const int&) const) 50 | &DQ_SerialManipulatorDenso::raw_pose_jacobian_derivative, "Retrieves the raw pose Jacobian derivative."); 51 | } 52 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulatorMDH_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Juan Jose Quiroz Omana - juanjqo@g.ecc.u-tokyo.ac.jp 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_SerialManipulatorMDH_py(py::module& m) 26 | { 27 | /*************************************************** 28 | * DQ SerialManipulatorMDH 29 | * **************************************************/ 30 | py::class_< 31 | DQ_SerialManipulatorMDH, 32 | std::shared_ptr, 33 | DQ_SerialManipulator 34 | > dqserialmanipulatormdh_py(m, "DQ_SerialManipulatorMDH"); 35 | dqserialmanipulatormdh_py.def(py::init()); 36 | 37 | ///Methods 38 | //Concrete 39 | dqserialmanipulatormdh_py.def("get_thetas", &DQ_SerialManipulatorMDH::get_thetas, "Retrieves the vector of thetas."); 40 | dqserialmanipulatormdh_py.def("get_ds", &DQ_SerialManipulatorMDH::get_ds, "Retrieves the vector of ds."); 41 | dqserialmanipulatormdh_py.def("get_as", &DQ_SerialManipulatorMDH::get_as, "Retrieves the vector of as."); 42 | dqserialmanipulatormdh_py.def("get_alphas", &DQ_SerialManipulatorMDH::get_alphas, "Retrieves the vector of alphas."); 43 | dqserialmanipulatormdh_py.def("get_types", &DQ_SerialManipulatorMDH::get_types, "Retrieves the vector of types."); 44 | 45 | //Overrides from DQ_SerialManipulator 46 | dqserialmanipulatormdh_py.def("raw_pose_jacobian", (MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorMDH::raw_pose_jacobian, "Retrieves the raw pose Jacobian."); 47 | dqserialmanipulatormdh_py.def("raw_fkm", (DQ (DQ_SerialManipulatorMDH::*)(const VectorXd&, const int&) const)&DQ_SerialManipulatorMDH::raw_fkm, "Retrieves the raw FKM."); 48 | dqserialmanipulatormdh_py.def("raw_pose_jacobian_derivative",(MatrixXd (DQ_SerialManipulatorMDH::*)(const VectorXd&, const VectorXd&, const int&) const) 49 | &DQ_SerialManipulatorMDH::raw_pose_jacobian_derivative, "Retrieves the raw pose Jacobian derivative."); 50 | } 51 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulator_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_SerialManipulator_py(py::module& m) 26 | { 27 | /*************************************************** 28 | * DQ SerialManipulator 29 | * **************************************************/ 30 | py::class_< 31 | DQ_SerialManipulator, 32 | std::shared_ptr, 33 | DQ_Kinematics 34 | > dqserialmanipulator_py(m, "DQ_SerialManipulator"); 35 | //dqserialmanipulator_py.def(py::init()); 36 | 37 | ///Methods 38 | //Concrete 39 | dqserialmanipulator_py.def("get_effector", &DQ_SerialManipulator::get_effector,"Retrieves the effector."); 40 | dqserialmanipulator_py.def("set_effector", &DQ_SerialManipulator::set_effector,"Sets the effector."); 41 | dqserialmanipulator_py.def("get_lower_q_limit", &DQ_SerialManipulator::get_lower_q_limit,"Retrieves the lower limit for the joint values."); 42 | dqserialmanipulator_py.def("set_lower_q_limit", &DQ_SerialManipulator::set_lower_q_limit,"Sets the lower limit for the joint values."); 43 | dqserialmanipulator_py.def("get_lower_q_dot_limit", &DQ_SerialManipulator::get_lower_q_dot_limit,"Retrieves the lower limit for the joint velocities."); 44 | dqserialmanipulator_py.def("set_lower_q_dot_limit", &DQ_SerialManipulator::set_lower_q_dot_limit,"Sets the lower limit for the joint velocities."); 45 | dqserialmanipulator_py.def("get_upper_q_limit", &DQ_SerialManipulator::get_upper_q_limit,"Retrieves the upper limit for the joint values."); 46 | dqserialmanipulator_py.def("set_upper_q_limit", &DQ_SerialManipulator::set_upper_q_limit,"Sets the upper limit for the joint values."); 47 | dqserialmanipulator_py.def("get_upper_q_dot_limit", &DQ_SerialManipulator::get_upper_q_dot_limit,"Retrieves the upper limit for the joint velocities."); 48 | dqserialmanipulator_py.def("set_upper_q_dot_limit", &DQ_SerialManipulator::set_upper_q_dot_limit,"Sets the upper limit for the joint velocities."); 49 | 50 | //Virtual 51 | dqserialmanipulator_py.def("raw_fkm", (DQ (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::raw_fkm,"Gets the raw fkm."); 52 | dqserialmanipulator_py.def("raw_pose_jacobian", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::raw_pose_jacobian,"Returns the pose Jacobian without base or effector transformation"); 53 | dqserialmanipulator_py.def("raw_pose_jacobian_derivative",(MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const VectorXd&) const) 54 | &DQ_SerialManipulator::raw_pose_jacobian_derivative, 55 | "Returns the pose Jacobian derivative without base or effector transformation"); 56 | 57 | //Overrides from DQ_Kinematics 58 | dqserialmanipulator_py.def("fkm", (DQ (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::fkm,"Gets the fkm."); 59 | dqserialmanipulator_py.def("fkm", (DQ (DQ_SerialManipulator::*)(const VectorXd&,const int&) const)&DQ_SerialManipulator::fkm,"Gets the fkm."); 60 | 61 | dqserialmanipulator_py.def("get_dim_configuration_space", &DQ_SerialManipulator::get_dim_configuration_space,"Retrieves the number of links."); 62 | 63 | dqserialmanipulator_py.def("pose_jacobian", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const int&) const)&DQ_SerialManipulator::pose_jacobian,"Returns the pose Jacobian"); 64 | dqserialmanipulator_py.def("pose_jacobian", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&) const)&DQ_SerialManipulator::pose_jacobian,"Returns the pose Jacobian"); 65 | dqserialmanipulator_py.def("pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const VectorXd&, const int&) const) 66 | &DQ_SerialManipulator::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 67 | dqserialmanipulator_py.def("pose_jacobian_derivative", (MatrixXd (DQ_SerialManipulator::*)(const VectorXd&, const VectorXd&) const) 68 | &DQ_SerialManipulator::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 69 | } 70 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialWholeBody_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020-2023 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 21 | - Responsible for the original implementation. 22 | 23 | 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) 24 | - Added the two raw_fkm methods to fix issue 54 (https://github.com/dqrobotics/python/issues/54) 25 | */ 26 | 27 | #include "../dqrobotics_module.h" 28 | 29 | void init_DQ_SerialWholeBody_py(py::module& m) 30 | { 31 | /***************************************************** 32 | * DQ WholeBody 33 | * **************************************************/ 34 | py::class_< 35 | DQ_SerialWholeBody, 36 | std::shared_ptr, 37 | DQ_Kinematics 38 | > dqserialwholebody_py(m,"DQ_SerialWholeBody"); 39 | dqserialwholebody_py.def(py::init>()); 40 | dqserialwholebody_py.def("add",&DQ_SerialWholeBody::add,"Adds a DQ_Kinematics pointer to the kinematic chain."); 41 | dqserialwholebody_py.def("fkm",(DQ (DQ_SerialWholeBody::*)(const VectorXd&) const)&DQ_SerialWholeBody::fkm,"Gets the fkm."); 42 | dqserialwholebody_py.def("fkm",(DQ (DQ_SerialWholeBody::*)(const VectorXd&,const int&) const)&DQ_SerialWholeBody::fkm,"Gets the fkm."); 43 | dqserialwholebody_py.def("raw_fkm",(DQ (DQ_SerialWholeBody::*)(const VectorXd&) const)&DQ_SerialWholeBody::raw_fkm,"Gets the fkm but without considering base and end-effector changes."); 44 | dqserialwholebody_py.def("raw_fkm",(DQ (DQ_SerialWholeBody::*)(const VectorXd&, const int&) const)&DQ_SerialWholeBody::raw_fkm,"Gets the fkm but without considering base and end-effector changes."); 45 | dqserialwholebody_py.def("get_dim_configuration_space",&DQ_SerialWholeBody::get_dim_configuration_space,"Gets the dimention of the configuration space"); 46 | dqserialwholebody_py.def("get_chain",&DQ_SerialWholeBody::get_chain, "Returns the DQ_Kinematics at a given index of the chain"); 47 | dqserialwholebody_py.def("get_chain_as_serial_manipulator_dh",&DQ_SerialWholeBody::get_chain_as_serial_manipulator_dh, "Returns the DQ_SerialManipulatorDH at a given index of the chain"); 48 | dqserialwholebody_py.def("get_chain_as_holonomic_base",&DQ_SerialWholeBody::get_chain_as_holonomic_base, "Returns the DQ_HolonomicBase at a given index of the chain"); 49 | dqserialwholebody_py.def("pose_jacobian",(MatrixXd (DQ_SerialWholeBody::*)(const VectorXd&, const int&) const)&DQ_SerialWholeBody::pose_jacobian,"Returns the pose Jacobian"); 50 | dqserialwholebody_py.def("pose_jacobian",(MatrixXd (DQ_SerialWholeBody::*)(const VectorXd&) const)&DQ_SerialWholeBody::pose_jacobian,"Returns the pose Jacobian"); 51 | dqserialwholebody_py.def("pose_jacobian_derivative",(MatrixXd (DQ_SerialWholeBody::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_SerialWholeBody::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 52 | dqserialwholebody_py.def("pose_jacobian_derivative",(MatrixXd (DQ_SerialWholeBody::*)(const VectorXd&, const VectorXd&) const)&DQ_SerialWholeBody::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 53 | } 54 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_WholeBody_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_WholeBody_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ WholeBody 29 | * **************************************************/ 30 | py::class_< 31 | DQ_WholeBody, 32 | std::shared_ptr, 33 | DQ_Kinematics 34 | > dqwholebody_py(m,"DQ_WholeBody"); 35 | dqwholebody_py.def(py::init>()); 36 | dqwholebody_py.def("add",&DQ_WholeBody::add,"Adds a DQ_Kinematics pointer to the kinematic chain."); 37 | dqwholebody_py.def("fkm",(DQ (DQ_WholeBody::*)(const VectorXd&) const)&DQ_WholeBody::fkm,"Gets the fkm."); 38 | dqwholebody_py.def("fkm",(DQ (DQ_WholeBody::*)(const VectorXd&,const int&) const)&DQ_WholeBody::fkm,"Gets the fkm."); 39 | dqwholebody_py.def("get_dim_configuration_space",&DQ_WholeBody::get_dim_configuration_space,"Gets the dimention of the configuration space"); 40 | dqwholebody_py.def("get_chain",&DQ_WholeBody::get_chain, "Returns the DQ_Kinematics at a given index of the chain"); 41 | dqwholebody_py.def("get_chain_as_serial_manipulator_dh",&DQ_WholeBody::get_chain_as_serial_manipulator_dh, "Returns the DQ_SerialManipulatorDH at a given index of the chain"); 42 | dqwholebody_py.def("get_chain_as_holonomic_base",&DQ_WholeBody::get_chain_as_holonomic_base, "Returns the DQ_HolonomicBase at a given index of the chain"); 43 | dqwholebody_py.def("pose_jacobian",(MatrixXd (DQ_WholeBody::*)(const VectorXd&, const int&) const)&DQ_WholeBody::pose_jacobian,"Returns the pose Jacobian"); 44 | dqwholebody_py.def("pose_jacobian",(MatrixXd (DQ_WholeBody::*)(const VectorXd&) const)&DQ_WholeBody::pose_jacobian,"Returns the pose Jacobian"); 45 | dqwholebody_py.def("pose_jacobian_derivative",(MatrixXd (DQ_WholeBody::*)(const VectorXd&, const VectorXd&, const int&) const)&DQ_WholeBody::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 46 | dqwholebody_py.def("pose_jacobian_derivative",(MatrixXd (DQ_WholeBody::*)(const VectorXd&, const VectorXd&) const)&DQ_WholeBody::pose_jacobian_derivative,"Returns the pose Jacobian derivative"); 47 | } 48 | -------------------------------------------------------------------------------- /src/solvers/DQ_QuadraticProgrammingSolver_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | //https://pybind11.readthedocs.io/en/stable/advanced/classes.html 26 | //Trampoline class 27 | class DQ_QuadraticProgrammingSolverPy : public DQ_QuadraticProgrammingSolver 28 | { 29 | protected: 30 | /* Inherit the constructors */ 31 | using DQ_QuadraticProgrammingSolver::DQ_QuadraticProgrammingSolver; 32 | public: 33 | /* Trampoline (need one for each virtual function) */ 34 | VectorXd solve_quadratic_program(const MatrixXd &H, const VectorXd &f, const MatrixXd& A, const VectorXd &b, const MatrixXd &Aeq, const VectorXd &beq) override 35 | { 36 | PYBIND11_OVERLOAD_PURE( 37 | VectorXd, /* Return type */ 38 | DQ_QuadraticProgrammingSolver, /* Parent class */ 39 | solve_quadratic_program, /* Name of function in C++ (must match Python name) */ 40 | H, f, A, b, Aeq, beq /* Argument(s) */ 41 | ) 42 | } 43 | }; 44 | 45 | void init_DQ_QuadraticProgrammingSolver_py(py::module& m) 46 | { 47 | /***************************************************** 48 | * DQ_QuadraticProgrammingSolver 49 | * **************************************************/ 50 | py::class_< 51 | DQ_QuadraticProgrammingSolver, 52 | std::shared_ptr, 53 | DQ_QuadraticProgrammingSolverPy 54 | > dqquadraticprogrammingsolver_py(m,"DQ_QuadraticProgrammingSolver"); 55 | dqquadraticprogrammingsolver_py.def(py::init<>()); 56 | dqquadraticprogrammingsolver_py.def("solve_quadratic_program", &DQ_QuadraticProgrammingSolver::solve_quadratic_program, "Solves a quadratic program"); 57 | } 58 | -------------------------------------------------------------------------------- /src/utils/DQ_Geometry_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_Geometry_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ_Geometry 29 | * **************************************************/ 30 | //#include 31 | py::class_ geometry_py(m, "DQ_Geometry"); 32 | geometry_py.def_static("point_to_point_squared_distance", &DQ_Geometry::point_to_point_squared_distance, "Returns the squared distance between two points"); 33 | geometry_py.def_static("point_to_line_squared_distance", &DQ_Geometry::point_to_line_squared_distance, "Returns the squared distance between a point and a line"); 34 | geometry_py.def_static("point_to_plane_distance", &DQ_Geometry::point_to_plane_distance, "Returns the distance between a point and a plane"); 35 | geometry_py.def_static("line_to_line_squared_distance", &DQ_Geometry::line_to_line_squared_distance, "Returns the squared distance between two lines"); 36 | geometry_py.def_static("line_to_line_angle", &DQ_Geometry::line_to_line_angle, "Returns the angle between two lines"); 37 | geometry_py.def_static("point_projected_in_line", &DQ_Geometry::point_projected_in_line, "Returns the point projected in a line."); 38 | geometry_py.def_static("closest_points_between_lines", &DQ_Geometry::closest_points_between_lines, "Returns the closest points between lines"); 39 | geometry_py.def_static("closest_points_between_line_segments", &DQ_Geometry::closest_points_between_line_segments, "Returns the closes points between line segments"); 40 | geometry_py.def_static("line_segment_to_line_segment_squared_distance", &DQ_Geometry::line_segment_to_line_segment_squared_distance, "Returns the squared distance between two line segments."); 41 | 42 | geometry_py.def_static("is_line_segment",&DQ_Geometry::is_line_segment,"Verifies if the inputs constitute a valid line segment."); 43 | //Overload with the default threshold 44 | geometry_py.def_static("is_line_segment", 45 | [](const DQ& a1, const DQ& a2, const DQ& a3) 46 | { 47 | return DQ_Geometry::is_line_segment(a1,a2,a3); 48 | }, 49 | "Verifies if the inputs constitute a valid line segment."); 50 | } 51 | 52 | -------------------------------------------------------------------------------- /src/utils/DQ_LinearAlgebra_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_LinearAlgebra_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ_LinearAlgebra 29 | * **************************************************/ 30 | //#include 31 | py::module linearalgebra_py = m.def_submodule("_DQ_LinearAlgebra","A submodule of utils"); 32 | linearalgebra_py.def("pinv", (MatrixXd (*) (const MatrixXd&))&DQ_robotics::pinv, "Retrieves the pseudo-inverse of the input matrix"); 33 | } 34 | -------------------------------------------------------------------------------- /src/utils/DQ_Math_py.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020 DQ Robotics Developers 3 | 4 | This file is part of DQ Robotics. 5 | 6 | DQ Robotics is free software: you can redistribute it and/or modify 7 | it under the terms of the GNU Lesser General Public License as published by 8 | the Free Software Foundation, either version 3 of the License, or 9 | (at your option) any later version. 10 | 11 | DQ Robotics is distributed in the hope that it will be useful, 12 | but WITHOUT ANY WARRANTY; without even the implied warranty of 13 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 14 | GNU Lesser General Public License for more details. 15 | 16 | You should have received a copy of the GNU Lesser General Public License 17 | along with DQ Robotics. If not, see . 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include "../dqrobotics_module.h" 24 | 25 | void init_DQ_Math_py(py::module& m) 26 | { 27 | /***************************************************** 28 | * DQ_Math 29 | * **************************************************/ 30 | //#include 31 | py::module math_py = m.def_submodule("_DQ_Math","A submodule of utils"); 32 | math_py.def("deg2rad", static_cast(&DQ_robotics::deg2rad), "Converts from degrees to radians."); 33 | math_py.def("deg2rad", static_cast(&DQ_robotics::deg2rad), "Converts from degrees to radians."); 34 | math_py.def("rad2deg", static_cast(&DQ_robotics::rad2deg), "Converts from degrees to radians."); 35 | math_py.def("rad2deg", static_cast(&DQ_robotics::rad2deg), "Converts from degrees to radians."); 36 | } 37 | --------------------------------------------------------------------------------