├── .github ├── scripts │ ├── macos │ │ ├── install_prerequisites.sh │ │ ├── install.sh │ │ └── build.sh │ ├── ubuntu │ │ ├── install.sh │ │ ├── install_prerequisites.sh │ │ └── build.sh │ └── windows │ │ ├── install.ps1 │ │ ├── build.ps1 │ │ └── install_prerequisites.ps1 └── workflows │ ├── cpp_build_windows.yml │ ├── cpp_build_macos.yml │ └── cpp_build_ubuntu.yml ├── README.md ├── debian ├── control ├── copyright ├── rules └── changelog ├── include └── dqrobotics │ ├── utils │ ├── DQ_Constants.h │ ├── DQ_LinearAlgebra.h │ ├── DQ_Math.h │ └── DQ_Geometry.h │ ├── robots │ ├── KukaLw4Robot.h │ ├── FrankaEmikaPandaRobot.h │ ├── KukaYoubotRobot.h │ ├── ComauSmartSixRobot.h │ ├── Ax18ManipulatorRobot.h │ └── BarrettWamArmRobot.h │ ├── solvers │ └── DQ_QuadraticProgrammingSolver.h │ ├── robot_control │ ├── DQ_PseudoinverseController.h │ ├── DQ_ClassicQPController.h │ ├── DQ_KinematicConstrainedController.h │ ├── DQ_QuadraticProgrammingController.h │ ├── DQ_NumericalFilteredPseudoInverseController.h │ └── DQ_KinematicController.h │ ├── robot_modeling │ ├── DQ_MobileBase.h │ ├── DQ_CooperativeDualTaskSpace.h │ ├── DQ_DifferentialDriveRobot.h │ ├── DQ_HolonomicBase.h │ ├── DQ_SerialManipulatorDenso.h │ ├── DQ_WholeBody.h │ ├── DQ_SerialManipulatorDH.h │ ├── DQ_SerialManipulatorMDH.h │ ├── DQ_SerialWholeBody.h │ ├── DQ_ParameterDH.h │ ├── DQ_SerialManipulator.h │ ├── DQ_JointType.h │ └── DQ_Kinematics.h │ └── internal │ └── _dq_linesegment.h ├── src ├── robot_modeling │ ├── DQ_MobileBase.cpp │ ├── DQ_CooperativeDualTaskSpace.cpp │ ├── DQ_DifferentialDriveRobot.cpp │ ├── DQ_SerialManipulatorDenso.cpp │ ├── DQ_HolonomicBase.cpp │ ├── DQ_WholeBody.cpp │ ├── DQ_SerialManipulator.cpp │ └── DQ_SerialWholeBody.cpp ├── robots │ ├── Ax18ManipulatorRobot.cpp │ ├── BarrettWamArmRobot.cpp │ ├── KukaLw4Robot.cpp │ ├── ComauSmartSixRobot.cpp │ ├── KukaYoubotRobot.cpp │ └── FrankaEmikaPandaRobot.cpp ├── utils │ ├── DQ_Math.cpp │ └── DQ_LinearAlgebra.cpp ├── robot_control │ ├── DQ_KinematicConstrainedController.cpp │ ├── DQ_ClassicQPController.cpp │ ├── DQ_PseudoinverseController.cpp │ ├── DQ_QuadraticProgrammingController.cpp │ ├── DQ_NumericalFilteredPseudoInverseController.cpp │ └── DQ_KinematicController.cpp └── internal │ └── _dq_linesegment.cpp ├── CMakeLists.txt └── LICENSE /.github/scripts/macos/install_prerequisites.sh: -------------------------------------------------------------------------------- 1 | brew install eigen 2 | -------------------------------------------------------------------------------- /.github/scripts/macos/install.sh: -------------------------------------------------------------------------------- 1 | cd build 2 | sudo make install 3 | cd .. 4 | -------------------------------------------------------------------------------- /.github/scripts/ubuntu/install.sh: -------------------------------------------------------------------------------- 1 | sudo apt install ../libdqrobotics*.deb 2 | -------------------------------------------------------------------------------- /.github/scripts/windows/install.ps1: -------------------------------------------------------------------------------- 1 | cd build 2 | cmake --install . 3 | cd .. 4 | -------------------------------------------------------------------------------- /.github/scripts/macos/build.sh: -------------------------------------------------------------------------------- 1 | mkdir build 2 | cd build 3 | cmake .. 4 | make 5 | cd .. 6 | -------------------------------------------------------------------------------- /.github/scripts/ubuntu/install_prerequisites.sh: -------------------------------------------------------------------------------- 1 | sudo apt install cmake g++ libeigen3-dev fakeroot 2 | -------------------------------------------------------------------------------- /.github/scripts/ubuntu/build.sh: -------------------------------------------------------------------------------- 1 | chmod +x debian/rules 2 | fakeroot debian/rules clean 3 | fakeroot debian/rules build 4 | fakeroot debian/rules binary 5 | -------------------------------------------------------------------------------- /.github/scripts/windows/build.ps1: -------------------------------------------------------------------------------- 1 | mkdir build 2 | cd build 3 | cmake -DCMAKE_TOOLCHAIN_FILE=C:/vcpkg/scripts/buildsystems/vcpkg.cmake .. 4 | #Not used, but good to keep in mind -DCMAKE_INSTALL_PREFIX="$ENV:UserProfile\dqrobotics" .. 5 | cmake --build . --config Release 6 | cd .. 7 | -------------------------------------------------------------------------------- /.github/scripts/windows/install_prerequisites.ps1: -------------------------------------------------------------------------------- 1 | echo "Setting up vcpkg..." 2 | cd C:\vcpkg 3 | .\bootstrap-vcpkg.bat 4 | vcpkg integrate install 5 | echo "Installing eigen3..." 6 | .\vcpkg install eigen3:x64-windows 7 | echo "Adding symlink to vcpkg..." 8 | cmd /c mklink /d c:\Tools\vcpkg c:\vcpkg 9 | cd ~ 10 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # dqrobotics-cpp [![CPP Build and Test](https://github.com/dqrobotics/cpp/actions/workflows/cpp_build_and_test.yml/badge.svg)](https://github.com/dqrobotics/cpp/actions/workflows/cpp_build_and_test.yml) 2 | The DQ Robotics library in C++ 3 | 4 | Refer to the [docs](https://dqroboticsgithubio.readthedocs.io/en/latest/installation/cpp.html) 5 | -------------------------------------------------------------------------------- /debian/control: -------------------------------------------------------------------------------- 1 | Source: libdqrobotics 2 | Section: devel 3 | Priority: optional 4 | Maintainer: Murilo Marinho 5 | Build-Depends: cmake, libeigen3-dev 6 | Homepage: https://dqrobotics.github.io/ 7 | 8 | Package: libdqrobotics 9 | Architecture: any 10 | Depends: 11 | Description: DQ Robotics 12 | The dual quaternion robotics library. 13 | -------------------------------------------------------------------------------- /debian/copyright: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2017 DQ Robotics Developers 3 | * 4 | * This program is free software: you can redistribute it and/or modify 5 | * it under the terms of the GNU Lesser General Public License as 6 | * published by the Free Software Foundation, version 3. 7 | * 8 | * This program is distributed in the hope that it will be useful, but 9 | * WITHOUT ANY WARRANTY; without even the implied warranty of 10 | * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU 11 | * Lesser General Lesser Public License for more details. 12 | * 13 | * You should have received a copy of the GNU Lesser General Public License 14 | * along with this program. If not, see . 15 | */ 16 | -------------------------------------------------------------------------------- /debian/rules: -------------------------------------------------------------------------------- 1 | #!/usr/bin/make -f 2 | 3 | BUILDDIR = build_dir 4 | 5 | # secondly called by launchpad 6 | build: 7 | mkdir $(BUILDDIR); 8 | cd $(BUILDDIR); cmake -DCMAKE_CXX_FLAGS="-fdebug-prefix-map=$(shell pwd)/src/=/usr/src/dqrobotics/ -fdebug-prefix-map=$(shell pwd)/include/=/usr/include/" -DCMAKE_BUILD_TYPE=RelWithDebInfo -DCMAKE_INSTALL_PREFIX=../debian/tmp/usr .. 9 | make -C $(BUILDDIR) 10 | #$(info ************ $(shell pwd) ************) 11 | 12 | # thirdly called by launchpad 13 | binary: binary-indep binary-arch 14 | 15 | binary-indep: 16 | # nothing to be done 17 | 18 | binary-arch: 19 | cd $(BUILDDIR); cmake -P cmake_install.cmake 20 | mkdir -p debian/tmp/DEBIAN 21 | dpkg-gencontrol -plibdqrobotics 22 | dpkg --build debian/tmp .. 23 | 24 | # firstly called by launchpad 25 | clean: 26 | rm -f build 27 | rm -rf $(BUILDDIR) 28 | 29 | .PHONY: binary binary-arch binary-indep clean 30 | -------------------------------------------------------------------------------- /.github/workflows/cpp_build_windows.yml: -------------------------------------------------------------------------------- 1 | name: CPP Build Windows 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: windows-latest 8 | continue-on-error: true 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | testbranches: ['release','master'] 13 | 14 | steps: 15 | - uses: actions/checkout@v2 16 | 17 | - name: Install prerequisites 18 | run: .github\scripts\windows\install_prerequisites.ps1 19 | 20 | - name: Build 21 | run: .github\scripts\windows\build.ps1 22 | 23 | - name: Install 24 | run: .github\scripts\windows\install.ps1 25 | 26 | - name: Download examples ((${{ matrix.testbranches }})) 27 | run: git clone -b ${{ matrix.testbranches }} https://github.com/dqrobotics/cpp-examples.git 28 | 29 | - name: Build pure examples 30 | run: | 31 | cd cpp-examples 32 | .\build_pure.ps1 33 | -------------------------------------------------------------------------------- /.github/workflows/cpp_build_macos.yml: -------------------------------------------------------------------------------- 1 | name: CPP Build MacOS 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: macos-latest 8 | continue-on-error: true 9 | strategy: 10 | fail-fast: false 11 | matrix: 12 | testbranches: ['release','master'] 13 | 14 | steps: 15 | - uses: actions/checkout@v2 16 | 17 | - name: Install prerequisites 18 | run: sh .github/scripts/macos/install_prerequisites.sh 19 | 20 | - name: Build 21 | run: sh .github/scripts/macos/build.sh 22 | 23 | - name: Install 24 | run: sh .github/scripts/macos/install.sh 25 | 26 | - name: Download examples ((${{ matrix.testbranches }})) 27 | run: git clone -b ${{ matrix.testbranches }} https://github.com/dqrobotics/cpp-examples.git 28 | 29 | - name: Build pure examples 30 | run: | 31 | cd cpp-examples 32 | chmod +x .build_pure.sh 33 | ./.build_pure.sh 34 | 35 | -------------------------------------------------------------------------------- /include/dqrobotics/utils/DQ_Constants.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 26 | namespace DQ_robotics 27 | { 28 | 29 | constexpr double pi = M_PI; 30 | 31 | } 32 | -------------------------------------------------------------------------------- /.github/workflows/cpp_build_ubuntu.yml: -------------------------------------------------------------------------------- 1 | name: CPP Build Ubuntu 2 | 3 | on: [push, pull_request] 4 | 5 | jobs: 6 | build: 7 | runs-on: ubuntu-latest 8 | strategy: 9 | fail-fast: false 10 | matrix: 11 | testbranches: ['release','master'] 12 | 13 | steps: 14 | - uses: actions/checkout@v2 15 | 16 | - name: apt update 17 | run: sudo apt update 18 | 19 | - name: Install pre-requisites 20 | run: sh .github/scripts/ubuntu/install_prerequisites.sh 21 | 22 | - name: Build 23 | run: sh .github/scripts/ubuntu/build.sh 24 | 25 | - name: Install 26 | run: sh .github/scripts/ubuntu/install.sh 27 | 28 | - name: Download examples ((${{ matrix.testbranches }})) 29 | run: git clone -b ${{ matrix.testbranches }} https://github.com/dqrobotics/cpp-examples.git 30 | 31 | - name: Build pure examples 32 | run: | 33 | cd cpp-examples 34 | chmod +x .build_pure.sh 35 | ./.build_pure.sh 36 | 37 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/KukaLw4Robot.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTS_KUKALW4ROBOT_H 24 | #define DQ_ROBOTS_KUKALW4ROBOT_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class KukaLw4Robot 32 | { 33 | public: 34 | static DQ_SerialManipulatorDH kinematics(); 35 | }; 36 | 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/FrankaEmikaPandaRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2022 DQ Robotics Developers 3 | This file is part of DQ Robotics. 4 | DQ Robotics is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU Lesser General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | DQ Robotics is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public License 13 | along with DQ Robotics. If not, see . 14 | Contributors: 15 | - Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) 16 | */ 17 | 18 | #pragma once 19 | #include 20 | 21 | namespace DQ_robotics 22 | { 23 | 24 | class FrankaEmikaPandaRobot 25 | { 26 | public: 27 | static DQ_SerialManipulatorMDH kinematics(); 28 | //static DQ_SerialManipulatorMDH dynamics(); To be implemented 29 | }; 30 | 31 | } 32 | 33 | 34 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/KukaYoubotRobot.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTS_KUKAYOUBOTROBOT_H 24 | #define DQ_ROBOTS_KUKAYOUBOTROBOT_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class KukaYoubotRobot 32 | { 33 | public: 34 | static DQ_SerialWholeBody kinematics(); 35 | }; 36 | 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /include/dqrobotics/utils/DQ_LinearAlgebra.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 27 | using namespace Eigen; 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | int rank(const MatrixXd& matrix); 33 | 34 | MatrixXd pinv(const MatrixXd& matrix); 35 | 36 | std::tuple svd(const MatrixXd& matrix); 37 | 38 | } 39 | 40 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/ComauSmartSixRobot.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTS_COMAUSMARTSIXROBOT_H 24 | #define DQ_ROBOTS_COMAUSMARTSIXROBOT_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class ComauSmartSixRobot 32 | { 33 | public: 34 | static DQ_SerialManipulatorDH kinematics(); 35 | }; 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/Ax18ManipulatorRobot.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTS_AX18MANIPULATORROBOT_H 24 | #define DQ_ROBOTS_AX18MANIPULATORROBOT_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics{ 29 | 30 | class Ax18ManipulatorRobot 31 | { 32 | public: 33 | static DQ_SerialManipulatorDH kinematics(); 34 | }; 35 | 36 | } 37 | 38 | #endif 39 | -------------------------------------------------------------------------------- /include/dqrobotics/robots/BarrettWamArmRobot.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTS_BARRETTWAMARMROBOT_H 24 | #define DQ_ROBOTS_BARRETTWAMARMROBOT_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class BarrettWamArmRobot 32 | { 33 | public: 34 | static DQ_SerialManipulatorDH kinematics(); 35 | }; 36 | 37 | } 38 | 39 | #endif 40 | -------------------------------------------------------------------------------- /debian/changelog: -------------------------------------------------------------------------------- 1 | libdqrobotics (20.04.0) bionic; urgency=medium 2 | 3 | * DQ: Improved to_string() to be more similar to MATLAB. 4 | * DQ_SerialMainpulator: removed support for dummy joints. 5 | * Added DQ_SerialWholeBody. This class represents the serial connection of robots. 6 | * Added DQ_SerialManipulatorDH. This class represents serial robots modeled using DH parameters and can consider prismatic joints. 7 | * DQ_Robots: changed most serial manipulator robots to use DQ_SerialManipulatorDH instead of DQ_SerialManipulator. Changed KukaYoubotRobot to use DQ_SerialWholeBody instead of DQ_WholeBody. 8 | 9 | -- Murilo Wed, 06 May 2020 19:24:42 -0700 10 | 11 | libdqrobotics (19.10.1) bionic; urgency=medium 12 | 13 | * DQ_KinematicController: Added DistanceToPlane, stability_counter_ and related methods, and changed is_stable() to system_reached_stable_region() 14 | 15 | -- Murilo Thu, 13 Feb 2020 23:19:17 -0800 16 | 17 | libdqrobotics (19.10.0) bionic; urgency=medium 18 | 19 | * Initial 19.10.0 release 20 | 21 | -- Murilo Thu, 13 Feb 2020 23:17:48 -0800 22 | 23 | libdqrobotics (0.1-0ppa0) oneiric; urgency=low 24 | 25 | * Initial upload! 26 | 27 | -- Murilo Marinho Sun, 03 Sep 2017 16:45:26 +0900 28 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_MobileBase.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 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_MobileBase::DQ_MobileBase() 29 | { 30 | frame_displacement_ = DQ(1); 31 | } 32 | 33 | DQ DQ_MobileBase::frame_displacement() 34 | { 35 | return frame_displacement_; 36 | } 37 | 38 | void DQ_MobileBase::set_frame_displacement(const DQ &pose) 39 | { 40 | frame_displacement_ = pose; 41 | } 42 | 43 | } 44 | -------------------------------------------------------------------------------- /include/dqrobotics/utils/DQ_Math.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2020-2022 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 | 27 | namespace DQ_robotics 28 | { 29 | 30 | constexpr double deg2rad(const double& a) noexcept 31 | { 32 | return (a)*pi/(180.0); 33 | } 34 | 35 | VectorXd deg2rad(const VectorXd& v); 36 | 37 | constexpr double rad2deg(const double& a) noexcept 38 | { 39 | return (a)*180.0/(pi); 40 | } 41 | 42 | VectorXd rad2deg(const VectorXd& v); 43 | 44 | } 45 | -------------------------------------------------------------------------------- /include/dqrobotics/solvers/DQ_QuadraticProgrammingSolver.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 26 | using namespace Eigen; 27 | 28 | namespace DQ_robotics 29 | { 30 | class DQ_QuadraticProgrammingSolver 31 | { 32 | protected: 33 | DQ_QuadraticProgrammingSolver() = default; 34 | public: 35 | virtual ~DQ_QuadraticProgrammingSolver() = default; 36 | 37 | virtual VectorXd solve_quadratic_program(const MatrixXd& H, const VectorXd& f, const MatrixXd& A, const VectorXd& b, const MatrixXd& Aeq, const VectorXd& beq)=0; 38 | }; 39 | } 40 | 41 | -------------------------------------------------------------------------------- /src/robots/Ax18ManipulatorRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 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 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | #include 23 | #include 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_SerialManipulatorDH Ax18ManipulatorRobot::kinematics() 29 | { 30 | const double pi2 = pi/2.0; 31 | 32 | Matrix ax18_dh(5,5); 33 | ax18_dh << 0, 0, -pi2, -pi2, -pi2, 34 | 0.167, 0, 0, 0.1225, 0, 35 | 0, 0.159, 0.02225, 0 , 0.170, 36 | -pi2, 0, -pi2, -pi2, 0, 37 | 0, 0, 0, 0, 0; 38 | 39 | DQ_SerialManipulatorDH ax18(ax18_dh); 40 | 41 | return ax18; 42 | } 43 | 44 | } 45 | -------------------------------------------------------------------------------- /src/utils/DQ_Math.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020-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 | #include 23 | 24 | namespace DQ_robotics 25 | { 26 | 27 | 28 | /** 29 | * @brief deg2rad 30 | * @param v 31 | * @return 32 | */ 33 | VectorXd deg2rad(const VectorXd& v) 34 | { 35 | VectorXd ret(v); 36 | for(auto i=0;i. 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | namespace DQ_robotics 27 | { 28 | 29 | DQ_SerialManipulatorDH BarrettWamArmRobot::kinematics() 30 | { 31 | const double pi2 = pi/2.0; 32 | 33 | Matrix wam_dh(5,7); 34 | wam_dh << 0, 0, 0, 0, 0, 0, 0, 35 | 0, 0, 0.55, 0, 0.3, 0, 0.0609, 36 | 0, 0, 0.045, -0.045, 0, 0, 0, 37 | -pi2, pi2, -pi2, pi2, -pi2, pi2, 0, 38 | 0, 0, 0, 0, 0, 0, 0; 39 | 40 | DQ_SerialManipulatorDH wam(wam_dh); 41 | 42 | return wam; 43 | }; 44 | 45 | } 46 | -------------------------------------------------------------------------------- /src/robots/KukaLw4Robot.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 | #ifndef DQ_ROBOTICS_KUKKA_DH_H 24 | #define DQ_ROBOTICS_KUKKA_DH_H 25 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | DQ_SerialManipulatorDH KukaLw4Robot::kinematics() 33 | { 34 | const double pi2 = pi/2.0; 35 | 36 | Matrix kukka_dh(5,7); 37 | kukka_dh << 0, 0, 0, 0, 0, 0, 0, 38 | 0.310, 0, 0.4, 0, 0.39, 0, 0, 39 | 0, 0, 0, 0, 0, 0, 0, 40 | pi2, -pi2, -pi2, pi2, pi2, -pi2, 0, 41 | 0, 0, 0, 0, 0, 0, 0; 42 | DQ_SerialManipulatorDH kukka(kukka_dh); 43 | 44 | return kukka; 45 | } 46 | 47 | } 48 | 49 | #endif 50 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_PseudoinverseController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | #include 24 | #include 25 | 26 | namespace DQ_robotics 27 | { 28 | 29 | class DQ_PseudoinverseController: public DQ_KinematicController 30 | { 31 | public: 32 | DQ_PseudoinverseController() = delete; 33 | 34 | [[deprecated("Use the smart pointer version instead")]] 35 | DQ_PseudoinverseController(DQ_Kinematics* robot); 36 | DQ_PseudoinverseController(const std::shared_ptr& robot); 37 | 38 | VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) override; 39 | VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward) override; 40 | }; 41 | 42 | } 43 | -------------------------------------------------------------------------------- /src/robots/ComauSmartSixRobot.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 | #include 23 | #include 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_SerialManipulatorDH ComauSmartSixRobot::kinematics() 29 | { 30 | throw(std::runtime_error("ComauSmartSixRobot::kinematics() is not compatible with this version of DQRobotics")); 31 | 32 | const double pi2 = pi/2.0; 33 | 34 | Matrix comau_dh(5,7); 35 | comau_dh << 0, -pi2, pi2, 0, 0, 0, pi, 36 | -0.45, 0, 0, -0.64707, 0, -0.095, 0, 37 | 0, 0.150, 0.590, 0.13, 0, 0, 0, 38 | pi, pi2, pi, -pi2, -pi2, pi2, pi, 39 | 0, 0, 0, 0, 0, 0, 1; 40 | 41 | DQ_SerialManipulatorDH comau(comau_dh); 42 | 43 | return comau; 44 | }; 45 | 46 | } 47 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_ClassicQPController.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 | 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class DQ_ClassicQPController:public DQ_QuadraticProgrammingController 32 | { 33 | public: 34 | DQ_ClassicQPController() = delete; 35 | 36 | //Deprecated 37 | DQ_ClassicQPController(DQ_Kinematics* robot, DQ_QuadraticProgrammingSolver* solver); 38 | DQ_ClassicQPController(const std::shared_ptr& robot, 39 | const std::shared_ptr& solver); 40 | 41 | MatrixXd compute_objective_function_symmetric_matrix(const MatrixXd& J, const VectorXd&) override; 42 | VectorXd compute_objective_function_linear_component(const MatrixXd& J, const VectorXd& task_error) override; 43 | }; 44 | 45 | } 46 | -------------------------------------------------------------------------------- /src/robot_control/DQ_KinematicConstrainedController.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 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_KinematicConstrainedController::DQ_KinematicConstrainedController(DQ_Kinematics* robot):DQ_KinematicController(robot) 29 | { 30 | } 31 | 32 | DQ_KinematicConstrainedController::DQ_KinematicConstrainedController(const std::shared_ptr &robot): 33 | DQ_KinematicController(robot) 34 | { 35 | 36 | } 37 | 38 | void DQ_KinematicConstrainedController::set_equality_constraint(const MatrixXd &B, const VectorXd &b) 39 | { 40 | equality_constraint_matrix_ = B; 41 | equality_constraint_vector_ = b; 42 | } 43 | 44 | void DQ_KinematicConstrainedController::set_inequality_constraint(const MatrixXd &B, const VectorXd &b) 45 | { 46 | inequality_constraint_matrix_ = B; 47 | inequality_constraint_vector_ = b; 48 | } 49 | 50 | } 51 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_MobileBase.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTICS_ROBOT_MODELING_DQ_MOBILEBASE 24 | #define DQ_ROBOTICS_ROBOT_MODELING_DQ_MOBILEBASE 25 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | class DQ_MobileBase : public DQ_Kinematics 33 | { 34 | protected: 35 | DQ frame_displacement_; 36 | 37 | DQ_MobileBase(); 38 | public: 39 | virtual ~DQ_MobileBase() = default; 40 | 41 | //Abstract methods (Inherited from DQ_Kinematics) 42 | //virtual int get_dim_configuration_space() const = 0; 43 | //virtual DQ fkm(const VectorXd& joint_configurations) const = 0; 44 | //virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations,const int& to_link) const = 0; 45 | //virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_link) const = 0; 46 | 47 | void set_frame_displacement(const DQ& pose); 48 | DQ frame_displacement(); 49 | 50 | }; 51 | 52 | } 53 | 54 | #endif 55 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_CooperativeDualTaskSpace.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_COOPERATIVEDUALTASKSPACE_H 24 | #define DQ_COOPERATIVEDUALTASKSPACE_H 25 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | class DQ_CooperativeDualTaskSpace 33 | { 34 | private: 35 | DQ_Kinematics* robot1_; 36 | DQ_Kinematics* robot2_; 37 | 38 | public: 39 | //Remove default constructor 40 | DQ_CooperativeDualTaskSpace()=delete; 41 | 42 | DQ_CooperativeDualTaskSpace(DQ_Kinematics* robot1, DQ_Kinematics* robot2); 43 | 44 | DQ pose1(const VectorXd& theta); 45 | DQ pose2(const VectorXd& theta); 46 | 47 | MatrixXd pose_jacobian1(const VectorXd& theta); 48 | MatrixXd pose_jacobian2(const VectorXd& theta); 49 | 50 | DQ relative_pose(const VectorXd& theta); 51 | DQ absolute_pose(const VectorXd& theta); 52 | 53 | MatrixXd relative_pose_jacobian(const VectorXd& theta); 54 | MatrixXd absolute_pose_jacobian(const VectorXd& theta); 55 | 56 | }; 57 | 58 | 59 | } 60 | 61 | #endif 62 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_KinematicConstrainedController.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOT_CONTROL_DQ_KINEMATICCONSTRAINEDCONTROLLER_H 24 | #define DQ_ROBOT_CONTROL_DQ_KINEMATICCONSTRAINEDCONTROLLER_H 25 | 26 | #include 27 | 28 | #include 29 | 30 | using namespace Eigen; 31 | 32 | namespace DQ_robotics 33 | { 34 | 35 | class DQ_KinematicConstrainedController: public DQ_KinematicController 36 | { 37 | protected: 38 | MatrixXd equality_constraint_matrix_; 39 | VectorXd equality_constraint_vector_; 40 | MatrixXd inequality_constraint_matrix_; 41 | VectorXd inequality_constraint_vector_; 42 | 43 | [[deprecated("Use the smart pointer version instead")]] 44 | DQ_KinematicConstrainedController(DQ_Kinematics* robot); 45 | DQ_KinematicConstrainedController(const std::shared_ptr& robot); 46 | public: 47 | //Remove default constructor 48 | DQ_KinematicConstrainedController()=delete; 49 | 50 | virtual void set_equality_constraint(const MatrixXd& B, const VectorXd& b); 51 | virtual void set_inequality_constraint(const MatrixXd& B, const VectorXd& b); 52 | 53 | }; 54 | 55 | 56 | } 57 | 58 | 59 | #endif 60 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_DifferentialDriveRobot.h: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 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 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #ifndef DQ_ROBOTICS_ROBOT_MODELING_DQ_DIFFERENTIALDRIVEROBOT 24 | #define DQ_ROBOTICS_ROBOT_MODELING_DQ_DIFFERENTIALDRIVEROBOT 25 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | class DQ_DifferentialDriveRobot : public DQ_HolonomicBase 33 | { 34 | protected: 35 | double wheel_radius_; 36 | double distance_between_wheels_; 37 | public: 38 | DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels); 39 | 40 | MatrixXd constraint_jacobian(const double& phi) const; 41 | MatrixXd constraint_jacobian_derivative(const double& phi, const double& phi_dot) const; 42 | MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; 43 | MatrixXd pose_jacobian(const VectorXd &q) const override; 44 | MatrixXd pose_jacobian_derivative(const VectorXd& q, 45 | const VectorXd& q_dot, 46 | const int& to_link) const override; 47 | MatrixXd pose_jacobian_derivative (const VectorXd& q, 48 | const VectorXd& q_dot) const override; 49 | }; 50 | 51 | } 52 | 53 | #endif 54 | -------------------------------------------------------------------------------- /src/robots/KukaYoubotRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019-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 | #ifndef DQ_ROBOTICS_KUKAYOUBOT_DH_H 24 | #define DQ_ROBOTICS_KUKAYOUBOT_DH_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | namespace DQ_robotics 33 | { 34 | 35 | DQ_SerialWholeBody KukaYoubotRobot::kinematics() 36 | { 37 | const double pi2 = pi/2.0; 38 | MatrixXd arm_DH_matrix(5,5); 39 | arm_DH_matrix << 0, pi2, 0, pi2, 0, 40 | 0.147, 0, 0, 0, 0.218, 41 | 0, 0.155, 0.135, 0, 0, 42 | pi2, 0, 0, pi2, 0, 43 | 0, 0, 0, 0, 0; 44 | 45 | auto arm = std::make_shared(DQ_SerialManipulatorDH(arm_DH_matrix)); 46 | auto base = std::make_shared(DQ_HolonomicBase()); 47 | 48 | DQ x_bm= 1 + E_*0.5*(0.22575*i_ + 0.1441*k_); 49 | 50 | base->set_frame_displacement(x_bm); 51 | 52 | DQ_SerialWholeBody kin(std::static_pointer_cast(base)); 53 | kin.add(std::static_pointer_cast(arm)); 54 | 55 | return kin; 56 | } 57 | 58 | } 59 | 60 | #endif 61 | -------------------------------------------------------------------------------- /include/dqrobotics/internal/_dq_linesegment.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | #pragma message("_dq_linesegment.h is an internal DQRobotics header and its ABI/API is not stable.") 3 | /** 4 | (C) Copyright 2022 DQ Robotics Developers 5 | 6 | This file is part of DQ Robotics. 7 | 8 | DQ Robotics is free software: you can redistribute it and/or modify 9 | it under the terms of the GNU Lesser General Public License as published by 10 | the Free Software Foundation, either version 3 of the License, or 11 | (at your option) any later version. 12 | 13 | DQ Robotics is distributed in the hope that it will be useful, 14 | but WITHOUT ANY WARRANTY; without even the implied warranty of 15 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 16 | GNU Lesser General Public License for more details. 17 | 18 | You should have received a copy of the GNU Lesser General Public License 19 | along with DQ Robotics. If not, see . 20 | 21 | Contributors: 22 | - Murilo M. Marinho (murilomarinho@ieee.org) 23 | */ 24 | #include 25 | #include 26 | 27 | namespace DQ_robotics 28 | { 29 | 30 | 31 | namespace internal 32 | { 33 | 34 | class LineSegment 35 | { 36 | public: 37 | enum class Element{ 38 | Line,P1,P2 39 | }; 40 | 41 | using Primitives = std::tuple; 42 | using ClosestElements = std::tuple; 43 | using ClosestElementsAndDistance = std::tuple; 44 | 45 | static ClosestElementsAndDistance closest_elements_between_line_segments(const Primitives& line_1_primitives, 46 | const Primitives& line_2_primitives); 47 | 48 | 49 | static bool is_inside_line_segment(const DQ &point, const Primitives &line_1_primitives); 50 | 51 | static std::string to_string(const Element& e); 52 | 53 | 54 | private: 55 | 56 | static ClosestElementsAndDistance _update_closest_pair( 57 | const ClosestElementsAndDistance& current, 58 | const ClosestElementsAndDistance& candidate); 59 | 60 | static double _line_to_point_feasibility_and_distance( 61 | const Primitives& line_segment, 62 | const DQ& point); 63 | }; 64 | 65 | } 66 | 67 | } 68 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_HolonomicBase.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOTICS_ROBOT_MODELING_DQ_HOLONOMICBASE 24 | #define DQ_ROBOTICS_ROBOT_MODELING_DQ_HOLONOMICBASE 25 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | 33 | class DQ_HolonomicBase: public DQ_MobileBase 34 | { 35 | public: 36 | DQ_HolonomicBase(); 37 | 38 | //Virtual method overloads (DQ_Kinematics) 39 | virtual DQ fkm(const VectorXd& q) const override; 40 | virtual DQ fkm(const VectorXd& q, const int& to_ith_link) const override; 41 | virtual MatrixXd pose_jacobian(const VectorXd& q, const int& to_link) const override; 42 | virtual MatrixXd pose_jacobian(const VectorXd& q) const override; 43 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, 44 | const VectorXd& q_dot, const int& to_link) const override; 45 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, 46 | const VectorXd& q_dot) const override; 47 | 48 | DQ raw_fkm(const VectorXd& q) const; 49 | MatrixXd raw_pose_jacobian(const VectorXd& q, const int& to_link=2) const; 50 | MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, 51 | const VectorXd& q_dot, const int& to_link = 2) const; 52 | }; 53 | 54 | } 55 | 56 | #endif 57 | -------------------------------------------------------------------------------- /src/robot_control/DQ_ClassicQPController.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2019-2024 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 | 21 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 22 | Responsible for the original implementation 23 | 24 | 2. Juan Jose Quiroz Omana 25 | Fixed bug 59 (https://github.com/dqrobotics/python/issues/59) 26 | - Initialized a default damping to match the Matlab implementation 27 | of the class DQ_ClassicQPController.m 28 | */ 29 | 30 | #include 31 | 32 | namespace DQ_robotics 33 | { 34 | 35 | DQ_ClassicQPController::DQ_ClassicQPController(DQ_Kinematics* robot, DQ_QuadraticProgrammingSolver* solver): 36 | DQ_QuadraticProgrammingController (robot,solver) 37 | { 38 | set_damping(1e-3); // Default damping. 39 | } 40 | 41 | DQ_ClassicQPController::DQ_ClassicQPController(const std::shared_ptr &robot, 42 | const std::shared_ptr &solver): 43 | DQ_QuadraticProgrammingController(robot,solver) 44 | { 45 | set_damping(1e-3); // Default damping. 46 | } 47 | 48 | MatrixXd DQ_ClassicQPController::compute_objective_function_symmetric_matrix(const MatrixXd &J, const VectorXd&) 49 | { 50 | return (J.transpose()*J + damping_*MatrixXd::Identity(_get_robot_ptr()->get_dim_configuration_space(),_get_robot_ptr()->get_dim_configuration_space())); 51 | } 52 | 53 | VectorXd DQ_ClassicQPController::compute_objective_function_linear_component(const MatrixXd &J, const VectorXd &task_error) 54 | { 55 | return gain_*J.transpose()*task_error; 56 | } 57 | } 58 | 59 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_QuadraticProgrammingController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 27 | using namespace Eigen; 28 | 29 | namespace DQ_robotics 30 | { 31 | class DQ_QuadraticProgrammingController:public DQ_KinematicConstrainedController 32 | { 33 | protected: 34 | //Deprecated together with the raw pointer constructors but without the C++14 attribute as it is too noisy. 35 | DQ_QuadraticProgrammingSolver* qp_solver_; 36 | std::shared_ptr qp_solver_sptr_; 37 | 38 | DQ_QuadraticProgrammingSolver* _get_solver_ptr(); 39 | 40 | [[deprecated("Use the smart pointer version instead")]] 41 | DQ_QuadraticProgrammingController(DQ_Kinematics *robot, DQ_QuadraticProgrammingSolver *solver); 42 | DQ_QuadraticProgrammingController(const std::shared_ptr& robot, 43 | const std::shared_ptr& solver); 44 | public: 45 | //Remove default constructor 46 | DQ_QuadraticProgrammingController()=delete; 47 | 48 | virtual MatrixXd compute_objective_function_symmetric_matrix(const MatrixXd& J, const VectorXd& task_error)=0; 49 | virtual VectorXd compute_objective_function_linear_component(const MatrixXd& J, const VectorXd& task_error)=0; 50 | 51 | virtual VectorXd compute_setpoint_control_signal(const VectorXd&q, const VectorXd& task_reference) override; 52 | virtual VectorXd compute_tracking_control_signal(const VectorXd&q, const VectorXd& task_reference, const VectorXd& feed_forward) override; 53 | 54 | }; 55 | } 56 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_SerialManipulatorDenso.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2011-2025 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 | 21 | Contributors: 22 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 23 | - Responsible for the original implementation. 24 | 25 | 2. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) 26 | - Added the get_supported_joint_types() method. 27 | */ 28 | 29 | #include 30 | 31 | namespace DQ_robotics 32 | { 33 | 34 | class DQ_SerialManipulatorDenso: public DQ_SerialManipulator 35 | { 36 | protected: 37 | MatrixXd denso_matrix_; 38 | 39 | DQ _denso2dh(const double& q, const int& ith) const; 40 | public: 41 | std::vector get_supported_joint_types() const override; 42 | 43 | // Deprecated on 22.04, will be removed on the next release. 44 | [[deprecated("Use ? instead.")]] VectorXd get_as() const; 45 | [[deprecated("Use ? instead.")]] VectorXd get_bs() const; 46 | [[deprecated("Use ? instead.")]] VectorXd get_ds() const; 47 | [[deprecated("Use ? instead.")]] VectorXd get_alphas() const; 48 | [[deprecated("Use ? instead.")]] VectorXd get_betas() const; 49 | [[deprecated("Use ? instead.")]] VectorXd get_gammas() const; 50 | 51 | DQ_SerialManipulatorDenso()=delete; 52 | DQ_SerialManipulatorDenso(const MatrixXd& denso_matrix); 53 | 54 | using DQ_SerialManipulator::raw_pose_jacobian; 55 | using DQ_SerialManipulator::raw_pose_jacobian_derivative; 56 | using DQ_SerialManipulator::raw_fkm; 57 | 58 | MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; 59 | MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; 60 | DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; 61 | }; 62 | 63 | } 64 | -------------------------------------------------------------------------------- /src/robot_control/DQ_PseudoinverseController.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 24 | #include 25 | 26 | namespace DQ_robotics 27 | { 28 | 29 | DQ_PseudoinverseController::DQ_PseudoinverseController(DQ_Kinematics *robot): 30 | DQ_KinematicController (robot) 31 | { 32 | 33 | } 34 | 35 | DQ_PseudoinverseController::DQ_PseudoinverseController(const std::shared_ptr &robot): 36 | DQ_KinematicController(robot) 37 | { 38 | 39 | } 40 | 41 | VectorXd DQ_PseudoinverseController::compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) 42 | { 43 | return DQ_PseudoinverseController::compute_tracking_control_signal(q,task_reference,VectorXd::Zero(task_reference.size())); 44 | } 45 | 46 | VectorXd DQ_PseudoinverseController::compute_tracking_control_signal(const VectorXd &q, const VectorXd &task_reference, const VectorXd &feed_forward) 47 | { 48 | if(is_set()) 49 | { 50 | const VectorXd task_variable = get_task_variable(q); 51 | 52 | const MatrixXd J = get_jacobian(q); 53 | 54 | const VectorXd task_error = task_variable-task_reference; 55 | 56 | VectorXd u; 57 | if(damping_ == 0.0) 58 | u = pinv(J)*(-gain_*task_error + feed_forward); 59 | else 60 | u = (J.transpose()*J + damping_*damping_*MatrixXd::Identity(q.size(), q.size())).inverse()* 61 | J.transpose()* 62 | (-gain_*task_error + feed_forward); 63 | 64 | verify_stability(task_error); 65 | 66 | last_control_signal_ = u; 67 | last_error_signal_ = task_error; 68 | return u; 69 | } 70 | else 71 | { 72 | throw std::runtime_error("Tried computing the control signal of an unset controller."); 73 | } 74 | } 75 | 76 | } 77 | 78 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_WholeBody.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOT_MODELLING_DQ_WHOLE_BODY_H 24 | #define DQ_ROBOT_MODELLING_DQ_WHOLE_BODY_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace DQ_robotics 34 | { 35 | 36 | class DQ_WholeBody : public DQ_Kinematics 37 | { 38 | protected: 39 | std::vector> chain_; 40 | void _check_to_ith_chain(const int& to_ith_chain) const; 41 | public: 42 | DQ_WholeBody()=delete; 43 | DQ_WholeBody(std::shared_ptr robot); 44 | 45 | void add(std::shared_ptr robot); 46 | DQ raw_fkm(const VectorXd& q, const int& to_ith_chain) const; 47 | DQ raw_fkm(const VectorXd& q) const; 48 | void set_effector(const DQ& effector); 49 | DQ_Kinematics* get_chain(const int& to_ith_chain); 50 | DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const; 51 | DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const; 52 | 53 | //Abstract methods' implementation 54 | DQ fkm(const VectorXd& q) const override; 55 | DQ fkm(const VectorXd&, const int& to_chain) const override; 56 | MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_chain) const override; 57 | MatrixXd pose_jacobian(const VectorXd& q) const override; 58 | MatrixXd pose_jacobian_derivative(const VectorXd& q, 59 | const VectorXd& q_dot, 60 | const int& to_ith_link) const override; //To be implemented. 61 | MatrixXd pose_jacobian_derivative (const VectorXd& q, 62 | const VectorXd& q_dot) const override; //To be implemented. 63 | }; 64 | 65 | } 66 | 67 | #endif 68 | -------------------------------------------------------------------------------- /include/dqrobotics/utils/DQ_Geometry.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 25 | #include 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | 31 | class DQ_Geometry 32 | { 33 | public: 34 | static double point_to_point_squared_distance(const DQ& point1, const DQ& point2); 35 | 36 | static double point_to_line_squared_distance(const DQ& point, const DQ& line); 37 | 38 | static double point_to_plane_distance(const DQ& point, const DQ& plane); 39 | 40 | static double line_to_line_squared_distance(const DQ& line1, const DQ& line2); 41 | 42 | static double line_to_line_angle(const DQ& line1, const DQ& line2); 43 | 44 | static DQ point_projected_in_line(const DQ& point, const DQ& line); 45 | 46 | static std::tuple closest_points_between_lines(const DQ& line1, const DQ& line2); 47 | 48 | static bool is_line_segment(const DQ& line, 49 | const DQ& line_point_1, 50 | const DQ& line_point_2, 51 | const double& threshold=DQ_threshold); 52 | 53 | static std::tuple closest_points_between_line_segments(const DQ& line_1, 54 | const DQ& line_1_point_1, 55 | const DQ& line_1_point_2, 56 | const DQ& line_2, 57 | const DQ& line_2_point_1, 58 | const DQ& line_2_point_2); 59 | 60 | static double line_segment_to_line_segment_squared_distance(const DQ& line_1, 61 | const DQ& line_1_point_1, 62 | const DQ& line_1_point_2, 63 | const DQ& line_2, 64 | const DQ& line_2_point_1, 65 | const DQ& line_2_point_2); 66 | 67 | }; 68 | 69 | } 70 | 71 | 72 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_NumericalFilteredPseudoInverseController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | //This is an implementation of the controller described in the following work: 3 | //S. Chiaverini, 4 | //"Singularity-robust task-priority redundancy resolution for real-time kinematic control of robot manipulators," 5 | //in IEEE Transactions on Robotics and Automation, 6 | //vol. 13, no. 3, pp. 398-410, June 1997, 7 | //doi: 10.1109/70.585902. 8 | /** 9 | (C) Copyright 2022 DQ Robotics Developers 10 | 11 | This file is part of DQ Robotics. 12 | 13 | DQ Robotics is free software: you can redistribute it and/or modify 14 | it under the terms of the GNU Lesser General Public License as published by 15 | the Free Software Foundation, either version 3 of the License, or 16 | (at your option) any later version. 17 | 18 | DQ Robotics is distributed in the hope that it will be useful, 19 | but WITHOUT ANY WARRANTY; without even the implied warranty of 20 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 21 | GNU Lesser General Public License for more details. 22 | 23 | You should have received a copy of the GNU Lesser General Public License 24 | along with DQ Robotics. If not, see . 25 | 26 | Contributors: 27 | - Murilo M. Marinho (murilomarinho@ieee.org) 28 | */ 29 | #include 30 | #include 31 | 32 | namespace DQ_robotics 33 | { 34 | 35 | class DQ_NumericalFilteredPseudoinverseController: public DQ_PseudoinverseController 36 | { 37 | protected: 38 | double epsilon_; //Size of the singular region, described on the text after Eq. (15) 39 | double lambda_max_; //Maximum value for the numerical filtered damping, described on the text after Eq. (15) 40 | //double damping_; //(Member variable of DQ_KinematicController: Isotropic damping described on the text above Eq. (20) 41 | 42 | //log 43 | MatrixXd last_filtered_damping_; 44 | double last_jacobian_rank_; 45 | std::tuple last_jacobian_svd_; 46 | public: 47 | DQ_NumericalFilteredPseudoinverseController() = delete; 48 | [[deprecated("Use the smart pointer version instead")]] 49 | DQ_NumericalFilteredPseudoinverseController(DQ_Kinematics* robot); 50 | DQ_NumericalFilteredPseudoinverseController(const std::shared_ptr& robot); 51 | 52 | VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) override; 53 | VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward) override; 54 | 55 | void set_maximum_numerical_filtered_damping(const double& numerical_filtered_damping); 56 | void set_singular_region_size(const double& singular_region_size); 57 | 58 | double get_maximum_numerical_filtered_damping() const; 59 | double get_singular_region_size() const; 60 | MatrixXd get_last_filtered_damping() const; 61 | int get_last_jacobian_rank() const; 62 | std::tuple get_last_jacobian_svd() const; 63 | }; 64 | 65 | } 66 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_SerialManipulatorDH.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2011-2025 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 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 22 | - Responsible for the original implementation. 23 | 24 | 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) 25 | - Added methods to get and set the DH parameters. 26 | - Added the get_supported_joint_types() method. 27 | */ 28 | 29 | 30 | #include 31 | #include 32 | 33 | namespace DQ_robotics 34 | { 35 | 36 | class DQ_SerialManipulatorDH: public DQ_SerialManipulator 37 | { 38 | protected: 39 | MatrixXd dh_matrix_; 40 | 41 | DQ _get_w(const int& ith) const; 42 | DQ _dh2dq(const double& q, const int& ith) const; 43 | public: 44 | VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; 45 | double get_parameter(const DQ_ParameterDH& parameter_type, 46 | const int& to_ith_link) const; 47 | void set_parameters(const DQ_ParameterDH& parameter_type, 48 | const VectorXd& vector_parameters); 49 | void set_parameter(const DQ_ParameterDH& parameter_type, 50 | const int& to_ith_link, 51 | const double& parameter); 52 | 53 | std::vector get_supported_joint_types()const override; 54 | 55 | // Deprecated on 22.04, will be removed on the next release. 56 | enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; 57 | 58 | [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; 59 | [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; 60 | [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; 61 | [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; 62 | [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; 63 | 64 | DQ_SerialManipulatorDH()=delete; 65 | DQ_SerialManipulatorDH(const MatrixXd& dh_matrix); 66 | 67 | using DQ_SerialManipulator::raw_pose_jacobian; 68 | using DQ_SerialManipulator::raw_pose_jacobian_derivative; 69 | using DQ_SerialManipulator::raw_fkm; 70 | 71 | MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; 72 | MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; 73 | DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; 74 | }; 75 | 76 | } 77 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_SerialManipulatorMDH.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2011-2025 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 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 22 | - Responsible for the original implementation. 23 | 24 | 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) 25 | - Added methods to get and set the DH parameters. 26 | - Added the get_supported_joint_types() method. 27 | */ 28 | 29 | #include 30 | #include 31 | 32 | namespace DQ_robotics 33 | { 34 | 35 | class DQ_SerialManipulatorMDH: public DQ_SerialManipulator 36 | { 37 | protected: 38 | MatrixXd mdh_matrix_; 39 | 40 | DQ _get_w(const int& ith) const; 41 | DQ _mdh2dq(const double& q, const int& ith) const; 42 | public: 43 | VectorXd get_parameters(const DQ_ParameterDH& parameter_type) const; 44 | double get_parameter(const DQ_ParameterDH& parameter_type, 45 | const int& to_ith_link) const; 46 | void set_parameters(const DQ_ParameterDH& parameter_type, 47 | const VectorXd& vector_parameters); 48 | void set_parameter(const DQ_ParameterDH& parameter_type, 49 | const int& to_ith_link, 50 | const double& parameter); 51 | 52 | std::vector get_supported_joint_types()const override; 53 | 54 | // Deprecated on 22.04, will be removed on the next release. 55 | enum [[deprecated("Use DQ_JointType instead.")]] JOINT_TYPES{ JOINT_ROTATIONAL=0, JOINT_PRISMATIC }; 56 | 57 | [[deprecated("Use get_parameters(DQ_ParameterDH::THETA) instead.")]] VectorXd get_thetas() const; 58 | [[deprecated("Use get_parameters(DQ_ParameterDH::D) instead.")]] VectorXd get_ds() const; 59 | [[deprecated("Use get_parameters(DQ_ParameterDH::A) instead.")]] VectorXd get_as() const; 60 | [[deprecated("Use get_parameters(DQ_ParameterDH::ALPHA) instead.")]] VectorXd get_alphas() const; 61 | [[deprecated("Use get_joint_types() instead.")]] VectorXd get_types() const; 62 | 63 | DQ_SerialManipulatorMDH()=delete; 64 | DQ_SerialManipulatorMDH(const MatrixXd& mdh_matrix); 65 | 66 | using DQ_SerialManipulator::raw_pose_jacobian; 67 | using DQ_SerialManipulator::raw_pose_jacobian_derivative; 68 | using DQ_SerialManipulator::raw_fkm; 69 | 70 | MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; 71 | MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; 72 | DQ raw_fkm(const VectorXd &q_vec, const int &to_ith_link) const override; 73 | }; 74 | 75 | } 76 | 77 | 78 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_SerialWholeBody.h: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2020-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 | #ifndef DQ_ROBOT_MODELLING_DQ_SERIAL_WHOLE_BODY_H 24 | #define DQ_ROBOT_MODELLING_DQ_SERIAL_WHOLE_BODY_H 25 | 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | namespace DQ_robotics 34 | { 35 | 36 | class DQ_SerialWholeBody : public DQ_Kinematics 37 | { 38 | protected: 39 | std::vector> chain_; 40 | void _check_to_ith_chain(const int& to_ith_chain) const; 41 | void _check_to_jth_link_of_ith_chain(const int& to_ith_chain, const int& to_jth_link) const; 42 | public: 43 | DQ_SerialWholeBody()=delete; 44 | DQ_SerialWholeBody(std::shared_ptr robot, const std::string type=std::string("standard")); 45 | 46 | void add(std::shared_ptr robot); 47 | 48 | DQ raw_fkm_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const; 49 | DQ raw_fkm_by_chain(const VectorXd& q, const int& to_ith_chain) const; 50 | std::tuple get_chain_and_link_from_index(const int& to_ith_link) const; 51 | 52 | DQ raw_fkm(const VectorXd& q) const; 53 | DQ raw_fkm(const VectorXd& q, const int& to_ith_link) const; 54 | 55 | void set_effector(const DQ& effector); 56 | 57 | DQ_Kinematics* get_chain(const int& to_ith_chain); 58 | DQ_SerialManipulatorDH get_chain_as_serial_manipulator_dh(const int& to_ith_chain) const; 59 | DQ_HolonomicBase get_chain_as_holonomic_base(const int& to_ith_chain) const; 60 | MatrixXd raw_pose_jacobian_by_chain(const VectorXd& q, const int& to_ith_chain, const int& to_jth_link) const; 61 | MatrixXd raw_pose_jacobian_derivative_by_chain(const VectorXd& q, 62 | const VectorXd& q_dot, 63 | const int& to_ith_chain, 64 | const int& to_jth_link) const; //To be implemented. 65 | 66 | //Abstract methods' implementation 67 | DQ fkm(const VectorXd& q) const override; 68 | DQ fkm(const VectorXd&, const int& to_ith_link) const override; 69 | MatrixXd pose_jacobian(const VectorXd& q, const int& to_ith_link) const override; 70 | MatrixXd pose_jacobian(const VectorXd& q) const override; 71 | MatrixXd pose_jacobian_derivative(const VectorXd& q, 72 | const VectorXd& q_dot, 73 | const int& to_ith_link) const override; //To be implemented. 74 | MatrixXd pose_jacobian_derivative (const VectorXd& q, 75 | const VectorXd& q_dot) const override; //To be implemented. 76 | }; 77 | 78 | } 79 | 80 | #endif 81 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_ParameterDH.h: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2011-2025 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. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) 21 | - Responsible for the original implementation. 22 | */ 23 | 24 | #include 25 | #include 26 | #pragma once 27 | 28 | namespace DQ_robotics 29 | { 30 | class DQ_ParameterDH 31 | { 32 | public: 33 | enum PARAMETER{ 34 | THETA, 35 | D, 36 | A, 37 | ALPHA 38 | }; 39 | private: 40 | PARAMETER parameter_; 41 | const std::unordered_map 42 | map_ = {{"THETA", THETA}, 43 | {"D" , D}, 44 | {"A" , A}, 45 | {"ALPHA", ALPHA}, 46 | }; 47 | 48 | /** 49 | * @brief _get_parameter sets the parameter member using a string as argument. 50 | * @param parameter The desired parameter to be set. Example: "THETA", "D", "A", or "ALPHA". 51 | */ 52 | void _set_parameter(const std::string& parameter) 53 | { 54 | try { 55 | parameter_ = map_.at(parameter); 56 | } catch (...) { 57 | throw std::runtime_error("The parameter \""+ parameter+ "\" is not supported. Use THETA, D, A, or ALPHA"); 58 | } 59 | } 60 | public: 61 | /** 62 | * @brief DQ_ParameterDH Default constructor method. 63 | */ 64 | DQ_ParameterDH() = default; 65 | 66 | /** 67 | * @brief DQ_ParameterDH Constructor method 68 | * @param parameter The desired DH parameter. Example: THETA, D, A, or ALPHA. 69 | */ 70 | DQ_ParameterDH(const PARAMETER& parameter): parameter_{parameter}{}; 71 | 72 | // This definition enables switch cases and comparisons. 73 | constexpr operator PARAMETER() const { return parameter_; } 74 | 75 | /** 76 | * @brief DQ_ParameterDH Constructor method that allows string parameters. 77 | * This is done to keep the language compatibility between 78 | * Matlab and Python/C++, as discussed in 79 | * https://github.com/dqrobotics/cpp/pull/69 80 | * @param parameter The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". 81 | */ 82 | DQ_ParameterDH(const std::string& parameter){ 83 | _set_parameter(parameter); 84 | } 85 | 86 | 87 | /** 88 | * @brief DQ_ParameterDH Constructor method that allows char parameters. 89 | * This is done to keep the language compatibility between 90 | * Matlab and Python/C++, as discussed in 91 | * https://github.com/dqrobotics/cpp/pull/69 92 | * @param parameter_c The desired DH parameter. Example: "THETA", "D", "A", or "ALPHA". 93 | */ 94 | DQ_ParameterDH(const char* parameter_c){ 95 | _set_parameter(parameter_c); 96 | } 97 | 98 | }; 99 | } 100 | 101 | 102 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_CooperativeDualTaskSpace.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 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_CooperativeDualTaskSpace::DQ_CooperativeDualTaskSpace(DQ_Kinematics *robot1, DQ_Kinematics *robot2) 29 | { 30 | robot1_ = robot1; 31 | robot2_ = robot2; 32 | } 33 | 34 | DQ DQ_CooperativeDualTaskSpace::pose1(const VectorXd &theta) 35 | { 36 | return robot1_->fkm(theta.head(robot1_->get_dim_configuration_space())); 37 | } 38 | 39 | DQ DQ_CooperativeDualTaskSpace::pose2(const VectorXd &theta) 40 | { 41 | return robot2_->fkm(theta.tail(robot2_->get_dim_configuration_space())); 42 | } 43 | 44 | MatrixXd DQ_CooperativeDualTaskSpace::pose_jacobian1(const VectorXd &theta) 45 | { 46 | return robot1_->pose_jacobian(theta.head(robot1_->get_dim_configuration_space())); 47 | } 48 | 49 | MatrixXd DQ_CooperativeDualTaskSpace::pose_jacobian2(const VectorXd &theta) 50 | { 51 | return robot2_->pose_jacobian(theta.tail(robot2_->get_dim_configuration_space())); 52 | } 53 | 54 | DQ DQ_CooperativeDualTaskSpace::relative_pose(const VectorXd &theta) 55 | { 56 | return conj(pose2(theta))*pose1(theta); 57 | } 58 | 59 | DQ DQ_CooperativeDualTaskSpace::absolute_pose(const VectorXd &theta) 60 | { 61 | return pose2(theta)*(pow(relative_pose(theta),0.5)); 62 | } 63 | 64 | MatrixXd DQ_CooperativeDualTaskSpace::relative_pose_jacobian(const VectorXd &theta) 65 | { 66 | const MatrixXd Jx1 = pose_jacobian1(theta); 67 | const DQ x1 = pose1(theta); 68 | const MatrixXd Jx2 = pose_jacobian2(theta); 69 | const DQ x2 = pose2(theta); 70 | 71 | MatrixXd Jxr(8,Jx1.cols()+Jx2.cols()); 72 | Jxr << (hamiplus8(conj(x2)))*Jx1,haminus8(x1)*C8()*Jx2; 73 | return Jxr; 74 | } 75 | 76 | MatrixXd DQ_CooperativeDualTaskSpace::absolute_pose_jacobian(const VectorXd &theta) 77 | { 78 | //Preliminaries 79 | const MatrixXd Jx2 = pose_jacobian2(theta); 80 | const DQ x2 = pose2(theta); 81 | const MatrixXd Jxr = relative_pose_jacobian(theta); 82 | const DQ xr = relative_pose(theta); 83 | const MatrixXd Jtr = DQ_Kinematics::translation_jacobian(Jxr,xr); 84 | 85 | //Rotation part 86 | MatrixXd Jrr2 = 0.5*haminus4(conj(xr.P())*pow(xr.P(),0.5))*Jxr.block(0,0,4,Jxr.cols()); 87 | 88 | MatrixXd Jxr2(8,Jrr2.cols()); 89 | Jxr2 << Jrr2, 0.25*(haminus4(pow(xr.P(),0.5))*Jtr + hamiplus4(translation(xr))*Jrr2); 90 | 91 | MatrixXd temp(8,robot1_->get_dim_configuration_space()+robot2_->get_dim_configuration_space()); 92 | temp << MatrixXd::Zero(8,robot1_->get_dim_configuration_space()),Jx2; 93 | 94 | MatrixXd Jxa(8,robot1_->get_dim_configuration_space()+robot2_->get_dim_configuration_space()); 95 | Jxa << haminus8(pow(xr,0.5))*(temp) + hamiplus8(x2)*Jxr2; 96 | 97 | return Jxa; 98 | } 99 | 100 | } 101 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_control/DQ_KinematicController.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2019-2022 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 | 26 | #include 27 | #include 28 | 29 | namespace DQ_robotics 30 | { 31 | 32 | enum ControlObjective 33 | { 34 | None, 35 | Distance, 36 | DistanceToPlane, 37 | Line, 38 | Plane, 39 | Pose, 40 | Rotation, 41 | Translation 42 | }; 43 | 44 | class DQ_KinematicController 45 | { 46 | protected: 47 | //Deprecated together with the raw pointer constructors but without the C++14 attribute as it is too noisy. 48 | DQ_Kinematics* robot_; 49 | std::shared_ptr robot_sptr_; 50 | ControlObjective control_objective_; 51 | DQ attached_primitive_; 52 | DQ target_primitive_; 53 | 54 | double gain_; 55 | double damping_; 56 | 57 | bool system_reached_stable_region_; 58 | VectorXd last_control_signal_; 59 | VectorXd last_error_signal_; 60 | 61 | double stability_threshold_; 62 | int stability_counter_; 63 | int stability_counter_max_; 64 | 65 | //For backwards compatibility reasons, to be removed 66 | DQ_Kinematics* _get_robot_ptr() const; 67 | 68 | std::shared_ptr _get_robot() const; 69 | 70 | //Deprecated 71 | [[deprecated("Use the smart pointer version instead.")]] 72 | DQ_KinematicController(DQ_Kinematics* robot); 73 | DQ_KinematicController(const std::shared_ptr& robot); 74 | DQ_KinematicController(); 75 | public: 76 | 77 | ControlObjective get_control_objective() const; 78 | 79 | MatrixXd get_jacobian(const VectorXd& q) const; 80 | 81 | VectorXd get_task_variable(const VectorXd& q) const; 82 | 83 | VectorXd get_last_error_signal() const; 84 | 85 | bool is_set() const; 86 | 87 | bool system_reached_stable_region() const; 88 | 89 | void set_control_objective(const ControlObjective& control_objective); 90 | 91 | void set_gain(const double& gain); 92 | double get_gain() const; 93 | 94 | void set_damping(const double& damping); 95 | double get_damping() const; 96 | 97 | void set_stability_threshold(const double& threshold); 98 | 99 | void set_primitive_to_effector(const DQ& primitive); 100 | 101 | void set_target_primitive(const DQ& primitive); 102 | 103 | void set_stability_counter_max(const int& max); 104 | 105 | void reset_stability_counter(); 106 | 107 | //Virtual 108 | virtual ~DQ_KinematicController()=default; 109 | virtual VectorXd compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference)=0; 110 | virtual VectorXd compute_tracking_control_signal(const VectorXd& q, const VectorXd& task_reference, const VectorXd& feed_forward)=0; 111 | virtual void verify_stability(const VectorXd& task_error); 112 | 113 | }; 114 | 115 | } 116 | 117 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Updated 2025.05 to 3.10 since compatibility with CMake < 3.10 will be removed from a future version of CMake. 2 | cmake_minimum_required(VERSION 3.10) 3 | 4 | # Added in case users want to build this library as STATIC 5 | # https://cmake.org/cmake/help/latest/variable/BUILD_SHARED_LIBS.html 6 | option(BUILD_SHARED_LIBS "Build using shared libraries" ON) 7 | 8 | if(WIN32) 9 | set(CMAKE_TOOLCHAIN_FILE C:/vcpkg/scripts/buildsystems/vcpkg.cmake) 10 | # A must so that we don't need to manually export each public part of the library 11 | set(CMAKE_WINDOWS_EXPORT_ALL_SYMBOLS ON) 12 | endif() 13 | 14 | project(dqrobotics) 15 | 16 | set(CMAKE_CXX_STANDARD 17) 17 | set(CMAKE_CXX_STANDARD_REQUIRED ON) 18 | 19 | find_package(Eigen3 REQUIRED) 20 | 21 | # As of 2025.05, Not using GLOB_RECURSE as it's explicitly discouraged. 22 | # https://stackoverflow.com/questions/3201154/automatically-add-all-files-in-a-folder-to-a-target-using-cmake 23 | # https://cmake.org/cmake/help/latest/command/file.html#filesystem 24 | add_library(dqrobotics 25 | src/DQ.cpp 26 | 27 | src/internal/_dq_linesegment.cpp 28 | 29 | src/utils/DQ_Geometry.cpp 30 | src/utils/DQ_LinearAlgebra.cpp 31 | src/utils/DQ_Math.cpp 32 | 33 | src/robot_modeling/DQ_CooperativeDualTaskSpace.cpp 34 | src/robot_modeling/DQ_Kinematics.cpp 35 | src/robot_modeling/DQ_SerialManipulator.cpp 36 | src/robot_modeling/DQ_SerialManipulatorDH.cpp 37 | src/robot_modeling/DQ_SerialManipulatorMDH.cpp 38 | src/robot_modeling/DQ_SerialManipulatorDenso.cpp 39 | src/robot_modeling/DQ_MobileBase.cpp 40 | src/robot_modeling/DQ_HolonomicBase.cpp 41 | src/robot_modeling/DQ_DifferentialDriveRobot.cpp 42 | src/robot_modeling/DQ_WholeBody.cpp 43 | src/robot_modeling/DQ_SerialWholeBody.cpp 44 | 45 | src/robot_control/DQ_KinematicController.cpp 46 | src/robot_control/DQ_PseudoinverseController.cpp 47 | src/robot_control/DQ_NumericalFilteredPseudoInverseController.cpp 48 | src/robot_control/DQ_KinematicConstrainedController.cpp 49 | src/robot_control/DQ_QuadraticProgrammingController.cpp 50 | src/robot_control/DQ_ClassicQPController.cpp 51 | 52 | src/robots/Ax18ManipulatorRobot.cpp 53 | src/robots/BarrettWamArmRobot.cpp 54 | src/robots/ComauSmartSixRobot.cpp 55 | src/robots/KukaLw4Robot.cpp 56 | src/robots/KukaYoubotRobot.cpp 57 | src/robots/FrankaEmikaPandaRobot.cpp 58 | ) 59 | 60 | set_property(TARGET dqrobotics PROPERTY CXX_STANDARD 17) 61 | 62 | target_include_directories(dqrobotics PUBLIC 63 | include 64 | ) 65 | 66 | if(UNIX) 67 | # WIN32 does not accept these flags 68 | target_compile_options(dqrobotics PUBLIC 69 | -Werror=return-type -Wall -Wextra -Wmissing-declarations -Wredundant-decls -Woverloaded-virtual -fPIC 70 | ) 71 | endif() 72 | 73 | if(WIN32) 74 | # https://learn.microsoft.com/en-us/cpp/c-runtime-library/math-constants?view=msvc-170 75 | target_compile_definitions(dqrobotics PUBLIC 76 | _USE_MATH_DEFINES 77 | ) 78 | endif() 79 | 80 | target_link_libraries(dqrobotics PUBLIC 81 | Eigen3::Eigen 82 | ) 83 | 84 | set_target_properties(dqrobotics 85 | PROPERTIES PUBLIC_HEADER 86 | include/dqrobotics/DQ.h 87 | ) 88 | 89 | install(TARGETS dqrobotics 90 | # https://stackoverflow.com/questions/21592361/cmake-install-is-not-installing-libraries-on-windows 91 | RUNTIME DESTINATION "bin" 92 | LIBRARY DESTINATION "lib" 93 | ARCHIVE DESTINATION "lib" 94 | PUBLIC_HEADER DESTINATION "include/dqrobotics" 95 | PERMISSIONS OWNER_READ OWNER_WRITE GROUP_READ WORLD_READ 96 | ) 97 | 98 | install(DIRECTORY 99 | include/ 100 | DESTINATION "include" 101 | ) 102 | 103 | # Install source files so that the debugger can find them 104 | install(DIRECTORY 105 | src/ 106 | DESTINATION "src/dqrobotics" 107 | ) 108 | -------------------------------------------------------------------------------- /src/robot_control/DQ_QuadraticProgrammingController.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 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 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_QuadraticProgrammingSolver *DQ_QuadraticProgrammingController::_get_solver_ptr() 29 | { 30 | return qp_solver_sptr_ ? qp_solver_sptr_.get() : qp_solver_; 31 | } 32 | 33 | DQ_QuadraticProgrammingController::DQ_QuadraticProgrammingController(DQ_Kinematics* robot, 34 | DQ_QuadraticProgrammingSolver* solver) 35 | :DQ_KinematicConstrainedController (robot), 36 | qp_solver_(solver) 37 | { 38 | 39 | } 40 | 41 | DQ_QuadraticProgrammingController::DQ_QuadraticProgrammingController(const std::shared_ptr &robot, 42 | const std::shared_ptr &solver): 43 | DQ_KinematicConstrainedController(robot), 44 | qp_solver_sptr_(solver) 45 | { 46 | 47 | } 48 | 49 | VectorXd DQ_QuadraticProgrammingController::compute_setpoint_control_signal(const VectorXd &q, const VectorXd &task_reference) 50 | { 51 | return DQ_QuadraticProgrammingController::compute_tracking_control_signal(q,task_reference,VectorXd::Zero(task_reference.size())); 52 | } 53 | 54 | VectorXd DQ_QuadraticProgrammingController::compute_tracking_control_signal(const VectorXd &q, const VectorXd &task_reference, const VectorXd &feed_forward) 55 | { 56 | if(is_set()) 57 | { 58 | const VectorXd task_variable = get_task_variable(q); 59 | 60 | const MatrixXd J = get_jacobian(q); 61 | 62 | if(task_variable.size() != task_reference.size()) 63 | throw std::runtime_error("Incompatible sizes between task variable and task reference in compute_tracking_control_signal"); 64 | 65 | const VectorXd task_error = task_variable - task_reference; 66 | 67 | if(J.rows() != task_error.size()) 68 | throw std::runtime_error("Incompatible sizes between the Jacobian and the task error in compute_tracking_control_signal"); 69 | if(task_error.size() != feed_forward.size()) 70 | throw std::runtime_error("Incompatible sizes between task error and feedforward in compute_tracking_control_signal"); 71 | 72 | const MatrixXd& A = inequality_constraint_matrix_; 73 | const VectorXd& b = inequality_constraint_vector_; 74 | const MatrixXd& Aeq = equality_constraint_matrix_; 75 | const VectorXd& beq = equality_constraint_vector_; 76 | 77 | const MatrixXd H = compute_objective_function_symmetric_matrix(J,task_error - (1.0/gain_)*feed_forward); 78 | const MatrixXd f = compute_objective_function_linear_component(J,task_error - (1.0/gain_)*feed_forward); 79 | 80 | VectorXd u = _get_solver_ptr()->solve_quadratic_program(H,f,A,b,Aeq,beq); 81 | 82 | verify_stability(task_error); 83 | 84 | last_control_signal_ = u; 85 | last_error_signal_ = task_error; 86 | 87 | return u; 88 | } 89 | { 90 | throw std::runtime_error("Trying to compute the control signal using an unset controller"); 91 | } 92 | } 93 | 94 | } 95 | 96 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_SerialManipulator.h: -------------------------------------------------------------------------------- 1 | #pragma once 2 | /** 3 | (C) Copyright 2011-2025 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 | 1. Murilo M. Marinho (murilomarinho@ieee.org) 22 | 2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) 23 | 24 | 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) 25 | - Added the joint_types member, and the following methods: 26 | _check_joint_types(), and {set,get}_joint_{type, types}. 27 | */ 28 | 29 | #include 30 | #include 31 | #include 32 | 33 | namespace DQ_robotics 34 | { 35 | 36 | class DQ_SerialManipulator: public DQ_Kinematics 37 | { 38 | protected: 39 | DQ curr_effector_; 40 | std::vector joint_types_; 41 | DQ_SerialManipulator(const int& dofs); 42 | void _check_joint_types() const; 43 | public: 44 | DQ get_effector() const; 45 | DQ set_effector(const DQ& new_effector); 46 | 47 | VectorXd get_lower_q_limit() const; 48 | void set_lower_q_limit(const VectorXd& lower_q_limit); 49 | VectorXd get_lower_q_dot_limit() const; 50 | void set_lower_q_dot_limit(const VectorXd &lower_q_dot_limit); 51 | VectorXd get_upper_q_limit() const; 52 | void set_upper_q_limit(const VectorXd& upper_q_limit); 53 | VectorXd get_upper_q_dot_limit() const; 54 | void set_upper_q_dot_limit(const VectorXd &upper_q_dot_limit); 55 | 56 | DQ_JointType get_joint_type(const int& ith_joint) const; 57 | std::vector get_joint_types() const; 58 | void set_joint_type(const DQ_JointType& joint_type, const int& ith_joint); 59 | void set_joint_types(const std::vector& joint_types); 60 | void set_joint_types(const VectorXd& joint_types); 61 | 62 | //Virtual 63 | virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec) const; 64 | virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const; 65 | virtual DQ raw_fkm(const VectorXd& q_vec) const; 66 | 67 | //Pure virtual 68 | virtual MatrixXd raw_pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const = 0; 69 | virtual MatrixXd raw_pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const = 0; 70 | virtual DQ raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const = 0; 71 | virtual std::vector get_supported_joint_types() const = 0; 72 | 73 | //Overrides from DQ_Kinematics 74 | virtual DQ fkm(const VectorXd& q_vec) const override; //Override from DQ_Kinematics 75 | virtual DQ fkm(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics 76 | 77 | virtual int get_dim_configuration_space() const override; //Override from DQ_Kinematics 78 | 79 | virtual MatrixXd pose_jacobian(const VectorXd& q_vec, const int& to_ith_link) const override; //Override from DQ_Kinematics 80 | virtual MatrixXd pose_jacobian(const VectorXd& q_vec) const override; //Override from DQ_Kinematics 81 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot, const int& to_ith_link) const override; //Override from DQ_Kinematics 82 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, const VectorXd& q_dot) const override; //Override from DQ_Kinematics 83 | 84 | }; 85 | 86 | } 87 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_JointType.h: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2011-2025 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. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) 21 | - Responsible for the original implementation. 22 | */ 23 | 24 | 25 | #pragma once 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | class DQ_JointType 31 | { 32 | public: 33 | enum JOINT_TYPE{ 34 | REVOLUTE = 0, 35 | PRISMATIC, 36 | SPHERICAL, 37 | CYLINDRICAL, 38 | PLANAR, 39 | SIX_DOF, 40 | HELICAL 41 | }; 42 | // This definition enables switch cases and comparisons. 43 | constexpr operator JOINT_TYPE() const { return joint_type_; } 44 | private: 45 | JOINT_TYPE joint_type_; 46 | 47 | public: 48 | /** 49 | * @brief DQ_JointType Default constructor method. 50 | * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). 51 | * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. 52 | */ 53 | DQ_JointType() = default; 54 | 55 | /** 56 | * @brief DQ_JointType Constructor method. 57 | * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). 58 | * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra. 59 | * @param joint_type The joint type. Example: REVOLUTE, PRISMATIC, 60 | * SPHERICAL, CYLINDRICAL, PLANAR, SIX_DOF, or HELICAL. 61 | */ 62 | DQ_JointType(const JOINT_TYPE& joint_type): joint_type_{joint_type}{}; 63 | 64 | /** 65 | * @brief DQ_JointType Constructor method that allows integer arguments. 66 | * This class is based on Table 1 of Silva, Quiroz-Omaña, and Adorno (2022). 67 | * Dynamics of Mobile Manipulators Using Dual Quaternion Algebra 68 | * @param joint_type The joint type. 69 | */ 70 | DQ_JointType(const int& joint_type){ 71 | switch (joint_type) { 72 | case 0: 73 | joint_type_ = REVOLUTE; 74 | break; 75 | case 1: 76 | joint_type_ = PRISMATIC; 77 | break; 78 | case 2: 79 | joint_type_ = SPHERICAL; 80 | break; 81 | case 3: 82 | joint_type_ = CYLINDRICAL; 83 | break; 84 | case 4: 85 | joint_type_ = PLANAR; 86 | break; 87 | case 5: 88 | joint_type_ = SIX_DOF; 89 | break; 90 | case 6: 91 | joint_type_ = HELICAL; 92 | break; 93 | default: 94 | throw std::runtime_error("Invalid joint type"); 95 | } 96 | } 97 | 98 | /** 99 | * @brief to_string() converts the DQ_JointType to string. 100 | * @return A string that corresponds to the joint type. 101 | */ 102 | std::string to_string() const { 103 | switch (joint_type_) { 104 | 105 | case REVOLUTE: 106 | return std::string("REVOLUTE"); 107 | case PRISMATIC: 108 | return std::string("PRISMATIC"); 109 | case SPHERICAL: 110 | return std::string("SPHERICAL"); 111 | case CYLINDRICAL: 112 | return std::string("CYLINDRICAL"); 113 | case PLANAR: 114 | return std::string("PLANAR"); 115 | case SIX_DOF: 116 | return std::string("SIX_DOF"); 117 | case HELICAL: 118 | return std::string("HELICAL"); 119 | default: 120 | throw std::runtime_error("Invalid joint type"); 121 | } 122 | } 123 | }; 124 | 125 | } 126 | -------------------------------------------------------------------------------- /src/utils/DQ_LinearAlgebra.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 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | /** 29 | * @brief pinv Calculates the pseudo inverse of the input @p matrix using Singular Value Decomposition with 30 | * a given tolerance for small singular values. 31 | * http://www.mathworks.com/help/matlab/ref/pinv.html 32 | * @param matrix the input matrix 33 | * @return the pseudo-inverse of @p matrix such that pinv(matrix)*matrix is as close as possible to the identity matrix. 34 | */ 35 | MatrixXd pinv(const MatrixXd& matrix) 36 | { 37 | int num_rows = matrix.rows(); 38 | int num_cols = matrix.cols(); 39 | 40 | MatrixXd pseudo_inverse(num_cols,num_rows); 41 | JacobiSVD svd(num_cols,num_rows); 42 | VectorXd singular_values; 43 | MatrixXd svd_sigma_inverted(num_cols,num_rows); 44 | svd_sigma_inverted = MatrixXd::Zero(num_cols,num_rows); 45 | 46 | svd.compute(matrix, ComputeFullU | ComputeFullV); 47 | singular_values = svd.singularValues(); 48 | 49 | //Tolerance Calculation 50 | double eps = std::numeric_limits::epsilon(); 51 | int max = (num_rows > num_cols) ? num_rows : num_cols; 52 | double norm = singular_values(0); //Matlab uses the 2-NORM, which is the largest singular value. Meyer p.281 53 | double tol = max*norm*eps; 54 | 55 | for(auto i=0;i tol) 58 | svd_sigma_inverted(i,i) = 1/(singular_values(i)); 59 | else 60 | svd_sigma_inverted(i,i) = 0; 61 | } 62 | 63 | pseudo_inverse = svd.matrixV() * (svd_sigma_inverted * svd.matrixU().adjoint()); 64 | 65 | return pseudo_inverse; 66 | } 67 | 68 | /** 69 | * @brief svd calculates the singular value decomposition of @a matrix. 70 | * The result is meant to mimic MATLAB's version of it. 71 | * Overload this function if you need a customized version of it. 72 | * https://www.mathworks.com/help/matlab/ref/double.svd.html 73 | * @param matrix the input matrix. 74 | * @return a tuple ordered as U*S*V such that the original matrix 75 | * can be obtained as matrix = U*S*V.adjoint(). 76 | */ 77 | std::tuple svd(const MatrixXd& matrix) 78 | { 79 | int num_rows = matrix.rows(); 80 | int num_cols = matrix.cols(); 81 | 82 | MatrixXd pseudo_inverse(num_cols,num_rows); 83 | JacobiSVD svd(num_cols,num_rows); 84 | svd.compute(matrix, ComputeFullU | ComputeFullV); 85 | 86 | return {svd.matrixU(), 87 | svd.singularValues().asDiagonal(), 88 | svd.matrixV()}; 89 | } 90 | 91 | /** 92 | * @brief rank calculates the rank of @a matrix. 93 | * The result is meant to mimic MATLAB's version of it. 94 | * https://www.mathworks.com/help/matlab/ref/rank.html 95 | * @param matrix the input matrix. 96 | * @return the rank of the matrix using singular value decomposition 97 | * and the default MATLAB tolerance. 98 | */ 99 | int rank(const MatrixXd &matrix) 100 | { 101 | int num_rows = matrix.rows(); 102 | int num_cols = matrix.cols(); 103 | 104 | JacobiSVD svd(num_cols,num_rows); 105 | VectorXd singular_values; 106 | 107 | svd.compute(matrix); 108 | singular_values = svd.singularValues(); 109 | 110 | //Tolerance Calculation 111 | const double eps = std::numeric_limits::epsilon(); 112 | const int max = (num_rows > num_cols) ? num_rows : num_cols; 113 | const double norm = singular_values(0); //Matlab uses the 2-NORM, which is the largest singular value. Meyer p.281 114 | const double tol = max*norm*eps; 115 | 116 | int rank=0; 117 | for(auto i=0;i tol) 120 | rank++; 121 | } 122 | 123 | return rank; 124 | } 125 | 126 | } 127 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_DifferentialDriveRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 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 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_DifferentialDriveRobot::DQ_DifferentialDriveRobot(const double& wheel_radius, const double& distance_between_wheels) 29 | { 30 | wheel_radius_ = wheel_radius; 31 | distance_between_wheels_ = distance_between_wheels; 32 | } 33 | 34 | MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian(const double &phi) const 35 | { 36 | const double& r = wheel_radius_; 37 | const double& l = distance_between_wheels_; 38 | double c = cos(phi); 39 | double s = sin(phi); 40 | 41 | MatrixXd J(3,2); 42 | J << (r/2)*c, (r/2)*c, 43 | (r/2)*s, (r/2)*s, 44 | r/l, -r/l; 45 | return J; 46 | } 47 | 48 | /** 49 | * @brief returns the time derivative of the constraint Jacobian 50 | * @param phi The orientation of the robot on the plane. 51 | * @param phi_dot The time derivative of phi. 52 | * @return a MatrixXd representing the desired Jacobian derivative 53 | */ 54 | MatrixXd DQ_DifferentialDriveRobot::constraint_jacobian_derivative(const double &phi, const double &phi_dot) const 55 | { 56 | const double& r = wheel_radius_; 57 | double c = cos(phi); 58 | double s = sin(phi); 59 | 60 | MatrixXd J_dot(3,2); 61 | J_dot << -(r/2)*s, -(r/2)*s, 62 | (r/2)*c, (r/2)*c, 63 | 0, 0; 64 | return J_dot*phi_dot; 65 | } 66 | 67 | MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q, const int &to_link) const 68 | { 69 | if(to_link!=0 && to_link!=1) 70 | throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian(q,to_link) only accepts to_link in {0,1}."); 71 | 72 | MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2); 73 | MatrixXd J = J_holonomic*constraint_jacobian(q(2)); 74 | return J.block(0,0,8,to_link+1); 75 | } 76 | 77 | MatrixXd DQ_DifferentialDriveRobot::pose_jacobian(const VectorXd &q) const 78 | { 79 | // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics 80 | // The size of the configuration space is three but there is one constraint, so there are only 81 | // to columns in the pose_jacobian 82 | return pose_jacobian(q,1); 83 | } 84 | 85 | 86 | /** 87 | * @brief returns the pose Jacobian derivative 88 | * @param q The VectorXd representing the robot configuration. 89 | * @param q_dot The VectorXd representing the robot configuration velocities. 90 | * @param to_link The ith link which we want to compute the Jacobian derivative. 91 | * @return a MatrixXd representing the desired Jacobian 92 | */ 93 | MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const 94 | { 95 | if(to_link!=0 && to_link!=1) 96 | throw std::runtime_error("DQ_DifferentialDriveRobot::pose_jacobian_derivative(q,q_dot, to_link) only accepts to_link in {0,1}."); 97 | 98 | /// Requirements 99 | MatrixXd J_holonomic = DQ_HolonomicBase::pose_jacobian(q,2); 100 | MatrixXd J_holonomic_dot = DQ_HolonomicBase::pose_jacobian_derivative(q,q_dot,2); 101 | MatrixXd J_c = constraint_jacobian(q(2)); 102 | MatrixXd J_c_dot = constraint_jacobian_derivative(q(2), q_dot(2)); 103 | MatrixXd J_dot = J_holonomic_dot*J_c + J_holonomic*J_c_dot; 104 | return J_dot.block(0,0,8,to_link+1); 105 | } 106 | 107 | /** 108 | * @brief returns the pose Jacobian derivative 109 | * @param q The VectorXd representing the robot configuration. 110 | * @param q_dot The VectorXd representing the robot configuration velocities. 111 | * @return a MatrixXd representing the desired Jacobian 112 | */ 113 | MatrixXd DQ_DifferentialDriveRobot::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const 114 | { 115 | // The DQ_DifferentialDriveRobot works differently from most other subclasses of DQ_Kinematics 116 | // The size of the configuration space is three but there is one constraint, so there are only 117 | // to columns in the pose_jacobian 118 | return pose_jacobian_derivative(q, q_dot, 1); 119 | } 120 | 121 | } 122 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulatorDenso.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2011-2025 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 (juanjose.quirozomana@manchester.ac.uk) 24 | - Added the get_supported_joint_types() method. 25 | */ 26 | 27 | #include 28 | namespace DQ_robotics 29 | { 30 | 31 | DQ_SerialManipulatorDenso::DQ_SerialManipulatorDenso(const MatrixXd& denso_matrix): 32 | DQ_SerialManipulator(denso_matrix.cols()) 33 | { 34 | if(denso_matrix.rows() != 6) 35 | { 36 | throw(std::range_error("Bad DQ_SerialManipulatorDenso(MatrixXd) call: denso_matrix should be 6xn")); 37 | } 38 | denso_matrix_ = denso_matrix; 39 | set_joint_types(std::vector(get_dim_configuration_space(), DQ_JointType::REVOLUTE)); 40 | } 41 | 42 | DQ DQ_SerialManipulatorDenso::_denso2dh(const double &q, const int &ith) const 43 | { 44 | const double& a = denso_matrix_(0,ith); 45 | const double& b = denso_matrix_(1,ith); 46 | const double& d = denso_matrix_(2,ith); 47 | const double& alpha = denso_matrix_(3,ith); 48 | const double& beta = denso_matrix_(4,ith); 49 | const double& gamma = denso_matrix_(5,ith); 50 | 51 | DQ z_rot = cos((gamma + q)/2.0) + k_*sin((gamma + q)/2.0); 52 | DQ q_t = 1.0+0.5*E_*(a*i_+b*j_+d*k_); 53 | DQ q_alpha = cos(alpha/2.)+i_*sin(alpha/2.); 54 | DQ q_beta = cos(beta/2.)+j_*sin(beta/2.); 55 | 56 | return z_rot*q_t*q_alpha*q_beta; 57 | } 58 | 59 | /** 60 | * @brief DQ_SerialManipulatorDenso::get_supported_joint_types gets the supported joint types. 61 | * @return A vector containing the supported joint types. 62 | */ 63 | std::vector DQ_SerialManipulatorDenso::get_supported_joint_types() const 64 | { 65 | return {DQ_JointType::REVOLUTE}; 66 | } 67 | 68 | VectorXd DQ_SerialManipulatorDenso::get_as() const 69 | { 70 | return denso_matrix_.row(0); 71 | } 72 | 73 | VectorXd DQ_SerialManipulatorDenso::get_bs() const 74 | { 75 | return denso_matrix_.row(1); 76 | } 77 | 78 | VectorXd DQ_SerialManipulatorDenso::get_ds() const 79 | { 80 | return denso_matrix_.row(2); 81 | } 82 | 83 | VectorXd DQ_SerialManipulatorDenso::get_alphas() const 84 | { 85 | return denso_matrix_.row(3); 86 | } 87 | 88 | VectorXd DQ_SerialManipulatorDenso::get_betas() const 89 | { 90 | return denso_matrix_.row(4); 91 | } 92 | 93 | VectorXd DQ_SerialManipulatorDenso::get_gammas() const 94 | { 95 | return denso_matrix_.row(5); 96 | } 97 | 98 | DQ DQ_SerialManipulatorDenso::raw_fkm(const VectorXd& q_vec, const int& to_ith_link) const 99 | { 100 | _check_q_vec(q_vec); 101 | _check_to_ith_link(to_ith_link); 102 | 103 | DQ q(1); 104 | int j = 0; 105 | for (int i = 0; i < (to_ith_link+1); i++) { 106 | q = q * _denso2dh(q_vec(i-j), i); 107 | } 108 | return q; 109 | } 110 | 111 | MatrixXd DQ_SerialManipulatorDenso::raw_pose_jacobian(const VectorXd &q_vec, const int &to_ith_link) const 112 | { 113 | _check_q_vec(q_vec); 114 | _check_to_ith_link(to_ith_link); 115 | 116 | MatrixXd J = MatrixXd::Zero(8,to_ith_link+1); 117 | DQ x_effector = raw_fkm(q_vec,to_ith_link); 118 | 119 | DQ x(1); 120 | 121 | for(int i=0;i. 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | namespace DQ_robotics 27 | { 28 | 29 | 30 | DQ_NumericalFilteredPseudoinverseController::DQ_NumericalFilteredPseudoinverseController(DQ_Kinematics *robot): 31 | DQ_PseudoinverseController (robot), 32 | epsilon_(0), 33 | lambda_max_(0), 34 | last_jacobian_rank_(-1) 35 | { 36 | 37 | } 38 | 39 | DQ_NumericalFilteredPseudoinverseController::DQ_NumericalFilteredPseudoinverseController(const std::shared_ptr &robot): 40 | DQ_PseudoinverseController (robot), 41 | epsilon_(0), 42 | lambda_max_(0), 43 | last_jacobian_rank_(-1) 44 | { 45 | 46 | } 47 | 48 | VectorXd DQ_NumericalFilteredPseudoinverseController::compute_setpoint_control_signal(const VectorXd& q, const VectorXd& task_reference) 49 | { 50 | return DQ_NumericalFilteredPseudoinverseController::compute_tracking_control_signal(q, task_reference, VectorXd::Zero(task_reference.size())); 51 | } 52 | 53 | VectorXd DQ_NumericalFilteredPseudoinverseController::compute_tracking_control_signal(const VectorXd &q, const VectorXd &task_reference, const VectorXd &feed_forward) 54 | { 55 | //Trivial lambda_max_ and epsilon_ mean it can be calculated using the DQ_PseudoinverseController 56 | if(lambda_max_==0 || epsilon_==0) 57 | return DQ_PseudoinverseController::compute_tracking_control_signal(q,task_reference,feed_forward); 58 | 59 | if(is_set()) 60 | { 61 | const VectorXd& task_variable = get_task_variable(q); 62 | const VectorXd& task_error = task_variable-task_reference; 63 | 64 | const MatrixXd& J = get_jacobian(q); 65 | const int& jacobian_rank = rank(J); 66 | last_jacobian_rank_ = jacobian_rank; 67 | const int max_rank = std::min(J.rows(),J.cols()); 68 | if(jacobian_rank==max_rank) 69 | { 70 | //When the matrix is full rank, it can be calculated using DQ_PseudoinverseController 71 | return DQ_PseudoinverseController::compute_tracking_control_signal(q,task_reference,feed_forward); 72 | } 73 | 74 | MatrixXd U, S, V; 75 | std::tie(U,S,V) = svd(J); 76 | last_jacobian_svd_ = {U,S,V}; 77 | MatrixXd total_filtered_damping; 78 | for(auto i=0;i= 0 and not " 129 | +std::to_string(singular_region_size) 130 | +"."); 131 | epsilon_ = singular_region_size; 132 | } 133 | 134 | double DQ_NumericalFilteredPseudoinverseController::get_maximum_numerical_filtered_damping() const 135 | { 136 | return lambda_max_; 137 | } 138 | 139 | double DQ_NumericalFilteredPseudoinverseController::get_singular_region_size() const 140 | { 141 | return epsilon_; 142 | } 143 | 144 | MatrixXd DQ_NumericalFilteredPseudoinverseController::get_last_filtered_damping() const 145 | { 146 | return last_filtered_damping_; 147 | } 148 | 149 | int DQ_NumericalFilteredPseudoinverseController::get_last_jacobian_rank() const 150 | { 151 | return last_jacobian_rank_; 152 | } 153 | 154 | std::tuple DQ_NumericalFilteredPseudoinverseController::get_last_jacobian_svd() const 155 | { 156 | return last_jacobian_svd_; 157 | } 158 | 159 | } 160 | 161 | -------------------------------------------------------------------------------- /src/robots/FrankaEmikaPandaRobot.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2022 DQ Robotics Developers 3 | This file is part of DQ Robotics. 4 | DQ Robotics is free software: you can redistribute it and/or modify 5 | it under the terms of the GNU Lesser General Public License as published by 6 | the Free Software Foundation, either version 3 of the License, or 7 | (at your option) any later version. 8 | DQ Robotics is distributed in the hope that it will be useful, 9 | but WITHOUT ANY WARRANTY; without even the implied warranty of 10 | MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the 11 | GNU Lesser General Public License for more details. 12 | You should have received a copy of the GNU Lesser General Public License 13 | along with DQ Robotics. If not, see . 14 | Contributors: 15 | - Juan Jose Quiroz Omana (juanjqo@g.ecc.u-tokyo.ac.jp) 16 | */ 17 | 18 | #include 19 | #include 20 | #include 21 | 22 | namespace DQ_robotics 23 | { 24 | 25 | ///**************************************************************************************** 26 | /// PRIVATE FUNCTIONS 27 | /// *************************************************************************************** 28 | /** 29 | * @brief _get_mdh_matrix returns a matrix related to the modified D-H parameters of the 30 | * Franka Emika Panda robot, which is 31 | * defined as 32 | * 33 | * Matrix raw_franka_mdh; 34 | * raw_franka_mdh << theta, 35 | * d, 36 | * a, 37 | * alpha, 38 | * type_of_joints; 39 | * Source: https://frankaemika.github.io/docs/control_parameters.html 40 | * 41 | * @return MatrixXd raw_franka_mdh a matrix related to the modified D-H parameters 42 | */ 43 | MatrixXd _get_mdh_matrix() 44 | { 45 | const double pi2 = pi/2.0; 46 | Matrix raw_franka_mdh(5,7); 47 | raw_franka_mdh << 0, 0, 0, 0, 0, 0, 0, 48 | 0.333, 0, 3.16e-1, 0, 3.84e-1, 0, 0, 49 | 0, 0, 0, 8.25e-2, -8.25e-2, 0, 8.8e-2, 50 | 0, -pi2, pi2, pi2, -pi2, pi2, pi2, 51 | 0, 0, 0, 0, 0, 0, 0; 52 | 53 | return raw_franka_mdh; 54 | } 55 | 56 | 57 | /** 58 | * @brief _get_offset_base returns the base offset of the Franka Emika Panda robot. 59 | * @return a unit dual quatenion representing the base offset of the robot. 60 | */ 61 | DQ _get_offset_base() 62 | { 63 | return 1 + E_ * 0.5 * DQ(0, 0.0413, 0, 0); 64 | } 65 | 66 | /** 67 | * @brief _get_offset_flange returns the end-effector offset of the Franka Emika Panda robot, 68 | * which is called "Flange" by the manufacturer. 69 | * This offset does not correspond to the end-effector tool but is part 70 | * of the modeling based on Craig's convention. 71 | * Source: https://frankaemika.github.io/docs/control_parameters.html 72 | * @return 73 | */ 74 | DQ _get_offset_flange() 75 | { 76 | return 1+E_*0.5*k_*1.07e-1; 77 | } 78 | 79 | /** 80 | * @brief _get_q_limits returns the joint limits of the Franka Emika Panda robot 81 | * as suggested by the manufacturer. 82 | * 83 | * source: https://frankaemika.github.io/docs/control_parameters.html 84 | * 85 | * @return a std::make_tuple(q_min_, q_max_) containing the joint limits. 86 | */ 87 | std::tuple _get_q_limits() 88 | { 89 | const VectorXd q_max_ = ((VectorXd(7) << 2.3093, 1.5133, 2.4937, -0.4461, 2.4800, 4.2094, 2.6895).finished()); 90 | const VectorXd q_min_ = ((VectorXd(7) << -2.3093,-1.5133,-2.4937, -2.7478,-2.4800, 0.8521, -2.6895).finished()); 91 | return std::make_tuple(q_min_, q_max_); 92 | } 93 | 94 | 95 | /** 96 | * @brief _get_q_dot_limits returns the joint velocity limits of the Franka Emika Panda robot 97 | * as suggested by the manufacturer. 98 | * 99 | * source: https://frankaemika.github.io/docs/control_parameters.html 100 | * 101 | * @return a std::make_tuple(q_min_dot_, q_max_dot_) containing the joint velocity limits. 102 | */ 103 | std::tuple _get_q_dot_limits() 104 | { 105 | const VectorXd q_min_dot_ = ((VectorXd(7) << -2, -1, -1.5, -1.25, -3, -1.5, -3).finished()); 106 | const VectorXd q_max_dot_ = ((VectorXd(7) << 2, 1, 1.5, 1.25, 3, 1.5, 3).finished()); 107 | return std::make_tuple(q_min_dot_, q_max_dot_); 108 | } 109 | 110 | 111 | 112 | 113 | ///**************************************************************************************** 114 | /// PUBLIC FUNCTIONS 115 | /// *************************************************************************************** 116 | 117 | 118 | /** 119 | * @brief FrankaEmikaPandaRobot::kinematics returns an object of the class DQ_SerialManipulatorMDH() 120 | * that contain the kinematic model of the Franka Emika Panda robot. 121 | * Example of use: 122 | * 123 | * #include 124 | * DQ_SerialManipulatorMDH franka = FrankaEmikaPandaRobot::kinematics(); 125 | * DQ x = franka.fkm(VectorXd::Zero(franka.get_dim_configuration_space())); 126 | * 127 | * @return A DQ_SerialManipulatorMDH() object of the desire robot 128 | */ 129 | DQ_SerialManipulatorMDH FrankaEmikaPandaRobot::kinematics() 130 | { 131 | DQ_SerialManipulatorMDH franka(_get_mdh_matrix()); 132 | franka.set_base_frame(_get_offset_base()); 133 | franka.set_reference_frame(_get_offset_base()); 134 | franka.set_effector(_get_offset_flange()); 135 | VectorXd q_min; 136 | VectorXd q_max; 137 | VectorXd q_dot_min; 138 | VectorXd q_dot_max; 139 | std::tie(q_min, q_max) = _get_q_limits(); 140 | std::tie(q_dot_min, q_dot_max) = _get_q_dot_limits(); 141 | franka.set_lower_q_limit(q_min); 142 | franka.set_upper_q_limit(q_max); 143 | franka.set_lower_q_dot_limit(q_dot_min); 144 | franka.set_upper_q_dot_limit(q_dot_max); 145 | return franka; 146 | } 147 | 148 | } 149 | 150 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_HolonomicBase.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 24 | 25 | namespace DQ_robotics 26 | { 27 | 28 | DQ_HolonomicBase::DQ_HolonomicBase() 29 | { 30 | dim_configuration_space_ = 3; 31 | } 32 | 33 | DQ DQ_HolonomicBase::raw_fkm(const VectorXd& q) const 34 | { 35 | const double& x = q(0); 36 | const double& y = q(1); 37 | const double& phi = q(2); 38 | 39 | double c = cos(phi/2.0); 40 | double s = sin(phi/2.0); 41 | 42 | DQ real_part = c + k_*s; 43 | DQ dual_part = 0.5*i_*(x*c + y*s) + 0.5*j_*(-x*s + y*c); 44 | 45 | return real_part + E_*dual_part; 46 | } 47 | 48 | DQ DQ_HolonomicBase::fkm(const VectorXd& q) const 49 | { 50 | return raw_fkm(q)*frame_displacement_; 51 | } 52 | 53 | DQ DQ_HolonomicBase::fkm(const VectorXd &q, const int& to_ith_link) const 54 | { 55 | if(to_ith_link != 2) 56 | { 57 | throw std::runtime_error(std::string("DQ_HolonomicBase(q,to_ith_link) only accepts to_ith_link=2")); 58 | } 59 | return fkm(q); 60 | } 61 | 62 | MatrixXd DQ_HolonomicBase::raw_pose_jacobian(const VectorXd& q, const int& to_link) const 63 | { 64 | if(to_link >= 3 || to_link < 0) 65 | { 66 | throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable.")); 67 | } 68 | 69 | const double& x = q(0); 70 | const double& y = q(1); 71 | const double& phi = q(2); 72 | 73 | const double c = cos(phi/2.0); 74 | const double s = sin(phi/2.0); 75 | 76 | const double j71 = -0.5*s; 77 | const double j62 = -j71; 78 | const double j13 = -j62; 79 | 80 | const double j72 = 0.5*c; 81 | const double j61 = j72; 82 | const double j43 = j61; 83 | 84 | const double j63 = 0.25*(-x*s + y*c); 85 | 86 | const double j73 = 0.25*(-x*c - y*s); 87 | 88 | MatrixXd J(8,3); 89 | J << 0.0, 0.0, j13, 90 | 0.0, 0.0, 0.0, 91 | 0.0, 0.0, 0.0, 92 | 0.0, 0.0, j43, 93 | 0.0, 0.0, 0.0, 94 | j61, j62, j63, 95 | j71, j72, j73, 96 | 0.0, 0.0, 0.0; 97 | return J.block(0,0,8,to_link+1); 98 | } 99 | 100 | MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q, const int &to_link) const 101 | { 102 | return haminus8(frame_displacement_)*raw_pose_jacobian(q,to_link); 103 | } 104 | 105 | MatrixXd DQ_HolonomicBase::pose_jacobian(const VectorXd &q) const 106 | { 107 | return pose_jacobian(q,get_dim_configuration_space()-1); 108 | } 109 | 110 | /** 111 | * @brief returns the Jacobian derivative (J_dot) that satisfies 112 | vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time 113 | derivative of the pose and 'q_dot' represents the robot configuration velocities. 114 | This method does not take into account the base displacement. 115 | * @param q The VectorXd representing the robot configurations. 116 | * @param q_dot The VectorXd representing the robot configuration velocities. 117 | * @param to_link The ith link which we want to compute the Jacobian derivative. 118 | * @return a MatrixXd representing the desired Jacobian derivative. 119 | */ 120 | MatrixXd DQ_HolonomicBase::raw_pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const 121 | { 122 | if(to_link >= 3 || to_link < 0) 123 | { 124 | throw std::runtime_error(std::string("Tried to access link index ") + std::to_string(to_link) + std::string(" which is unnavailable.")); 125 | } 126 | 127 | const double& x = q(0); 128 | const double& y = q(1); 129 | const double& phi = q(2); 130 | 131 | const double& x_dot = q_dot(0); 132 | const double& y_dot = q_dot(1); 133 | const double& phi_dot = q_dot(2); 134 | 135 | const double c = cos(phi/2.0); 136 | const double s = sin(phi/2.0); 137 | 138 | const double j71 = -0.25*c*phi_dot; 139 | const double j62 = -j71; 140 | const double j13 = -j62; 141 | 142 | const double j72 = -0.25*s*phi_dot; 143 | const double j61 = j72; 144 | const double j43 = j61; 145 | 146 | const double j63 = 0.25 * (-x_dot*s - 0.5*x*c*phi_dot + y_dot*c - 0.5*y*s*phi_dot); 147 | const double j73 = 0.25 * (-x_dot*c + 0.5*x*s*phi_dot - y_dot*s - 0.5*y*c*phi_dot); 148 | 149 | MatrixXd J_dot(8,3); 150 | J_dot << 0.0, 0.0, j13, 151 | 0.0, 0.0, 0.0, 152 | 0.0, 0.0, 0.0, 153 | 0.0, 0.0, j43, 154 | 0.0, 0.0, 0.0, 155 | j61, j62, j63, 156 | j71, j72, j73, 157 | 0.0, 0.0, 0.0; 158 | return J_dot.block(0,0,8,to_link+1); 159 | } 160 | 161 | /** 162 | * @brief returns the Jacobian derivative 'J_dot' that satisfies 163 | vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time 164 | derivative of the pose and 'q_dot' represents the robot configuration velocities. 165 | * @param q The VectorXd representing the robot configurations. 166 | * @param q_dot The VectorXd representing the robot configuration velocities. 167 | * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative. 168 | * @return a MatrixXd representing the desired Jacobian derivative. 169 | */ 170 | MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_link) const 171 | { 172 | return haminus8(frame_displacement_)*raw_pose_jacobian_derivative(q, q_dot, to_link); 173 | } 174 | 175 | /** 176 | * @brief returns the Jacobian derivative 'J_dot' that satisfies 177 | vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time 178 | derivative of the pose and 'q_dot' represents the robot configuration velocities. 179 | * @param q The VectorXd representing the robot configurations. 180 | * @param q_dot The VectorXd representing the robot configuration velocities. 181 | * @return a MatrixXd representing the desired Jacobian derivative. 182 | */ 183 | MatrixXd DQ_HolonomicBase::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const 184 | { 185 | return pose_jacobian_derivative(q, q_dot, get_dim_configuration_space()-1); 186 | } 187 | 188 | } 189 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_WholeBody.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 24 | #include 25 | #include 26 | 27 | namespace DQ_robotics 28 | { 29 | 30 | void DQ_WholeBody::_check_to_ith_chain(const int &to_ith_chain) const 31 | { 32 | if(to_ith_chain >= static_cast(chain_.size()) || to_ith_chain < 0) 33 | { 34 | throw std::runtime_error(std::string("Tried to access chain index ") + std::to_string(to_ith_chain) + std::string(" which is unnavailable.")); 35 | } 36 | } 37 | 38 | DQ_WholeBody::DQ_WholeBody(std::shared_ptr robot) 39 | { 40 | chain_.push_back(robot); 41 | dim_configuration_space_ = robot->get_dim_configuration_space(); 42 | } 43 | 44 | DQ_Kinematics* DQ_WholeBody::get_chain(const int& to_ith_chain) 45 | { 46 | _check_to_ith_chain(to_ith_chain); 47 | 48 | return chain_[static_cast::size_type>(to_ith_chain)].get(); 49 | } 50 | 51 | DQ_SerialManipulatorDH DQ_WholeBody::get_chain_as_serial_manipulator_dh(const int &to_ith_chain) const 52 | { 53 | _check_to_ith_chain(to_ith_chain); 54 | 55 | try 56 | { 57 | return DQ_SerialManipulatorDH(*dynamic_cast(chain_[static_cast::size_type>(to_ith_chain)].get())); 58 | } catch (const std::bad_cast& e) 59 | { 60 | throw std::runtime_error("Index requested in get_chain_as_serial_manipulator_dh is not a DQ_SerialManipulatorDH" + std::string(e.what())); 61 | } 62 | } 63 | 64 | DQ_HolonomicBase DQ_WholeBody::get_chain_as_holonomic_base(const int &to_ith_chain) const 65 | { 66 | _check_to_ith_chain(to_ith_chain); 67 | 68 | try 69 | { 70 | return DQ_HolonomicBase(*dynamic_cast(chain_[static_cast::size_type>(to_ith_chain)].get())); 71 | } catch (const std::bad_cast& e) 72 | { 73 | throw std::runtime_error("Index requested in get_chain_as_holonomic_base is not a DQ_HolonomicBase" + std::string(e.what())); 74 | } 75 | } 76 | 77 | void DQ_WholeBody::add(std::shared_ptr robot) 78 | { 79 | dim_configuration_space_+= robot->get_dim_configuration_space(); 80 | chain_.push_back(robot); 81 | } 82 | 83 | DQ DQ_WholeBody::fkm(const VectorXd& q) const 84 | { 85 | return reference_frame_ * raw_fkm(q); 86 | } 87 | 88 | DQ DQ_WholeBody::fkm(const VectorXd& q, const int& to_chain) const 89 | { 90 | return reference_frame_ * raw_fkm(q,to_chain); 91 | } 92 | 93 | DQ DQ_WholeBody::raw_fkm(const VectorXd &q) const 94 | { 95 | return raw_fkm(q,chain_.size()-1); 96 | } 97 | 98 | DQ DQ_WholeBody::raw_fkm(const VectorXd &q, const int &to_ith_chain) const 99 | { 100 | _check_to_ith_chain(to_ith_chain); 101 | 102 | DQ pose(1); 103 | 104 | int q_counter = 0; 105 | for(int i=0;iget_dim_configuration_space(); 108 | const VectorXd current_robot_q = q.segment(q_counter,current_robot_dim); 109 | pose = pose * chain_[i]->fkm(current_robot_q); 110 | q_counter += current_robot_dim; 111 | } 112 | 113 | return pose; 114 | } 115 | 116 | MatrixXd DQ_WholeBody::pose_jacobian(const VectorXd &q, const int &to_ith_chain) const 117 | { 118 | _check_q_vec(q); 119 | _check_to_ith_chain(to_ith_chain); 120 | 121 | int n = chain_.size(); 122 | DQ x_0_to_n = fkm(q,n-1); 123 | int q_counter = 0; 124 | 125 | //Not a good implementation but similar to MATLAB 126 | std::vector J_vector; 127 | for(int i=0;iget_dim_configuration_space(); 130 | 131 | const DQ x_0_to_iplus1 = fkm(q,i); 132 | const DQ x_iplus1_to_n = conj(x_0_to_iplus1)*x_0_to_n; 133 | 134 | const VectorXd q_iplus1 = q.segment(q_counter,dim); 135 | q_counter += dim; 136 | 137 | if(i==0) 138 | J_vector.push_back(haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1)); 139 | else 140 | J_vector.push_back(hamiplus8(fkm(q,i-1))*haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1)); 141 | } 142 | 143 | MatrixXd J_pose(8,q_counter); 144 | int col_counter = 0; 145 | for(int i=0;iget_dim_configuration_space(); 148 | MatrixXd current_J = J_vector[i]; 149 | for(int j=0;j(chain_[chain_.size()-1])->set_effector(effector); 168 | } 169 | catch (const std::bad_cast& e) 170 | { 171 | throw std::runtime_error("The last element of the chain needs to be a DQ_SerialManipulator to use set_effector in a DQ_WholeBody " + std::string(e.what())); 172 | } 173 | } 174 | 175 | /** 176 | * @brief returns the Jacobian derivative 'J_dot' that satisfies 177 | vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time 178 | derivative of the pose and 'q_dot' represents the robot configuration velocities. 179 | * @param q The VectorXd representing the robot configurations. 180 | * @param q_dot The VectorXd representing the robot configuration velocities. 181 | * @param to_ith_link The 'to_ith_link' link which we want to compute the Jacobian derivative. 182 | * @return a MatrixXd representing the desired Jacobian derivative. 183 | */ 184 | MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot, const int &to_ith_link) const 185 | { 186 | throw std::runtime_error("pose_jacobian_derivative is not implemented yet."); 187 | } 188 | 189 | 190 | /** 191 | * @brief returns the Jacobian derivative 'J_dot' that satisfies 192 | vec8(pose_dot_dot) = J_dot * q_dot + J*q_dot_dot, where pose = fkm(), 'pose_dot' is the time 193 | derivative of the pose and 'q_dot' represents the robot configuration velocities. 194 | * @param q The VectorXd representing the robot configurations. 195 | * @param q_dot The VectorXd representing the robot configuration velocities. 196 | * @return a MatrixXd representing the desired Jacobian derivative. 197 | */ 198 | MatrixXd DQ_WholeBody::pose_jacobian_derivative(const VectorXd &q, const VectorXd &q_dot) const 199 | { 200 | return pose_jacobian_derivative(q,q_dot, get_dim_configuration_space()-1); 201 | } 202 | 203 | } 204 | -------------------------------------------------------------------------------- /include/dqrobotics/robot_modeling/DQ_Kinematics.h: -------------------------------------------------------------------------------- 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 | #ifndef DQ_ROBOT_MODELLING_DQ_KINEMATICS_H 24 | #define DQ_ROBOT_MODELLING_DQ_KINEMATICS_H 25 | 26 | #include 27 | 28 | namespace DQ_robotics 29 | { 30 | class DQ_Kinematics 31 | { 32 | protected: 33 | std::string name_; 34 | 35 | DQ reference_frame_; 36 | DQ base_frame_; 37 | 38 | VectorXd q_; 39 | int dim_configuration_space_; 40 | 41 | VectorXd lower_q_limit_; 42 | VectorXd upper_q_limit_; 43 | VectorXd lower_q_dot_limit_; 44 | VectorXd upper_q_dot_limit_; 45 | 46 | void _check_to_ith_link(const int& to_ith_link) const; 47 | void _check_q_vec(const VectorXd& q_vec) const; 48 | 49 | //Constructor 50 | DQ_Kinematics(); 51 | public: 52 | //Virtual destructor 53 | virtual ~DQ_Kinematics() = default; 54 | 55 | //Concrete methods 56 | void set_reference_frame(const DQ& get_reference_frame); 57 | DQ get_reference_frame() const; 58 | void set_base_frame(const DQ& get_base_frame); 59 | DQ get_base_frame() const; 60 | void set_name(const std::string& get_name); 61 | std::string get_name() const; 62 | 63 | //PURE virtual methods 64 | virtual DQ fkm (const VectorXd& joint_configurations) const = 0; 65 | virtual DQ fkm (const VectorXd& joint_configurations, const int& to_ith_link) const = 0; 66 | virtual MatrixXd pose_jacobian(const VectorXd& joint_configurations, const int& to_ith_link) const = 0; 67 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, 68 | const VectorXd& q_dot, 69 | const int& to_ith_link) const = 0; 70 | //Virtual methods 71 | virtual MatrixXd pose_jacobian (const VectorXd& joint_configurations) const; 72 | virtual MatrixXd pose_jacobian_derivative(const VectorXd& q, 73 | const VectorXd& q_dot) const; 74 | virtual int get_dim_configuration_space() const; 75 | 76 | //Static methods 77 | static MatrixXd distance_jacobian (const MatrixXd& pose_jacobian, const DQ& pose); 78 | static MatrixXd translation_jacobian (const MatrixXd& pose_jacobian, const DQ& pose); 79 | static MatrixXd rotation_jacobian (const MatrixXd& pose_jacobian); 80 | static MatrixXd line_jacobian (const MatrixXd& pose_jacobian, const DQ& pose, const DQ& line_direction); 81 | static MatrixXd plane_jacobian (const MatrixXd& pose_jacobian, const DQ& pose, const DQ& plane_normal); 82 | static MatrixXd rotation_jacobian_derivative (const MatrixXd& pose_jacobian_derivative); 83 | static MatrixXd translation_jacobian_derivative (const MatrixXd& pose_jacobian, 84 | const MatrixXd& pose_jacobian_derivative, 85 | const DQ& pose, 86 | const VectorXd &q_dot); 87 | static MatrixXd distance_jacobian_derivative (const MatrixXd& pose_jacobian, 88 | const MatrixXd& pose_jacobian_derivative, 89 | const DQ& pose, 90 | const VectorXd &q_dot); 91 | static MatrixXd plane_jacobian_derivative (const MatrixXd& pose_jacobian, 92 | const MatrixXd& pose_jacobian_derivative, 93 | const DQ& pose, 94 | const DQ& plane_normal, 95 | const VectorXd &q_dot); 96 | static MatrixXd line_jacobian_derivative (const MatrixXd& pose_jacobian, 97 | const MatrixXd& pose_jacobian_derivative, 98 | const DQ& pose, 99 | const DQ& line_direction, 100 | const VectorXd &q_dot); 101 | static MatrixXd point_to_point_distance_jacobian(const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_point); 102 | static double point_to_point_residual (const DQ& robot_point, const DQ& workspace_point, const DQ& workspace_point_derivative); 103 | static MatrixXd point_to_line_distance_jacobian (const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_line); 104 | static double point_to_line_residual (const DQ& robot_point, const DQ& workspace_line, const DQ& workspace_line_derivative); 105 | static MatrixXd point_to_plane_distance_jacobian(const MatrixXd& translation_jacobian, const DQ& robot_point, const DQ& workspace_plane); 106 | static double point_to_plane_residual (const DQ& translation, const DQ& plane_derivative); 107 | static MatrixXd line_to_point_distance_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_point); 108 | static double line_to_point_residual (const DQ& robot_line, const DQ& workspace_point, const DQ& workspace_point_derivative); 109 | static MatrixXd line_to_line_distance_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_line); 110 | static double line_to_line_residual (const DQ& robot_line, const DQ& workspace_line, const DQ& workspace_line_derivative); 111 | static MatrixXd plane_to_point_distance_jacobian(const MatrixXd& plane_jacobian, const DQ& workspace_point); 112 | static double plane_to_point_residual (const DQ& robot_plane, const DQ& workspace_point_derivative); 113 | static MatrixXd line_to_line_angle_jacobian (const MatrixXd& line_jacobian, const DQ& robot_line, const DQ& workspace_line); 114 | static double line_to_line_angle_residual (const DQ& robot_line, const DQ& workspace_line, const DQ& workspace_line_derivative); 115 | 116 | static MatrixXd line_segment_to_line_segment_distance_jacobian(const MatrixXd& line_jacobian, 117 | const MatrixXd& robot_point_1_translation_jacobian, 118 | const MatrixXd& robot_point_2_translation_jacobian, 119 | const DQ& robot_line, 120 | const DQ& robot_point_1, 121 | const DQ& robot_point_2, 122 | const DQ& workspace_line, 123 | const DQ& workspace_point_1, 124 | const DQ& workspace_point_2); 125 | 126 | }; 127 | } 128 | 129 | #endif 130 | -------------------------------------------------------------------------------- /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 | -------------------------------------------------------------------------------- /src/robot_control/DQ_KinematicController.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 24 | #include 25 | #include 26 | 27 | namespace DQ_robotics 28 | { 29 | 30 | DQ_Kinematics *DQ_KinematicController::_get_robot_ptr() const 31 | { 32 | return robot_sptr_ ? robot_sptr_.get() : robot_; 33 | } 34 | 35 | std::shared_ptr DQ_KinematicController::_get_robot() const 36 | { 37 | if(!robot_sptr_) 38 | throw std::runtime_error("DQ_KinematicController::_get_robot invalid robot pointer"); 39 | return robot_sptr_; 40 | } 41 | 42 | DQ_KinematicController::DQ_KinematicController(DQ_Kinematics* robot): 43 | DQ_KinematicController() 44 | { 45 | robot_ = robot; 46 | } 47 | 48 | DQ_KinematicController::DQ_KinematicController(const std::shared_ptr &robot): 49 | DQ_KinematicController() 50 | { 51 | robot_sptr_ = robot; 52 | } 53 | 54 | DQ_KinematicController::DQ_KinematicController(): 55 | robot_(nullptr), 56 | control_objective_(ControlObjective::None), 57 | attached_primitive_(0.0), 58 | target_primitive_(0.0), 59 | gain_(0.0), 60 | damping_(0),//Todo: change this inialization to use empty vector 61 | system_reached_stable_region_(false),//Todo: change this inialization to use empty vector 62 | last_control_signal_(VectorXd::Zero(1)), 63 | last_error_signal_(VectorXd::Zero(1)), 64 | stability_threshold_(0.0), 65 | stability_counter_(0.0), 66 | stability_counter_max_(10.0) 67 | { 68 | 69 | } 70 | 71 | void DQ_KinematicController::verify_stability(const VectorXd& task_error) 72 | { 73 | if((last_error_signal_-task_error).norm() < stability_threshold_) 74 | { 75 | if(stability_counter_ < stability_counter_max_) 76 | stability_counter_++; 77 | } 78 | else 79 | { 80 | reset_stability_counter(); 81 | } 82 | 83 | if(stability_counter_ >= stability_counter_max_) 84 | { 85 | system_reached_stable_region_ = true; 86 | } 87 | } 88 | 89 | ControlObjective DQ_KinematicController::get_control_objective() const 90 | { 91 | return control_objective_; 92 | } 93 | 94 | VectorXd DQ_KinematicController::get_last_error_signal() const 95 | { 96 | return last_error_signal_; 97 | } 98 | 99 | MatrixXd DQ_KinematicController::get_jacobian(const VectorXd &q) const 100 | { 101 | DQ_Kinematics* robot_local =_get_robot_ptr(); 102 | 103 | if(q.size() != robot_local->get_dim_configuration_space()) 104 | throw std::runtime_error("Calling get_jacobian with an incorrect number of joints " + std::to_string(q.size())); 105 | 106 | const MatrixXd J_pose = robot_local->pose_jacobian(q); 107 | const DQ x_pose = robot_local->fkm(q); 108 | 109 | switch(control_objective_) 110 | { 111 | case ControlObjective::None: 112 | throw std::runtime_error("The control objective must be initialized with set_control_objective()"); 113 | 114 | case ControlObjective::Distance: 115 | return DQ_Kinematics::distance_jacobian(J_pose,x_pose); 116 | 117 | case ControlObjective::DistanceToPlane: 118 | { 119 | if(!is_plane(target_primitive_)) 120 | { 121 | throw std::runtime_error("Please set the target plane with the method set_target_primitive()"); 122 | } 123 | MatrixXd Jt = robot_local->translation_jacobian(J_pose, x_pose); 124 | DQ t = translation(x_pose); 125 | return robot_local->point_to_plane_distance_jacobian(Jt, t, target_primitive_); 126 | } 127 | 128 | case ControlObjective::Line: 129 | return DQ_Kinematics::line_jacobian(J_pose,x_pose,attached_primitive_); 130 | 131 | case ControlObjective::Plane: 132 | return DQ_Kinematics::plane_jacobian(J_pose,x_pose,attached_primitive_); 133 | 134 | case ControlObjective::Rotation: 135 | return DQ_Kinematics::rotation_jacobian(J_pose); 136 | 137 | case ControlObjective::Translation: 138 | return DQ_Kinematics::translation_jacobian(J_pose,x_pose); 139 | 140 | case ControlObjective::Pose: 141 | return J_pose; 142 | } 143 | 144 | //The only way I found to fix both possible warnings of either having a default in the switch or not having the default. 145 | throw std::runtime_error("Unknown ControlObjective"); 146 | } 147 | 148 | VectorXd DQ_KinematicController::get_task_variable(const VectorXd &q) const 149 | { 150 | DQ_Kinematics* robot_local =_get_robot_ptr(); 151 | 152 | if(q.size() != robot_local->get_dim_configuration_space()) 153 | throw std::runtime_error("Calling get_task_variable with an incorrect number of joints " + std::to_string(q.size())); 154 | 155 | const DQ x_pose = robot_local->fkm(q); 156 | 157 | switch(control_objective_) 158 | { 159 | case ControlObjective::None: 160 | throw std::runtime_error("The control objective must be initialized with set_control_objective()"); 161 | 162 | case ControlObjective::Distance: 163 | { 164 | VectorXd p = vec4(translation(x_pose)); 165 | return p.transpose()*p; 166 | } 167 | 168 | case ControlObjective::DistanceToPlane: 169 | { 170 | if(!is_plane(target_primitive_)) 171 | { 172 | throw std::runtime_error("Set the target plane with the method set_target_primitive()"); 173 | } 174 | DQ t = translation(x_pose); 175 | VectorXd distance(1); 176 | distance(0)=DQ_Geometry::point_to_plane_distance(t, target_primitive_); 177 | return distance; 178 | } 179 | 180 | case ControlObjective::Line: 181 | return vec8(Ad(x_pose,attached_primitive_)); 182 | 183 | case ControlObjective::Plane: 184 | return vec8(Adsharp(x_pose,attached_primitive_)); 185 | 186 | case ControlObjective::Rotation: 187 | return vec4(rotation(x_pose)); 188 | 189 | case ControlObjective::Translation: 190 | return vec4(translation(x_pose)); 191 | 192 | case ControlObjective::Pose: 193 | return vec8(x_pose); 194 | } 195 | 196 | //The only way I found to fix both possible warnings of either having a default in the switch or not having the default. 197 | throw std::runtime_error("Unknown ControlObjective"); 198 | } 199 | 200 | bool DQ_KinematicController::is_set() const 201 | { 202 | if(control_objective_==ControlObjective::None) 203 | { 204 | return false; 205 | } 206 | else 207 | { 208 | return true; 209 | } 210 | } 211 | 212 | bool DQ_KinematicController::system_reached_stable_region() const 213 | { 214 | return system_reached_stable_region_; 215 | } 216 | 217 | void DQ_KinematicController::set_control_objective(const ControlObjective &control_objective) 218 | { 219 | control_objective_ = control_objective; 220 | 221 | switch(control_objective) 222 | { 223 | case ControlObjective::Distance: //This was intentional https://en.cppreference.com/w/cpp/language/attributes/fallthrough 224 | case ControlObjective::DistanceToPlane: 225 | last_error_signal_ = VectorXd::Zero(1); 226 | break; 227 | case ControlObjective::Line: //This was intentional https://en.cppreference.com/w/cpp/language/attributes/fallthrough 228 | case ControlObjective::Plane: //This was intentional https://en.cppreference.com/w/cpp/language/attributes/fallthrough 229 | case ControlObjective::Pose: 230 | last_error_signal_ = VectorXd::Zero(8); 231 | break; 232 | case ControlObjective::Rotation: 233 | case ControlObjective::Translation: 234 | last_error_signal_ = VectorXd::Zero(4); 235 | break; 236 | case ControlObjective::None: 237 | break; 238 | } 239 | 240 | } 241 | 242 | void DQ_KinematicController::set_gain(const double& gain) 243 | { 244 | gain_ = gain; 245 | } 246 | 247 | double DQ_KinematicController::get_gain() const 248 | { 249 | return gain_; 250 | } 251 | 252 | void DQ_KinematicController::set_stability_threshold(const double &threshold) 253 | { 254 | stability_threshold_ = threshold; 255 | } 256 | 257 | void DQ_KinematicController::set_primitive_to_effector(const DQ &primitive) 258 | { 259 | attached_primitive_ = primitive; 260 | } 261 | 262 | void DQ_KinematicController::set_target_primitive(const DQ &primitive) 263 | { 264 | target_primitive_ = primitive; 265 | } 266 | 267 | void DQ_KinematicController::set_damping(const double &damping) 268 | { 269 | damping_ = damping; 270 | } 271 | 272 | double DQ_KinematicController::get_damping() const 273 | { 274 | return damping_; 275 | } 276 | 277 | void DQ_KinematicController::set_stability_counter_max(const int &max) 278 | { 279 | stability_counter_max_ = max; 280 | } 281 | 282 | void DQ_KinematicController::reset_stability_counter() 283 | { 284 | stability_counter_ = 0; 285 | system_reached_stable_region_ = false; 286 | } 287 | 288 | } 289 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialManipulator.cpp: -------------------------------------------------------------------------------- 1 | /** 2 | (C) Copyright 2011-2025 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 | 22 | 2. Mateus Rodrigues Martins (martinsrmateus@gmail.com) 23 | 24 | 3. Juan Jose Quiroz Omana (juanjose.quirozomana@manchester.ac.uk) 25 | - Added the joint_types member, and the following methods: 26 | _check_joint_types(), and {set,get}_joint_{type, types}. 27 | */ 28 | 29 | #include 30 | #include //library for math functions 31 | #include //For range_error 32 | #include 33 | 34 | namespace DQ_robotics 35 | { 36 | 37 | DQ_SerialManipulator::DQ_SerialManipulator(const int &dim_configuration_space): 38 | DQ_Kinematics() 39 | { 40 | curr_effector_ = DQ(1); 41 | lower_q_limit_.resize(dim_configuration_space); 42 | upper_q_limit_.resize(dim_configuration_space); 43 | lower_q_dot_limit_.resize(dim_configuration_space); 44 | upper_q_dot_limit_.resize(dim_configuration_space); 45 | dim_configuration_space_ = dim_configuration_space; 46 | } 47 | 48 | /** 49 | * @brief DQ_SerialManipulator::_check_joint_types throws an exception if the joint types are 50 | * different from the supported joints. 51 | */ 52 | void DQ_SerialManipulator::_check_joint_types() const 53 | { 54 | std::vector types = get_joint_types(); 55 | std::vector supported_types = get_supported_joint_types(); 56 | std::string msg = "Unsupported joint types. Use valid joint types: "; 57 | std::string msg_type; 58 | std::string ps; 59 | size_t k = supported_types.size(); 60 | size_t n = types.size(); 61 | for (size_t i=0;i DQ_SerialManipulator::get_joint_types() const 162 | { 163 | return joint_types_; 164 | } 165 | 166 | /** 167 | * @brief DQ_SerialManipulator::set_joint_type sets the joint type of the ith joint 168 | * @param joint_type The joint_type. 169 | * @param ith_joint The index to a joint. 170 | */ 171 | void DQ_SerialManipulator::set_joint_type(const DQ_JointType &joint_type, const int &ith_joint) 172 | { 173 | _check_to_ith_link(ith_joint); 174 | joint_types_.at(ith_joint) = joint_type; 175 | _check_joint_types(); 176 | } 177 | 178 | /** 179 | * @brief DQ_SerialManipulator::set_joint_types sets the joint types. 180 | * @param joint_types A vector containing the joint types. 181 | */ 182 | void DQ_SerialManipulator::set_joint_types(const std::vector &joint_types) 183 | { 184 | joint_types_ = joint_types; 185 | _check_joint_types(); 186 | } 187 | 188 | /** 189 | * @brief DQ_SerialManipulator::set_joint_types sets the joint types. 190 | * @param joint_types A vector containing the joint types. 191 | */ 192 | void DQ_SerialManipulator::set_joint_types(const VectorXd &joint_types) 193 | { 194 | for (int i=0;i. 18 | 19 | Contributors: 20 | - Murilo M. Marinho (murilomarinho@ieee.org) 21 | */ 22 | #include 23 | #include 24 | #include 25 | 26 | 27 | namespace DQ_robotics 28 | { 29 | namespace internal 30 | { 31 | 32 | #ifdef DQ_ROBOTICS_INTERNAL_DQ_LINESEGMENT_VERBOSE 33 | constexpr bool _verbose = true; 34 | #else 35 | constexpr bool _verbose = false; 36 | #endif 37 | 38 | bool LineSegment::is_inside_line_segment(const DQ& point, 39 | const Primitives& line_primitives) 40 | { 41 | const auto& p1 = std::get<1>(line_primitives); 42 | const auto& p2 = std::get<2>(line_primitives); 43 | 44 | const double& D_p1 = DQ_Geometry::point_to_point_squared_distance(point,p1); 45 | const double& D_p2 = DQ_Geometry::point_to_point_squared_distance(point,p2); 46 | 47 | const double& segment_size = DQ_Geometry::point_to_point_squared_distance(p1,p2); 48 | 49 | return ((D_p1 < segment_size) && (D_p2 < segment_size)); 50 | } 51 | 52 | std::string LineSegment::to_string(const Element &e) 53 | { 54 | switch(e) 55 | { 56 | case Element::Line: 57 | return std::string("Line"); 58 | case Element::P1: 59 | return std::string("P1"); 60 | case Element::P2: 61 | return std::string("P2"); 62 | default: 63 | throw std::runtime_error("Unknown case."); 64 | } 65 | } 66 | 67 | LineSegment::ClosestElementsAndDistance LineSegment::_update_closest_pair(const ClosestElementsAndDistance ¤t, const ClosestElementsAndDistance &candidate) 68 | { 69 | if(std::isnan(std::get<1>(candidate))) 70 | return current; 71 | if(std::isnan(std::get<1>(current))) 72 | { 73 | if(_verbose) 74 | { 75 | std::cout << "Update to candidate {" 76 | << to_string(std::get<0>(std::get<0>(candidate))) 77 | << "," 78 | << to_string(std::get<1>(std::get<0>(candidate))) 79 | << "}" 80 | << std::endl; 81 | } 82 | return candidate; 83 | } 84 | else 85 | { 86 | if(_verbose) 87 | { 88 | std::cout << "D = " 89 | << std::to_string(std::get<1>(current)) 90 | << " < " 91 | << std::to_string(std::get<1>(candidate)) 92 | << std::endl; 93 | } 94 | if(std::get<1>(candidate) < std::get<1>(current)) 95 | { 96 | if(_verbose) 97 | { 98 | std::cout << "Update to candidate {" 99 | << to_string(std::get<0>(std::get<0>(candidate))) 100 | << "," 101 | << to_string(std::get<1>(std::get<0>(candidate))) 102 | << "}" 103 | << std::endl; 104 | } 105 | return candidate; 106 | } 107 | else 108 | { 109 | if(_verbose) 110 | { 111 | std::cout << "Keep current {" 112 | << to_string(std::get<0>(std::get<0>(candidate))) 113 | << "," 114 | << to_string(std::get<1>(std::get<0>(candidate))) 115 | << "}" 116 | << std::endl; 117 | } 118 | return current; 119 | } 120 | } 121 | } 122 | 123 | double LineSegment::_line_to_point_feasibility_and_distance(const Primitives &line_segment, const DQ &point) 124 | { 125 | bool is_possible = is_inside_line_segment(DQ_Geometry::point_projected_in_line(point,std::get<0>(line_segment)), 126 | line_segment); 127 | if(is_possible) 128 | { 129 | return DQ_Geometry::point_to_line_squared_distance(point, 130 | std::get<0>(line_segment) 131 | ); 132 | } 133 | return std::nan(""); 134 | } 135 | 136 | /** 137 | * @brief LineSegment::closest_elements_between_line_segments. 138 | * Obtain the closest elements between two line segments and their distance. 139 | * A simplified logic is as follows: 140 | * 1. Obtain the closest points between lines. If those points rest in 141 | * the line segments, store the distance in D_i. Otherwise, D_i is invalid. 142 | * 2. Obtain projections of points in the lines. If those projections rest 143 | * in the line segments, store the distance in D_i. Otherwise, D_i is invalid. 144 | * 3. Obtain the distance between the relevant line segment endpoints and 145 | * store them in D_i. 146 | * The closest elements will be defined by the elements with smallest D_i 147 | * which is valid. 148 | * @param line_1_primitives a tuple{line,point,point} describing the line segment. 149 | * @param line_2_primitives a tuple{line,point,point} describing the line segment. 150 | * @return a tuple {{Element,Element},squared_distance}. 151 | */ 152 | LineSegment::ClosestElementsAndDistance 153 | LineSegment::closest_elements_between_line_segments(const Primitives& line_1_primitives, 154 | const Primitives& line_2_primitives) 155 | { 156 | const auto& line_1 = std::get<0>(line_1_primitives); 157 | const auto& line_2 = std::get<0>(line_2_primitives); 158 | 159 | DQ p_lines_1; 160 | DQ p_lines_2; 161 | std::tie(p_lines_1,p_lines_2) = DQ_Geometry::closest_points_between_lines(line_1,line_2); 162 | 163 | const bool& p_lines_1_possible = is_inside_line_segment(p_lines_1,line_1_primitives); 164 | const bool& p_lines_2_possible = is_inside_line_segment(p_lines_2,line_2_primitives); 165 | 166 | double D_line_to_line = std::nan(""); 167 | if(p_lines_1_possible && p_lines_2_possible) 168 | { 169 | D_line_to_line = DQ_Geometry::line_to_line_squared_distance(line_1,line_2); 170 | } 171 | 172 | ClosestElementsAndDistance current_result = {{Element::Line,Element::Line}, 173 | D_line_to_line 174 | }; 175 | 176 | std::vector all_closest_elements = {Element::Line, 177 | Element::P1, 178 | Element::P2}; 179 | 180 | 181 | for(const auto& closest_element_1: all_closest_elements) 182 | { 183 | for(const auto& closest_element_2: all_closest_elements) 184 | { 185 | double distance; 186 | 187 | switch(closest_element_1) 188 | { 189 | case Element::Line: 190 | { 191 | switch(closest_element_2) 192 | { 193 | case Element::Line: 194 | { 195 | continue; 196 | } 197 | case Element::P1: 198 | { 199 | distance = _line_to_point_feasibility_and_distance(line_1_primitives, 200 | std::get<1>(line_2_primitives)); 201 | break; 202 | } 203 | case Element::P2: 204 | { 205 | distance = _line_to_point_feasibility_and_distance(line_1_primitives, 206 | std::get<2>(line_2_primitives)); 207 | break; 208 | } 209 | } 210 | break; 211 | } 212 | case Element::P1: 213 | { 214 | switch(closest_element_2) 215 | { 216 | case Element::Line: 217 | { 218 | distance = _line_to_point_feasibility_and_distance(line_2_primitives, 219 | std::get<1>(line_1_primitives)); 220 | break; 221 | } 222 | case Element::P1: 223 | { 224 | distance = DQ_Geometry::point_to_point_squared_distance( 225 | std::get<1>(line_1_primitives), 226 | std::get<1>(line_2_primitives) 227 | ); 228 | break; 229 | } 230 | case Element::P2: 231 | { 232 | distance = DQ_Geometry::point_to_point_squared_distance( 233 | std::get<1>(line_1_primitives), 234 | std::get<2>(line_2_primitives) 235 | ); 236 | break; 237 | } 238 | } 239 | break; 240 | } 241 | case Element::P2: 242 | { 243 | switch(closest_element_2) 244 | { 245 | case Element::Line: 246 | { 247 | distance = _line_to_point_feasibility_and_distance(line_2_primitives, 248 | std::get<2>(line_1_primitives)); 249 | break; 250 | } 251 | case Element::P1: 252 | { 253 | distance = DQ_Geometry::point_to_point_squared_distance( 254 | std::get<2>(line_1_primitives), 255 | std::get<1>(line_2_primitives) 256 | ); 257 | break; 258 | } 259 | case Element::P2: 260 | { 261 | distance = DQ_Geometry::point_to_point_squared_distance( 262 | std::get<2>(line_1_primitives), 263 | std::get<2>(line_2_primitives) 264 | ); 265 | break; 266 | } 267 | } 268 | break; 269 | } 270 | } 271 | 272 | if(_verbose) 273 | { 274 | std::cout << " ...Checking pair {" << 275 | to_string(closest_element_1) << 276 | "," << 277 | to_string(closest_element_2) << 278 | "}" << 279 | std::endl; 280 | } 281 | current_result = _update_closest_pair(current_result, 282 | {{closest_element_1,closest_element_2},distance}); 283 | 284 | } 285 | } 286 | 287 | if(_verbose) 288 | { 289 | std::cout << "Closest pair is {" << 290 | to_string(std::get<0>(std::get<0>(current_result))) << 291 | "," << 292 | to_string(std::get<1>(std::get<0>(current_result))) << 293 | "}" << 294 | std::endl; 295 | } 296 | 297 | return current_result; 298 | } 299 | 300 | } 301 | } 302 | -------------------------------------------------------------------------------- /src/robot_modeling/DQ_SerialWholeBody.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 | 22 | 2. Juan Jose Quiroz Omana (juanjqogm@gmail.com) 23 | - Fixed bug 61 (https://github.com/dqrobotics/cpp/issues/61) in pose_jacobian method. 24 | 25 | */ 26 | 27 | #include 28 | #include 29 | 30 | namespace DQ_robotics 31 | { 32 | 33 | void DQ_SerialWholeBody::_check_to_ith_chain(const int &to_ith_chain) const 34 | { 35 | if(to_ith_chain >= static_cast(chain_.size()) || to_ith_chain < 0) 36 | { 37 | throw std::runtime_error(std::string("Tried to access chain index ") + std::to_string(to_ith_chain) + std::string(" which is unnavailable.")); 38 | } 39 | } 40 | 41 | void DQ_SerialWholeBody::_check_to_jth_link_of_ith_chain(const int &to_ith_chain, const int &to_jth_link) const 42 | { 43 | _check_to_ith_chain(to_ith_chain); 44 | if( (to_jth_link >= static_cast(chain_[to_ith_chain]->get_dim_configuration_space())) || to_jth_link < 0) 45 | { 46 | throw std::runtime_error( 47 | std::string("Tried to access link index ") 48 | + std::to_string(to_jth_link) 49 | + std::string(" of chain index ") 50 | + std::to_string(to_ith_chain) 51 | + std::string(" which is unnavailable.") 52 | ); 53 | } 54 | } 55 | 56 | DQ_SerialWholeBody::DQ_SerialWholeBody(std::shared_ptr robot, const std::string type) 57 | { 58 | chain_.push_back(robot); 59 | dim_configuration_space_ = robot->get_dim_configuration_space(); 60 | if(type == std::string("standard")) 61 | { 62 | //Nothing to do 63 | } 64 | else if(type == std::string("reversed")) 65 | { 66 | throw std::runtime_error(std::string("Reversed type DQ_SerialWholeBody is not implemented yet")); 67 | } 68 | else 69 | { 70 | throw std::runtime_error(std::string("Invalid type: ") + type); 71 | } 72 | } 73 | 74 | DQ_Kinematics* DQ_SerialWholeBody::get_chain(const int& to_ith_chain) 75 | { 76 | _check_to_ith_chain(to_ith_chain); 77 | 78 | return chain_[static_cast::size_type>(to_ith_chain)].get(); 79 | } 80 | 81 | DQ_SerialManipulatorDH DQ_SerialWholeBody::get_chain_as_serial_manipulator_dh(const int &to_ith_chain) const 82 | { 83 | _check_to_ith_chain(to_ith_chain); 84 | 85 | try 86 | { 87 | return DQ_SerialManipulatorDH(*dynamic_cast(chain_[static_cast::size_type>(to_ith_chain)].get())); 88 | } catch (const std::bad_cast& e) 89 | { 90 | throw std::runtime_error("Index requested in get_chain_as_serial_manipulator is not a SerialManipulator" + std::string(e.what())); 91 | } 92 | } 93 | 94 | DQ_HolonomicBase DQ_SerialWholeBody::get_chain_as_holonomic_base(const int &to_ith_chain) const 95 | { 96 | _check_to_ith_chain(to_ith_chain); 97 | 98 | try 99 | { 100 | return DQ_HolonomicBase(*dynamic_cast(chain_[static_cast::size_type>(to_ith_chain)].get())); 101 | } catch (const std::bad_cast& e) 102 | { 103 | throw std::runtime_error("Index requested in get_chain_as_holonomic_base is not a DQ_HolonomicBase" + std::string(e.what())); 104 | } 105 | } 106 | 107 | void DQ_SerialWholeBody::add(std::shared_ptr robot) 108 | { 109 | dim_configuration_space_+= robot->get_dim_configuration_space(); 110 | chain_.push_back(robot); 111 | } 112 | 113 | DQ DQ_SerialWholeBody::fkm(const VectorXd& q) const 114 | { 115 | return reference_frame_ * raw_fkm(q); 116 | } 117 | 118 | DQ DQ_SerialWholeBody::fkm(const VectorXd& q, const int& to_ith_link) const 119 | { 120 | return reference_frame_ * raw_fkm(q,to_ith_link); 121 | } 122 | 123 | std::tuple DQ_SerialWholeBody::get_chain_and_link_from_index(const int &to_ith_link) const 124 | { 125 | int ith_chain = 0; 126 | int jth_link = 0; 127 | 128 | int n = to_ith_link; 129 | for(size_t ith=0;ithget_dim_configuration_space()) >= 0) 132 | { 133 | ith_chain++; 134 | n = n - chain_[ith]->get_dim_configuration_space(); 135 | } 136 | else 137 | { 138 | jth_link = n; 139 | return std::make_tuple(ith_chain, jth_link); 140 | } 141 | } 142 | throw std::runtime_error("Unable to get_chain_and_link_from_index."); 143 | } 144 | 145 | DQ DQ_SerialWholeBody::raw_fkm(const VectorXd &q) const 146 | { 147 | return raw_fkm_by_chain(q, chain_.size()-1, chain_[chain_.size()-1]->get_dim_configuration_space()-1); 148 | } 149 | 150 | DQ DQ_SerialWholeBody::raw_fkm(const VectorXd &q, const int &to_ith_link) const 151 | { 152 | int to_ith_chain; 153 | int to_jth_link; 154 | std::tie(to_ith_chain,to_jth_link) = get_chain_and_link_from_index(to_ith_link); 155 | 156 | return raw_fkm_by_chain(q, to_ith_chain, to_jth_link); 157 | } 158 | 159 | DQ DQ_SerialWholeBody::raw_fkm_by_chain(const VectorXd &q, const int &to_ith_chain, const int &to_jth_link) const 160 | { 161 | _check_q_vec(q); 162 | _check_to_ith_chain(to_ith_chain); 163 | 164 | DQ pose(1); 165 | 166 | int q_counter = 0; 167 | int current_robot_dim; 168 | VectorXd current_robot_q; 169 | //Loop until the second-to-last element until to_ith_chain 170 | for(int i=0;iget_dim_configuration_space(); 173 | current_robot_q = q.segment(q_counter,current_robot_dim); 174 | pose = pose * chain_[i]->fkm(current_robot_q); 175 | q_counter += current_robot_dim; 176 | } 177 | //The last element in the chain can be partial 178 | current_robot_dim = chain_[to_ith_chain]->get_dim_configuration_space(); 179 | current_robot_q = q.segment(q_counter,current_robot_dim); 180 | pose = pose * chain_[to_ith_chain]->fkm(current_robot_q,to_jth_link); 181 | 182 | return pose; 183 | } 184 | 185 | DQ DQ_SerialWholeBody::raw_fkm_by_chain(const VectorXd &q, const int &to_ith_chain) const 186 | { 187 | _check_q_vec(q); 188 | _check_to_ith_chain(to_ith_chain); 189 | 190 | return raw_fkm_by_chain(q,to_ith_chain,chain_[to_ith_chain]->get_dim_configuration_space()-1); 191 | } 192 | 193 | MatrixXd DQ_SerialWholeBody::raw_pose_jacobian_by_chain(const VectorXd &q, const int &to_ith_chain, const int &to_jth_link) const 194 | { 195 | _check_q_vec(q); 196 | _check_to_jth_link_of_ith_chain(to_ith_chain, to_jth_link); 197 | 198 | int n = to_ith_chain+1; //Size of the partial or total chain 199 | DQ x_0_to_n = raw_fkm_by_chain(q,to_ith_chain,to_jth_link); 200 | int q_counter = 0; 201 | 202 | //Not the best implementation but similar to MATLAB 203 | std::vector J_vector; 204 | for(int i=0;iget_dim_configuration_space(); 207 | // Addressing the to_jth_link 208 | 209 | const DQ x_0_to_iplus1 = raw_fkm_by_chain(q,i); 210 | const DQ x_iplus1_to_n = conj(x_0_to_iplus1)*x_0_to_n; 211 | 212 | const VectorXd q_iplus1 = q.segment(q_counter,dim); 213 | q_counter += dim; 214 | 215 | if(i==0) 216 | { 217 | J_vector.push_back(haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1)); 218 | } 219 | else 220 | { 221 | if(i==n-1) //To address the last index in the chain, or partial chains 222 | J_vector.push_back(hamiplus8(raw_fkm_by_chain(q,i-1))*haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,to_jth_link)); 223 | else 224 | J_vector.push_back(hamiplus8(raw_fkm_by_chain(q,i-1))*haminus8(x_iplus1_to_n)*chain_[i]->pose_jacobian(q_iplus1,dim-1)); 225 | } 226 | } 227 | 228 | MatrixXd J_pose(8,q_counter); 229 | int col_counter = 0; 230 | for(int i=0;iget_dim_configuration_space(); 233 | MatrixXd current_J = J_vector[i]; 234 | for(int j=0;j(chain_[chain_.size()-1])->set_effector(effector); 300 | } 301 | catch (const std::bad_cast& e) 302 | { 303 | throw std::runtime_error("The last element of the chain needs to be a DQ_SerialManipulator to use set_effector in a DQ_WholeBody " + std::string(e.what())); 304 | } 305 | } 306 | 307 | } 308 | --------------------------------------------------------------------------------