├── .gitignore ├── README.md ├── libreflexxestype2 ├── CMakeLists.txt ├── LICENSE ├── README.txt ├── include │ └── libreflexxestype2 │ │ ├── RMLFlags.h │ │ ├── RMLInputParameters.h │ │ ├── RMLOutputParameters.h │ │ ├── RMLPositionFlags.h │ │ ├── RMLPositionInputParameters.h │ │ ├── RMLPositionOutputParameters.h │ │ ├── RMLVector.h │ │ ├── RMLVelocityFlags.h │ │ ├── RMLVelocityInputParameters.h │ │ ├── RMLVelocityOutputParameters.h │ │ ├── ReflexxesAPI.h │ │ ├── TypeIIRMLDecisionTree1A.h │ │ ├── TypeIIRMLDecisionTree1B.h │ │ ├── TypeIIRMLDecisionTree1C.h │ │ ├── TypeIIRMLDecisionTree2.h │ │ ├── TypeIIRMLDecisions.h │ │ ├── TypeIIRMLMath.h │ │ ├── TypeIIRMLPolynomial.h │ │ ├── TypeIIRMLPosition.h │ │ ├── TypeIIRMLQuicksort.h │ │ ├── TypeIIRMLStep1IntermediateProfiles.h │ │ ├── TypeIIRMLStep1Profiles.h │ │ ├── TypeIIRMLStep2IntermediateProfiles.h │ │ ├── TypeIIRMLStep2Profiles.h │ │ ├── TypeIIRMLStep2WithoutSynchronization.h │ │ └── TypeIIRMLVelocity.h ├── package.xml └── src │ ├── RMLPositionSampleApplications │ ├── 01_RMLPositionSampleApplication.cpp │ ├── 02_RMLPositionSampleApplication.cpp │ ├── 03_RMLPositionSampleApplication.cpp │ └── 07_RMLPositionSampleApplication.cpp │ ├── RMLVelocitySampleApplications │ ├── 04_RMLVelocitySampleApplication.cpp │ ├── 05_RMLVelocitySampleApplication.cpp │ ├── 06_RMLVelocitySampleApplication.cpp │ └── 08_RMLVelocitySampleApplication.cpp │ └── TypeIIRML │ ├── ReflexxesAPI.cpp │ ├── TypeIIRMLCalculatePositionalExtrems.cpp │ ├── TypeIIRMLDecisionTree1A.cpp │ ├── TypeIIRMLDecisionTree1B.cpp │ ├── TypeIIRMLDecisionTree1C.cpp │ ├── TypeIIRMLDecisionTree2.cpp │ ├── TypeIIRMLDecisions.cpp │ ├── TypeIIRMLFallBackStrategy.cpp │ ├── TypeIIRMLIsPhaseSynchronizationPossible.cpp │ ├── TypeIIRMLPolynomial.cpp │ ├── TypeIIRMLPosition.cpp │ ├── TypeIIRMLQuicksort.cpp │ ├── TypeIIRMLSetupModifiedSelectionVector.cpp │ ├── TypeIIRMLStep1.cpp │ ├── TypeIIRMLStep1IntermediateProfiles.cpp │ ├── TypeIIRMLStep1Profiles.cpp │ ├── TypeIIRMLStep2.cpp │ ├── TypeIIRMLStep2IntermediateProfiles.cpp │ ├── TypeIIRMLStep2PhaseSynchronization.cpp │ ├── TypeIIRMLStep2Profiles.cpp │ ├── TypeIIRMLStep2WithoutSynchronization.cpp │ ├── TypeIIRMLStep3.cpp │ ├── TypeIIRMLVelocity.cpp │ ├── TypeIIRMLVelocityCalculatePositionalExtrems.cpp │ ├── TypeIIRMLVelocityFallBackStrategy.cpp │ ├── TypeIIRMLVelocityIsPhaseSynchronizationPossible.cpp │ ├── TypeIIRMLVelocityMethods.cpp │ └── TypeIIRMLVelocitySetupPhaseSyncSelectionVector.cpp └── reflexxes_wrapper ├── CMakeLists.txt ├── README.txt ├── include └── reflexxes_wrapper │ └── reflexxes_wrapper.h └── package.xml /.gitignore: -------------------------------------------------------------------------------- 1 | # Ignore Reflexxes build artifacts 2 | libreflexxestype2/Linux/x64/* 3 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # ros_reflexxes 2 | 3 | ### Package *libreflexxestype2* 4 | 5 | This is a copy/paste of the publicly-available [Reflexxes II](http://www.reflexxes.ws/products/overview-and-download.html) source code, with a CMakeLists to compile and export it as a ROS library. 6 | 7 | ### Package *reflexxes_wrapper* 8 | 9 | Provides header-file-only convenience functions to make Reflexxes usage easier. 10 | -------------------------------------------------------------------------------- /libreflexxestype2/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(libreflexxestype2) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include_directories(include) 7 | 8 | add_library(reflexxes 9 | src/TypeIIRML/ReflexxesAPI.cpp 10 | src/TypeIIRML/TypeIIRMLCalculatePositionalExtrems.cpp 11 | src/TypeIIRML/TypeIIRMLDecisions.cpp 12 | src/TypeIIRML/TypeIIRMLDecisionTree1A.cpp 13 | src/TypeIIRML/TypeIIRMLDecisionTree1B.cpp 14 | src/TypeIIRML/TypeIIRMLDecisionTree1C.cpp 15 | src/TypeIIRML/TypeIIRMLDecisionTree2.cpp 16 | src/TypeIIRML/TypeIIRMLFallBackStrategy.cpp 17 | src/TypeIIRML/TypeIIRMLIsPhaseSynchronizationPossible.cpp 18 | src/TypeIIRML/TypeIIRMLPolynomial.cpp 19 | src/TypeIIRML/TypeIIRMLPosition.cpp 20 | src/TypeIIRML/TypeIIRMLQuicksort.cpp 21 | src/TypeIIRML/TypeIIRMLSetupModifiedSelectionVector.cpp 22 | src/TypeIIRML/TypeIIRMLStep1.cpp 23 | src/TypeIIRML/TypeIIRMLStep1IntermediateProfiles.cpp 24 | src/TypeIIRML/TypeIIRMLStep1Profiles.cpp 25 | src/TypeIIRML/TypeIIRMLStep2.cpp 26 | src/TypeIIRML/TypeIIRMLStep2IntermediateProfiles.cpp 27 | src/TypeIIRML/TypeIIRMLStep2PhaseSynchronization.cpp 28 | src/TypeIIRML/TypeIIRMLStep2Profiles.cpp 29 | src/TypeIIRML/TypeIIRMLStep2WithoutSynchronization.cpp 30 | src/TypeIIRML/TypeIIRMLStep3.cpp 31 | src/TypeIIRML/TypeIIRMLVelocityCalculatePositionalExtrems.cpp 32 | src/TypeIIRML/TypeIIRMLVelocity.cpp 33 | src/TypeIIRML/TypeIIRMLVelocityFallBackStrategy.cpp 34 | src/TypeIIRML/TypeIIRMLVelocityIsPhaseSynchronizationPossible.cpp 35 | src/TypeIIRML/TypeIIRMLVelocityMethods.cpp 36 | src/TypeIIRML/TypeIIRMLVelocitySetupPhaseSyncSelectionVector.cpp 37 | ) 38 | 39 | install( 40 | TARGETS 41 | reflexxes 42 | LIBRARY DESTINATION 43 | lib 44 | ) 45 | 46 | install( 47 | DIRECTORY 48 | include/ 49 | DESTINATION 50 | include 51 | ) 52 | 53 | # Export via these functions rather than ament_export_targets since we aren't doing a target based install 54 | ament_export_include_directories(include) 55 | ament_export_libraries(reflexxes) 56 | 57 | ament_package() 58 | -------------------------------------------------------------------------------- /libreflexxestype2/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 | -------------------------------------------------------------------------------- /libreflexxestype2/README.txt: -------------------------------------------------------------------------------- 1 | --------------------------------------------------------------- 2 | Example Projects for the Type II Reflexxes Motion Library 3 | --------------------------------------------------------------- 4 | 5 | 6 | *************************************************************** 7 | 1. Directory Contents 8 | *************************************************************** 9 | 10 | - include: Folder for all header files of the Reflexxes API 11 | and the Type II Reflexxes Motion Library 12 | - src: Folder for the source code files of the Type II 13 | Reflexxes Motion Library and the six sample applications 14 | - Linux: Folder with example makefiles for Linux 15 | - MacOS: Folder with example makefiles for Mac OS X 16 | - Windows: Folder with example project files for Microsoft 17 | Windows (Visual Studio 2008 Express) 18 | 19 | 20 | *************************************************************** 21 | 2. Getting Started 22 | *************************************************************** 23 | Change to the directory 'Linux' or 'MacOS', respectively, and 24 | enter 25 | 26 | make clean32 all32 27 | 28 | for 32-bit systems or 29 | 30 | make clean64 all64 31 | 32 | for 64-bit systems, respectively, to check whether all files 33 | compile correctly on your system. Under Microsoft Windows, you 34 | can open the Visual Studio solution file 35 | 'ReflexxesTypeII_ExampleProject.sln' and re-complie all 36 | projects to perform the check on your system. If everything 37 | compiles without error messages, you can take a look at one of 38 | the simple sample applications in the 'src' folder to learn 39 | about the simple and clean Reflexxes API and to use it for 40 | your own applications. In case of problems or issues with this 41 | procedure, please contact us at support@reflexxes.com. 42 | 43 | 44 | *************************************************************** 45 | 3. Documentation 46 | *************************************************************** 47 | 48 | http://www.reflexxes.com/software/typeiirml/v1.2.6 49 | 50 | 51 | *************************************************************** 52 | A. Appendix - Entire Folder Structure 53 | *************************************************************** 54 | 55 | * ReflexxesTypeII 56 | o include Folder for all header files of the Reflexxes API and the source code of the Type II Reflexxes Motion Library 57 | o Linux Folder with example makefiles for Linux 58 | + RMLPositionSampleApplications Folder for the makefile of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 59 | + RMLVelocitySampleApplications Folder for the makefile of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 60 | + TypeIIRML Folder for the makefile of the Type II Reflexxes Motion Library 61 | + x64 Binary files for 64-bit environments 62 | # debug Files with debug information (non-optimized) 63 | * bin Executable files of all sample applications 64 | * lib This folder contains the Reflexxes Type II Motion Library. 65 | * obj Object files 66 | # release Files without debug information (fully optimized) 67 | * bin Executable files of all sample applications 68 | * lib This folder contains the Reflexxes Type II Motion Library. 69 | * obj Object files 70 | + x86 Binary files for 32-bit environments 71 | # debug Files with debug information (non-optimized) 72 | * bin Executable files of all sample applications 73 | * lib This folder contains the Reflexxes Type II Motion Library. 74 | * obj Object files 75 | # release Files without debug information (fully optimized) 76 | * bin Executable files of all sample applications 77 | * lib This folder contains the Reflexxes Type II Motion Library. 78 | * obj Object files 79 | o MacOS Folder with example makefiles for Mac OS X 80 | + RMLPositionSampleApplications Folder for the makefile of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 81 | + RMLVelocitySampleApplications Folder for the makefile of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 82 | + TypeIIRML Folder for the makefile of the Type II Reflexxes Motion Library 83 | + x64 Binary files for 32-bit environments 84 | # debug Files with debug information (non-optimized) 85 | * bin Executable files of all sample applications 86 | * lib This folder contains the Reflexxes Type II Motion Library. 87 | * obj Object files 88 | # release Files without debug information (fully optimized) 89 | * bin Executable files of all sample applications 90 | * lib This folder contains the Reflexxes Type II Motion Library. 91 | * obj Object files 92 | + x86 Binary files for 32-bit environments 93 | # debug Files with debug information (non-optimized) 94 | * bin Executable files of all sample applications 95 | * lib This folder contains the Reflexxes Type II Motion Library. 96 | * obj Object files 97 | # release Files without debug information (fully optimized) 98 | * bin Executable files of all sample applications 99 | * lib This folder contains the Reflexxes Type II Motion Library. 100 | * obj Object files 101 | o src Folder for the source code files of the six sample applications 102 | + RMLPositionSampleApplications Source code of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 103 | + RMLVelocitySampleApplications Source code of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 104 | + TypeIIRML Source code of the Type II Reflexxes Motion Library 105 | o Windows Folder with example project files for Microsoft Windows (Visual Studio 2008 Express) 106 | + Debug Binary files with debug information (non-optimized) 107 | + Release Binary files without debug information (fully optimized) 108 | + 01_RMLPositionSampleApplication Folder for the project file of 01_RMLPositionSampleApplication.cpp 109 | + 02_RMLPositionSampleApplication Folder for the project file of 02_RMLPositionSampleApplication.cpp 110 | + 03_RMLPositionSampleApplication Folder for the project file of 03_RMLPositionSampleApplication.cpp 111 | + 04_RMLVelocitySampleApplication Folder for the project file of 04_RMLVelocitySampleApplication.cpp 112 | + 05_RMLVelocitySampleApplication Folder for the project file of 05_RMLVelocitySampleApplication.cpp 113 | + 06_RMLVelocitySampleApplication Folder for the project file of 06_RMLVelocitySampleApplication.cpp 114 | + 07_RMLPositionSampleApplication Folder for the project file of 07_RMLPositionSampleApplication.cpp 115 | + 08_RMLVelocitySampleApplication Folder for the project file of 08_RMLVelocitySampleApplication.cpp 116 | + TypeIIRML Folder for the project file of the Type II Reflexxes Motion Library 117 | 118 | --------------------------------------------------------------- 119 | Copyright (C) 2014 Google, Inc. -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/RMLFlags.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file RMLFlags.h 3 | //! 4 | //! \brief 5 | //! Header file for the class RMLFlags 6 | //! 7 | //! \details 8 | //! Flags to parameterize the On-Line Trajectory Generation algorithms. 9 | //! This basis data structure is inherited to the structures 10 | //! RMLPositionFlags and RMLVelocityFlags 11 | //! 12 | //! \sa RMLPositionFlags 13 | //! \sa RMLVelocityFlags 14 | //! 15 | //! \date March 2014 16 | //! 17 | //! \version 1.2.6 18 | //! 19 | //! \author Torsten Kroeger, \n 20 | //! 21 | //! \copyright Copyright (C) 2014 Google, Inc. 22 | //! \n 23 | //! \n 24 | //! GNU Lesser General Public License 25 | //! \n 26 | //! \n 27 | //! This file is part of the Type II Reflexxes Motion Library. 28 | //! \n\n 29 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 30 | //! it and/or modify it under the terms of the GNU Lesser General Public License 31 | //! as published by the Free Software Foundation, either version 3 of the 32 | //! License, or (at your option) any later version. 33 | //! \n\n 34 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 35 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 36 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 37 | //! the GNU Lesser General Public License for more details. 38 | //! \n\n 39 | //! You should have received a copy of the GNU Lesser General Public License 40 | //! along with the Type II Reflexxes Motion Library. If not, see 41 | //! . 42 | // ---------------------------------------------------------- 43 | // For a convenient reading of this file's source code, 44 | // please use a tab width of four characters. 45 | // ---------------------------------------------------------- 46 | 47 | 48 | #ifndef __RMLFlags__ 49 | #define __RMLFlags__ 50 | 51 | 52 | // ---------------------- Doxygen info ---------------------- 53 | //! \class RMLFlags 54 | //! 55 | //! \brief 56 | //! Data structure containing flags to parameterize the execution of the 57 | //! On-Line Trajectory Generation algorithm 58 | // ---------------------------------------------------------- 59 | class RMLFlags 60 | { 61 | 62 | protected: 63 | 64 | // ---------------------- Doxygen info ---------------------- 65 | //! \fn RMLFlags(void) 66 | //! 67 | //! \brief 68 | //! Constructor of the class 69 | //! 70 | //! \note 71 | //! This is only the base class for the classes\n\n 72 | //!
    73 | //!
  • RMLPositionFlags and
    • 74 | //!
  • RMLVelocityFlags,\n\n
    • 75 | //!
76 | //! such that the constructor is declared \c protected. 77 | // ---------------------------------------------------------- 78 | RMLFlags(void) 79 | { 80 | } 81 | 82 | public: 83 | 84 | 85 | // ---------------------- Doxygen info ---------------------- 86 | //! \fn ~RMLFlags(void) 87 | //! 88 | //! \brief 89 | //! Destructor of the class RMLFlags 90 | // ---------------------------------------------------------- 91 | ~RMLFlags(void) 92 | { 93 | } 94 | 95 | 96 | // ---------------------- Doxygen info ---------------------- 97 | //! \fn inline bool operator == (const RMLFlags &Flags) const 98 | //! 99 | //! \brief 100 | //! Equal operator 101 | //! 102 | //! \return 103 | //!\c true if all attributes of both objects are equal; \c false otherwise 104 | // ---------------------------------------------------------- 105 | inline bool operator == (const RMLFlags &Flags) const 106 | { 107 | return ( (this->SynchronizationBehavior 108 | == Flags.SynchronizationBehavior) 109 | && (this->EnableTheCalculationOfTheExtremumMotionStates 110 | == Flags.EnableTheCalculationOfTheExtremumMotionStates) ); 111 | } 112 | 113 | // ---------------------- Doxygen info ---------------------- 114 | //! \fn inline bool operator != (const RMLFlags &Flags) const 115 | //! 116 | //! \brief 117 | //! Unequal operator 118 | //! 119 | //! \return 120 | //!\c false if all attributes of both objects are equal; \c true otherwise 121 | // ---------------------------------------------------------- 122 | inline bool operator != (const RMLFlags &Flags) const 123 | { 124 | return(!(*this == Flags)); 125 | } 126 | 127 | 128 | // ---------------------- Doxygen info ---------------------- 129 | //! \enum SyncBehaviorEnum 130 | //! 131 | //! \brief 132 | //! Enumeration whose values specify the synchronization method of the 133 | //! On-Line Trajectory Generation algorithm 134 | //! 135 | //! \sa RMLFlags::SynchronizationBehavior 136 | //! \sa \ref page_SynchronizationBehavior 137 | //! \sa \ref page_PSIfPossible 138 | // ---------------------------------------------------------- 139 | enum SyncBehaviorEnum 140 | { 141 | //! \brief 142 | //! This is the default value. If it is possible to calculate a 143 | //! phase-synchronized (i.e., homothetic) trajectory, the algorithm 144 | //! will generate it. A more detailed description of this flag can 145 | //! be found on the page \ref page_PSIfPossible. 146 | PHASE_SYNCHRONIZATION_IF_POSSIBLE = 0 , 147 | //! \brief 148 | //! Even if it is possible to calculate a phase-synchronized 149 | //! trajectory, only a time-synchronized trajectory will be 150 | //! provided. More information can be found on page 151 | //! \ref page_SynchronizationBehavior. 152 | ONLY_TIME_SYNCHRONIZATION = 1 , 153 | //! \brief 154 | //! Only phase-synchronized trajectories are allowed. If it is not 155 | //! possible calculate a phase-synchronized trajectory, an error 156 | //! will be returned (but feasible, steady, and continuous 157 | //! output values will still be computed). More information can be 158 | //! found on page \ref page_SynchronizationBehavior. 159 | ONLY_PHASE_SYNCHRONIZATION = 2 , 160 | //! \brief 161 | //! No synchronization will be performed, and all selected degrees 162 | //! of freedom are treated independently. 163 | NO_SYNCHRONIZATION = 3 164 | }; 165 | 166 | 167 | // ---------------------- Doxygen info ---------------------- 168 | //! \var unsigned char SynchronizationBehavior 169 | //! 170 | //! \brief 171 | //! This value specifies the desired synchronization behavior 172 | //! 173 | //! \details 174 | //! The following values are possible: 175 | //! 176 | //! - RMLFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE 177 | //! - RMLFlags::ONLY_TIME_SYNCHRONIZATION 178 | //! - RMLFlags::ONLY_PHASE_SYNCHRONIZATION 179 | //! - RMLFlags::NO_SYNCHRONIZATION 180 | //! 181 | //! Further details about time- and phase-synchronization can be found 182 | //! in the section on \ref page_SynchronizationBehavior and at 183 | //! RMLFlags::SyncBehaviorEnum. 184 | //! 185 | //! \sa RMLPositionFlags 186 | //! \sa RMLVelocityFlags 187 | //! \sa RMLPositionOutputParameters::IsTrajectoryPhaseSynchronized() 188 | //! \sa RMLVelocityOutputParameters::IsTrajectoryPhaseSynchronized() 189 | //! \sa \ref page_SynchronizationBehavior 190 | //! \sa \ref page_PSIfPossible 191 | // ---------------------------------------------------------- 192 | unsigned char SynchronizationBehavior; 193 | 194 | 195 | // ---------------------- Doxygen info ---------------------- 196 | //! \var bool EnableTheCalculationOfTheExtremumMotionStates 197 | //! 198 | //! \brief 199 | //! A flag to enable or disable the calculation of the extremum states of motion of the 200 | //! currently calculated trajectory 201 | //! 202 | //! \details 203 | //! If this flag is set, a call of 204 | //! 205 | //! - ReflexxesAPI::RMLPosition() or 206 | //! - ReflexxesAPI::RMLVelocity() 207 | //! 208 | //! will not only calculate the desired trajectory, but also the motion states 209 | //! at the extremum positions. These values will be accessible through the methods 210 | //! of the classes 211 | //! 212 | //! - RMLPositionOutputParameters and 213 | //! - RMLVelocityOutputParameters 214 | //! 215 | //! \remark 216 | //! In average, the calculation of the extremum states of motion takes about five percent 217 | //! the overall computational effort of the OTG algorithm, such that the maximum 218 | //! execution time of the algorithm will be decreased by disabling this flag. 219 | //! 220 | //! \note 221 | //! The default value for this flag is \c true. 222 | //! 223 | //! \sa \ref page_OutputValues 224 | //! \sa \ref page_FinalStateReachedBehavior 225 | //! \sa \ref page_PSIfPossible\n\n 226 | // ---------------------------------------------------------- 227 | bool EnableTheCalculationOfTheExtremumMotionStates; 228 | 229 | };// class RMLFlags 230 | 231 | 232 | 233 | #endif 234 | 235 | 236 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/RMLPositionFlags.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file RMLPositionFlags.h 3 | //! 4 | //! \brief 5 | //! Header file for the class RMLPositionFlags 6 | //! 7 | //! \details 8 | //! Flags to parameterize the position-based On-Line Trajectory Generation 9 | //! algorithm. 10 | //! 11 | //! \sa RMLFlags.h 12 | //! \sa RMLVelocityFlags.h 13 | //! 14 | //! \date March 2014 15 | //! 16 | //! \version 1.2.6 17 | //! 18 | //! \author Torsten Kroeger, \n 19 | //! 20 | //! \copyright Copyright (C) 2014 Google, Inc. 21 | //! \n 22 | //! \n 23 | //! GNU Lesser General Public License 24 | //! \n 25 | //! \n 26 | //! This file is part of the Type II Reflexxes Motion Library. 27 | //! \n\n 28 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 29 | //! it and/or modify it under the terms of the GNU Lesser General Public License 30 | //! as published by the Free Software Foundation, either version 3 of the 31 | //! License, or (at your option) any later version. 32 | //! \n\n 33 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 34 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 35 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 36 | //! the GNU Lesser General Public License for more details. 37 | //! \n\n 38 | //! You should have received a copy of the GNU Lesser General Public License 39 | //! along with the Type II Reflexxes Motion Library. If not, see 40 | //! . 41 | // ---------------------------------------------------------- 42 | // For a convenient reading of this file's source code, 43 | // please use a tab width of four characters. 44 | // ---------------------------------------------------------- 45 | 46 | 47 | #ifndef __RMLPositionFlags__ 48 | #define __RMLPositionFlags__ 49 | 50 | 51 | #include 52 | 53 | 54 | // ---------------------- Doxygen info ---------------------- 55 | //! \class RMLPositionFlags 56 | //! 57 | //! \brief 58 | //! Data structure containing flags to parameterize the execution of the 59 | //! position-based On-Line Trajectory Generation algorithm 60 | //! 61 | //! \sa RMLFlags 62 | //! \sa RMLVelocityFlags 63 | // ---------------------------------------------------------- 64 | class RMLPositionFlags : public RMLFlags 65 | { 66 | public: 67 | 68 | 69 | // ---------------------- Doxygen info ---------------------- 70 | //! \fn RMLPositionFlags(void) 71 | //! 72 | //! \brief 73 | //! Constructor of the class 74 | //! 75 | //! \details 76 | //! Sets the default values: 77 | //! - Synchronization behavior: phase-synchronization if possible (RMLFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE) 78 | //! - Behavior after the target state of motion is reached: keep target velocity (RMLPositionFlags::KEEP_TARGET_VELOCITY) 79 | //! - Calculation of extremum motion states enabled 80 | //! - In case the fall back strategy becomes active: velocity to predefined values (default: zero) 81 | // ---------------------------------------------------------- 82 | RMLPositionFlags(void) 83 | { 84 | this->SynchronizationBehavior = RMLFlags::PHASE_SYNCHRONIZATION_IF_POSSIBLE ; 85 | this->BehaviorAfterFinalStateOfMotionIsReached = RMLPositionFlags::KEEP_TARGET_VELOCITY ; 86 | this->EnableTheCalculationOfTheExtremumMotionStates = true ; 87 | this->KeepCurrentVelocityInCaseOfFallbackStrategy = false ; 88 | } 89 | 90 | 91 | // ---------------------- Doxygen info ---------------------- 92 | //! \fn ~RMLPositionFlags(void) 93 | //! 94 | //! \brief 95 | //! Destructor of the class RMLPositionFlags 96 | // ---------------------------------------------------------- 97 | ~RMLPositionFlags(void) 98 | { 99 | } 100 | 101 | 102 | // ---------------------- Doxygen info ---------------------- 103 | //! \fn inline bool operator == (const RMLPositionFlags &Flags) const 104 | //! 105 | //! \brief 106 | //! Equal operator 107 | //! 108 | //! \return 109 | //!\c true if all attributes of both objects are equal; \c false otherwise 110 | // ---------------------------------------------------------- 111 | inline bool operator == (const RMLPositionFlags &Flags) const 112 | { 113 | return( (RMLFlags::operator==(Flags)) 114 | && (this->BehaviorAfterFinalStateOfMotionIsReached 115 | == Flags.BehaviorAfterFinalStateOfMotionIsReached) 116 | && (this->KeepCurrentVelocityInCaseOfFallbackStrategy 117 | == Flags.KeepCurrentVelocityInCaseOfFallbackStrategy)); 118 | } 119 | 120 | 121 | // ---------------------- Doxygen info ---------------------- 122 | //! \fn inline bool operator != (const RMLPositionFlags &Flags) const 123 | //! 124 | //! \brief 125 | //! Unequal operator 126 | //! 127 | //! \return 128 | //!\c false if all attributes of both objects are equal; \c true otherwise 129 | // ---------------------------------------------------------- 130 | inline bool operator != (const RMLPositionFlags &Flags) const 131 | { 132 | return(!(*this == Flags)); 133 | } 134 | 135 | 136 | // ---------------------- Doxygen info ---------------------- 137 | //! \enum FinalMotionBehaviorEnum 138 | //! 139 | //! \brief 140 | //! Enumeration whose values specify the behavior after the final state 141 | //! of motion is reached. 142 | //! 143 | //! \sa BehaviorAfterFinalStateOfMotionIsReached 144 | // ---------------------------------------------------------- 145 | enum FinalMotionBehaviorEnum 146 | { 147 | //! \brief 148 | //! The desired velocity of the target state of motion will be 149 | //! kept at zero acceleration. 150 | KEEP_TARGET_VELOCITY = 0 , 151 | //! \brief 152 | //! After the final state of motion is reached, a new trajectory 153 | //! will be computed, such that the desired state of motion will 154 | //! be reached again (and again, and again, etc.). 155 | RECOMPUTE_TRAJECTORY = 1 156 | }; 157 | 158 | 159 | // ---------------------- Doxygen info ---------------------- 160 | //! \var int BehaviorAfterFinalStateOfMotionIsReached 161 | //! 162 | //! \brief 163 | //! This flag defines the behavior for the time \em after the final 164 | //! state of motion was reached. 165 | //! 166 | //! \details 167 | //! Two values are possible: 168 | //! 169 | //! - RMLPositionFlags::KEEP_TARGET_VELOCITY or 170 | //! - RMLPositionFlags::RECOMPUTE_TRAJECTORY. 171 | //! 172 | //! \note 173 | //! If the desired target velocity vector is zero, both values will 174 | //! lead to the same behavior. 175 | //! 176 | //! \sa FinalMotionBehaviorEnum 177 | //! \sa \ref page_FinalStateReachedBehavior 178 | // ---------------------------------------------------------- 179 | int BehaviorAfterFinalStateOfMotionIsReached; 180 | 181 | 182 | // ---------------------- Doxygen info ---------------------- 183 | //! \var bool KeepCurrentVelocityInCaseOfFallbackStrategy 184 | //! 185 | //! \brief 186 | //! If true, RMLPositionInputParameters::AlternativeTargetVelocityVector 187 | //! will be used in TypeIVRMLPosition::FallBackStrategy() 188 | //! 189 | //! \details 190 | //! In case, the second safety layer becomes activated by calling 191 | //! TypeIVRMLPosition::FallBackStrategy(), this flag can optionally be used 192 | //! to set the current velocity vector \f$ \vec{V}_i \f$ as the 193 | //! alternative target velocity vector for the velocity-based On-Line 194 | //! Trajectory Generation algorithm. 195 | //! 196 | //! \note 197 | //! By default, this flag is set to \c false, and in most cases, 198 | //! is not necessary to change this. Depending on the 199 | //! application, it may be reasonable to keep the current velocity 200 | //! in case of an error. 201 | //! 202 | //! \sa RMLPositionInputParameters::AlternativeTargetVelocityVector 203 | //! \sa \ref page_ErrorHandling 204 | // ---------------------------------------------------------- 205 | bool KeepCurrentVelocityInCaseOfFallbackStrategy; 206 | 207 | 208 | };// class RMLPositionFlags 209 | 210 | 211 | 212 | #endif 213 | 214 | 215 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/RMLPositionOutputParameters.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file RMLPositionOutputParameters.h 3 | //! 4 | //! \brief 5 | //! Header file for the class RMLPositionOutputParameters 6 | //! 7 | //! \details 8 | //! The class RMLPositionOutputParameters is derived from the class 9 | //! RMLOutputParameters and constitutes a part of the interface for the 10 | //! position-based On-Line Trajectory Generation algorithm. 11 | //! 12 | //! \sa RMLInputParameters 13 | //! \sa RMLPositionInputParameters 14 | //! \sa RMLVelocityOutputParameters 15 | //! 16 | //! \date March 2014 17 | //! 18 | //! \version 1.2.6 19 | //! 20 | //! \author Torsten Kroeger, \n 21 | //! 22 | //! \copyright Copyright (C) 2014 Google, Inc. 23 | //! \n 24 | //! \n 25 | //! GNU Lesser General Public License 26 | //! \n 27 | //! \n 28 | //! This file is part of the Type II Reflexxes Motion Library. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 31 | //! it and/or modify it under the terms of the GNU Lesser General Public License 32 | //! as published by the Free Software Foundation, either version 3 of the 33 | //! License, or (at your option) any later version. 34 | //! \n\n 35 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 36 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 37 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 38 | //! the GNU Lesser General Public License for more details. 39 | //! \n\n 40 | //! You should have received a copy of the GNU Lesser General Public License 41 | //! along with the Type II Reflexxes Motion Library. If not, see 42 | //! . 43 | // ---------------------------------------------------------- 44 | // For a convenient reading of this file's source code, 45 | // please use a tab width of four characters. 46 | // ---------------------------------------------------------- 47 | 48 | 49 | #ifndef __RMLPositionOutputParameters__ 50 | #define __RMLPositionOutputParameters__ 51 | 52 | 53 | #include 54 | 55 | 56 | // ---------------------- Doxygen info ---------------------- 57 | //! \class RMLPositionOutputParameters 58 | //! 59 | //! \brief 60 | //! Class for the output parameters of the position-based On-Line 61 | //! Trajectory Generation algorithm 62 | //! 63 | //! \details 64 | //! The class RMLPositionOutputParameters is derived from the class 65 | //! RMLOutputParameters and constitutes a part of the interface for the 66 | //! position-based On-Line Trajectory Generation algorithm. 67 | //! 68 | //! \sa ReflexxesAPI 69 | //! \sa RMLOutputParameters 70 | //! \sa RMLVelocityOutputParameters 71 | //! \sa RMLPositionInputParameters 72 | // ---------------------------------------------------------- 73 | class RMLPositionOutputParameters : public RMLOutputParameters 74 | { 75 | 76 | public: 77 | 78 | // ---------------------- Doxygen info ---------------------- 79 | //! \fn RMLPositionOutputParameters(const unsigned int DegreesOfFreedom) 80 | //! 81 | //! \brief 82 | //! Constructor of class RMLPositionOutputParameters 83 | //! 84 | //! \warning 85 | //! The constructor is \b not real-time capable as heap memory has to be 86 | //! allocated. 87 | //! 88 | //! \param DegreesOfFreedom 89 | //! Specifies the number of degrees of freedom 90 | // ---------------------------------------------------------- 91 | RMLPositionOutputParameters(const unsigned int DegreesOfFreedom) : RMLOutputParameters(DegreesOfFreedom) 92 | { 93 | } 94 | 95 | 96 | // ---------------------- Doxygen info ---------------------- 97 | //! \fn RMLPositionOutputParameters(const RMLPositionOutputParameters &OP) 98 | //! 99 | //! \brief 100 | //! Copy constructor of class RMLPositionOutputParameters 101 | //! 102 | //! \warning 103 | //! The constructor is \b not real-time capable as heap memory has to be 104 | //! allocated. 105 | //! 106 | //! \param OP 107 | //! Object to be copied 108 | // ---------------------------------------------------------- 109 | RMLPositionOutputParameters(const RMLPositionOutputParameters &OP) : RMLOutputParameters(OP) 110 | { 111 | } 112 | 113 | 114 | // ---------------------- Doxygen info ---------------------- 115 | //! \fn ~RMLPositionOutputParameters(void) 116 | //! 117 | //! \brief 118 | //! Destructor of class RMLPositionOutputParameters 119 | // ---------------------------------------------------------- 120 | ~RMLPositionOutputParameters(void) 121 | { 122 | } 123 | 124 | 125 | // ---------------------- Doxygen info ---------------------- 126 | //! \fn void Echo(FILE* FileHandler = stdout) const 127 | //! 128 | //! \brief 129 | //! \copybrief RMLOutputParameters::Echo() 130 | //! 131 | //! \details 132 | //! \copydetails RMLOutputParameters::Echo() 133 | // ---------------------------------------------------------- 134 | void Echo(FILE* FileHandler = stdout) const 135 | { 136 | if (FileHandler == NULL) 137 | { 138 | return; 139 | } 140 | 141 | RMLOutputParameters::Echo(FileHandler); 142 | 143 | return; 144 | } 145 | 146 | 147 | };// class RMLPositionOutputParameters 148 | 149 | 150 | 151 | #endif 152 | 153 | 154 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/RMLVelocityFlags.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file RMLVelocityFlags.h 3 | //! 4 | //! \brief 5 | //! Header file for the class RMLVelocityFlags 6 | //! 7 | //! \details 8 | //! Flags to parameterize the velocity-based On-Line Trajectory Generation 9 | //! algorithm. 10 | //! 11 | //! \sa RMLFlags.h 12 | //! \sa RMLPositionFlags.h 13 | //! 14 | //! \date March 2014 15 | //! 16 | //! \version 1.2.6 17 | //! 18 | //! \author Torsten Kroeger, \n 19 | //! 20 | //! \copyright Copyright (C) 2014 Google, Inc. 21 | //! \n 22 | //! \n 23 | //! GNU Lesser General Public License 24 | //! \n 25 | //! \n 26 | //! This file is part of the Type II Reflexxes Motion Library. 27 | //! \n\n 28 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 29 | //! it and/or modify it under the terms of the GNU Lesser General Public License 30 | //! as published by the Free Software Foundation, either version 3 of the 31 | //! License, or (at your option) any later version. 32 | //! \n\n 33 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 34 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 35 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 36 | //! the GNU Lesser General Public License for more details. 37 | //! \n\n 38 | //! You should have received a copy of the GNU Lesser General Public License 39 | //! along with the Type II Reflexxes Motion Library. If not, see 40 | //! . 41 | // ---------------------------------------------------------- 42 | // For a convenient reading of this file's source code, 43 | // please use a tab width of four characters. 44 | // ---------------------------------------------------------- 45 | 46 | 47 | #ifndef __RMLVelocityFlags__ 48 | #define __RMLVelocityFlags__ 49 | 50 | 51 | #include 52 | 53 | 54 | // ---------------------- Doxygen info ---------------------- 55 | //! \class RMLVelocityFlags 56 | //! 57 | //! \brief 58 | //! Data structure containing flags to parameterize the execution of the 59 | //! velocity-based On-Line Trajectory Generation algorithm 60 | //! 61 | //! \sa RMLFlags 62 | //! \sa RMLPositionFlags 63 | // ---------------------------------------------------------- 64 | class RMLVelocityFlags : public RMLFlags 65 | { 66 | public: 67 | 68 | 69 | // ---------------------- Doxygen info ---------------------- 70 | //! \fn RMLVelocityFlags(void) 71 | //! 72 | //! \brief 73 | //! Constructor of the class 74 | //! 75 | //! \details 76 | //! Sets the default values: 77 | //! - Synchronization behavior: no synchronization (RMLFlags::NO_SYNCHRONIZATION) 78 | //! - Calculation of extremum motion states enabled 79 | // ---------------------------------------------------------- 80 | RMLVelocityFlags(void) 81 | { 82 | this->SynchronizationBehavior = RMLFlags::NO_SYNCHRONIZATION ; 83 | this->EnableTheCalculationOfTheExtremumMotionStates = true ; 84 | } 85 | 86 | 87 | // ---------------------- Doxygen info ---------------------- 88 | //! \fn ~RMLVelocityFlags(void) 89 | //! 90 | //! \brief 91 | //! Destructor of the class RMLVelocityFlags 92 | // ---------------------------------------------------------- 93 | ~RMLVelocityFlags(void) 94 | { 95 | } 96 | 97 | // ---------------------- Doxygen info ---------------------- 98 | //! \fn inline bool operator == (const RMLVelocityFlags &Flags) const 99 | //! 100 | //! \brief 101 | //! Equal operator 102 | //! 103 | //! \return 104 | //!\c true if all attributes of both objects are equal; \c false otherwise 105 | // ---------------------------------------------------------- 106 | inline bool operator == (const RMLVelocityFlags &Flags) const 107 | { 108 | return(RMLFlags::operator==(Flags)); 109 | } 110 | 111 | 112 | // ---------------------- Doxygen info ---------------------- 113 | //! \fn inline bool operator != (const RMLVelocityFlags &Flags) const 114 | //! 115 | //! \brief 116 | //! Unequal operator 117 | //! 118 | //! \return 119 | //!\c false if all attributes of both objects are equal; \c true otherwise 120 | // ---------------------------------------------------------- 121 | inline bool operator != (const RMLVelocityFlags &Flags) const 122 | { 123 | return(!(*this == Flags)); 124 | } 125 | 126 | };// class RMLVelocityFlags 127 | 128 | 129 | 130 | #endif 131 | 132 | 133 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLDecisionTree1A.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree1A.h 3 | //! 4 | //! \brief 5 | //! Header file for the Step 1 decision tree 1A of the Type II On-Line 6 | //! Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! Header file for the function TypeIIRMLMath::TypeIIRMLDecisionTree1A(), 10 | //! which contains the first decision tree (1A) of the Step 1 of the On- 11 | //! Line Trajectory Generation algorithm. It calculates the minimum possible 12 | //! trajectory execution time for one single degree of freedom. Details 13 | //! on this methodology may be found in\n 14 | //! \n 15 | //! T. Kroeger.\n 16 | //! On-Line Trajectory Generation in Robotic Systems.\n 17 | //! Springer Tracts in Advanced Robotics, Vol. 58, Springer, January 2010.\n 18 | //! http://www.springer.com/978-3-642-05174-6\n 19 | //! \n 20 | //! The function is part of the namespace TypeIIRMLMath. 21 | //! 22 | //! \date March 2014 23 | //! 24 | //! \version 1.2.6 25 | //! 26 | //! \author Torsten Kroeger, \n 27 | //! 28 | //! \copyright Copyright (C) 2014 Google, Inc. 29 | //! \n 30 | //! \n 31 | //! GNU Lesser General Public License 32 | //! \n 33 | //! \n 34 | //! This file is part of the Type II Reflexxes Motion Library. 35 | //! \n\n 36 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 37 | //! it and/or modify it under the terms of the GNU Lesser General Public License 38 | //! as published by the Free Software Foundation, either version 3 of the 39 | //! License, or (at your option) any later version. 40 | //! \n\n 41 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 42 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 43 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 44 | //! the GNU Lesser General Public License for more details. 45 | //! \n\n 46 | //! You should have received a copy of the GNU Lesser General Public License 47 | //! along with the Type II Reflexxes Motion Library. If not, see 48 | //! . 49 | // ---------------------------------------------------------- 50 | // For a convenient reading of this file's source code, 51 | // please use a tab width of four characters. 52 | // ---------------------------------------------------------- 53 | 54 | 55 | #ifndef __TypeIIRMLDecisionTree1A__ 56 | #define __TypeIIRMLDecisionTree1A__ 57 | 58 | #include 59 | 60 | 61 | namespace TypeIIRMLMath 62 | { 63 | 64 | // ---------------------- Doxygen info ---------------------- 65 | //! \fn void TypeIIRMLDecisionTree1A(const double&CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, Step1_Profile *AppliedProfile, double *MinimalExecutionTime) 66 | //! 67 | //! \brief 68 | //! This function contains the decision tree 1A of the Step 1 of the 69 | //! Type II On-Line Trajectory Generation algorithm 70 | //! 71 | //! \details 72 | //! This function calculates the minimum possible trajectory execution 73 | //! time \f$\ _{k}t_{i}^{\,min} \f$ for DOF \f$ k \f$ at instant 74 | //! \f$ T_{i} \f$. 75 | //! 76 | //! \param CurrentPosition 77 | //! Current position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 78 | //! \f$\ _{k}P_{i} \f$ 79 | //! 80 | //! \param CurrentVelocity 81 | //! Current velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 82 | //! \f$\ _{k}V_{i} \f$ 83 | //! 84 | //! \param TargetPosition 85 | //! Target position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 86 | //! \f$\ _{k}P_{i}^{\,trgt} \f$ 87 | //! 88 | //! \param TargetVelocity 89 | //! Target velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 90 | //! \f$\ _{k}V_{i}^{\,trgt} \f$ 91 | //! 92 | //! \param MaxVelocity 93 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 94 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$ 95 | //! 96 | //! \param MaxAcceleration 97 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 98 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$ 99 | //! 100 | //! \param AppliedProfile 101 | //! A pointer to an \c int value. The index of the motion profile, which 102 | //! is used to achieve the minimum execution time (cf. 103 | //! TypeIIRMLMath::Step1_Profile) will be copied to this variable. 104 | //! 105 | //! \param MinimalExecutionTime 106 | //! Pointer to a \c double value: The actual result of the of this 107 | //! function, that is, minimum possible execution time 108 | //! \f$\ _{k}t_{i}^{\,min} \f$ will be copied to this variable. 109 | //! 110 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 111 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 112 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 113 | // ---------------------------------------------------------- 114 | void TypeIIRMLDecisionTree1A( const double &CurrentPosition 115 | , const double &CurrentVelocity 116 | , const double &TargetPosition 117 | , const double &TargetVelocity 118 | , const double &MaxVelocity 119 | , const double &MaxAcceleration 120 | , Step1_Profile *AppliedProfile 121 | , double *MinimalExecutionTime ); 122 | 123 | 124 | } // namespace TypeIIRMLMath 125 | 126 | #endif 127 | 128 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLDecisionTree1B.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree1B.h 3 | //! 4 | //! \brief 5 | //! Header file for the Step 1 decision tree 1B of the Type II On-Line 6 | //! Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! Header file for the function TypeIIRMLMath::TypeIIRMLDecisionTree1B(), 10 | //! which contains a part of the second decision tree (1B) of the Step 1 11 | //! of the On-Line Trajectory Generation algorithm. It calculates the 12 | //! beginning of a possible inoperative time interval. Details on this 13 | //! methodology may be found in\n 14 | //! \n 15 | //! T. Kroeger.\n 16 | //! On-Line Trajectory Generation in Robotic Systems.\n 17 | //! Springer Tracts in Advanced Robotics, Vol. 58, Springer, January 2010.\n 18 | //! http://www.springer.com/978-3-642-05174-6\n 19 | //! \n 20 | //! The function is part of the namespace TypeIIRMLMath. 21 | //! 22 | //! \date March 2014 23 | //! 24 | //! \version 1.2.6 25 | //! 26 | //! \author Torsten Kroeger, \n 27 | //! 28 | //! \copyright Copyright (C) 2014 Google, Inc. 29 | //! \n 30 | //! \n 31 | //! GNU Lesser General Public License 32 | //! \n 33 | //! \n 34 | //! This file is part of the Type II Reflexxes Motion Library. 35 | //! \n\n 36 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 37 | //! it and/or modify it under the terms of the GNU Lesser General Public License 38 | //! as published by the Free Software Foundation, either version 3 of the 39 | //! License, or (at your option) any later version. 40 | //! \n\n 41 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 42 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 43 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 44 | //! the GNU Lesser General Public License for more details. 45 | //! \n\n 46 | //! You should have received a copy of the GNU Lesser General Public License 47 | //! along with the Type II Reflexxes Motion Library. If not, see 48 | //! . 49 | // ---------------------------------------------------------- 50 | // For a convenient reading of this file's source code, 51 | // please use a tab width of four characters. 52 | // ---------------------------------------------------------- 53 | 54 | 55 | #ifndef __TypeIIRMLDecisionTree1B__ 56 | #define __TypeIIRMLDecisionTree1B__ 57 | 58 | 59 | namespace TypeIIRMLMath 60 | { 61 | 62 | // ---------------------- Doxygen info ---------------------- 63 | //! \fn void TypeIIRMLDecisionTree1B(const double&CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, double *MaximalExecutionTime) 64 | //! 65 | //! \brief 66 | //! This function contains the decision tree 1B of the Step 1 of the 67 | //! Type II On-Line Trajectory Generation algorithm 68 | //! 69 | //! \details 70 | //! This function calculates the beginning of possible inoperative time 71 | //! interval, that is, the time \f$\ _{k}t_{i}^{\,begin} \f$ for DOF 72 | //! \f$ k \f$ at instant \f$ T_{i} \f$. 73 | //! 74 | //! \param CurrentPosition 75 | //! Current position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 76 | //! \f$\ _{k}P_{i} \f$ 77 | //! 78 | //! \param CurrentVelocity 79 | //! Current velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 80 | //! \f$\ _{k}V_{i} \f$ 81 | //! 82 | //! \param TargetPosition 83 | //! Target position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 84 | //! \f$\ _{k}P_{i}^{\,trgt} \f$ 85 | //! 86 | //! \param TargetVelocity 87 | //! Target velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 88 | //! \f$\ _{k}V_{i}^{\,trgt} \f$ 89 | //! 90 | //! \param MaxVelocity 91 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 92 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$ 93 | //! 94 | //! \param MaxAcceleration 95 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 96 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$ 97 | //! 98 | //! \param MaximalExecutionTime 99 | //! Pointer to a \c double value: The actual result of the of this 100 | //! function, that is, the value of the beginning of possible inoperative 101 | //! time interval, that is, the time \f$\ _{k}t_{i}^{\,begin} \f$ for DOF 102 | //! \f$ k \f$ at instant \f$ T_{i} \f$. If no inoperative time interval 103 | //! is existent, a value of \c RML_INFINITY will be written to this 104 | //! variable. 105 | //! 106 | //! \return 107 | //! For the case, an error during the calculations of one motion profile 108 | //! happens, the return value will be \c true, otherwise \c false. 109 | //! 110 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 111 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 112 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 113 | // ---------------------------------------------------------- 114 | void TypeIIRMLDecisionTree1B( const double &CurrentPosition 115 | , const double &CurrentVelocity 116 | , const double &TargetPosition 117 | , const double &TargetVelocity 118 | , const double &MaxVelocity 119 | , const double &MaxAcceleration 120 | , double *MaximalExecutionTime); 121 | 122 | 123 | } // namespace TypeIIRMLMath 124 | 125 | #endif 126 | 127 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLDecisionTree1C.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree1C.h 3 | //! 4 | //! \brief 5 | //! Header file for the Step 1 decision tree 1C of the Type II On-Line 6 | //! Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! Header file for the function TypeIIRMLMath::TypeIIRMLDecisionTree1C(), 10 | //! which contains the first decision tree (1C) of the Step 1 On-Line 11 | //! Trajectory Generation algorithm. It calculates the end of a 12 | //! possible inoperative time interval. Details on this methodology may 13 | //! be found in\n 14 | //! \n 15 | //! T. Kroeger.\n 16 | //! On-Line Trajectory Generation in Robotic Systems.\n 17 | //! Springer Tracts in Advanced Robotics, Vol. 58, Springer, January 2010.\n 18 | //! http://www.springer.com/978-3-642-05174-6\n 19 | //! \n 20 | //! The function is part of the namespace TypeIIRMLMath. 21 | //! 22 | //! \date March 2014 23 | //! 24 | //! \version 1.2.6 25 | //! 26 | //! \author Torsten Kroeger, \n 27 | //! 28 | //! \copyright Copyright (C) 2014 Google, Inc. 29 | //! \n 30 | //! \n 31 | //! GNU Lesser General Public License 32 | //! \n 33 | //! \n 34 | //! This file is part of the Type II Reflexxes Motion Library. 35 | //! \n\n 36 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 37 | //! it and/or modify it under the terms of the GNU Lesser General Public License 38 | //! as published by the Free Software Foundation, either version 3 of the 39 | //! License, or (at your option) any later version. 40 | //! \n\n 41 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 42 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 43 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 44 | //! the GNU Lesser General Public License for more details. 45 | //! \n\n 46 | //! You should have received a copy of the GNU Lesser General Public License 47 | //! along with the Type II Reflexxes Motion Library. If not, see 48 | //! . 49 | // ---------------------------------------------------------- 50 | // For a convenient reading of this file's source code, 51 | // please use a tab width of four characters. 52 | // ---------------------------------------------------------- 53 | 54 | 55 | #ifndef __TypeIIRMLDecisionTree1C__ 56 | #define __TypeIIRMLDecisionTree1C__ 57 | 58 | 59 | namespace TypeIIRMLMath 60 | { 61 | 62 | // ---------------------- Doxygen info ---------------------- 63 | //! \fn void TypeIIRMLDecisionTree1C(const double&CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, double *AlternativeExecutionTime); 64 | //! 65 | //! \brief 66 | //! This function contains the decision tree 1C of the Step 1 of the 67 | //! Type II On-Line Trajectory Generation algorithm 68 | //! 69 | //! \details 70 | //! This function calculates the end of an inoperative time 71 | //! interval, that is, the time \f$\ _{k}t_{i}^{\,end} \f$ for DOF 72 | //! \f$ k \f$ at instant \f$ T_{i} \f$. 73 | //! 74 | //! \param CurrentPosition 75 | //! Current position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 76 | //! \f$\ _{k}P_{i} \f$ 77 | //! 78 | //! \param CurrentVelocity 79 | //! Current velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 80 | //! \f$\ _{k}V_{i} \f$ 81 | //! 82 | //! \param TargetPosition 83 | //! Target position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 84 | //! \f$\ _{k}P_{i}^{\,trgt} \f$ 85 | //! 86 | //! \param TargetVelocity 87 | //! Target velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 88 | //! \f$\ _{k}V_{i}^{\,trgt} \f$ 89 | //! 90 | //! \param MaxVelocity 91 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 92 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$ 93 | //! 94 | //! \param MaxAcceleration 95 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 96 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$ 97 | //! 98 | //! \param AlternativeExecutionTime 99 | //! Pointer to a \c double value: The actual result of the of this 100 | //! function, that is, the value of the end of the inoperative 101 | //! time interval, that is, the time \f$\ _{k}t_{i}^{\,end} \f$ for DOF 102 | //! \f$ k \f$ at instant \f$ T_{i} \f$. 103 | //! 104 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 105 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 106 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 107 | // ---------------------------------------------------------- 108 | void TypeIIRMLDecisionTree1C( const double &CurrentPosition 109 | , const double &CurrentVelocity 110 | , const double &TargetPosition 111 | , const double &TargetVelocity 112 | , const double &MaxVelocity 113 | , const double &MaxAcceleration 114 | , double *AlternativeExecutionTime); 115 | 116 | 117 | } // namespace TypeIIRMLMath 118 | 119 | #endif 120 | 121 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLDecisionTree2.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree2.h 3 | //! 4 | //! \brief 5 | //! Header file for the Step 2 decision tree of the Type II On-Line 6 | //! Trajectory Generation algorithm (time-synchronized case) 7 | //! 8 | //! \details 9 | //! Header file for the function TypeIIRMLMath::TypeIIRMLDecisionTree2(), 10 | //! which contains the decision tree of the Step 2 of the On-Line 11 | //! Trajectory Generation algorithm. It synchronizes all selected 12 | //! degrees of freedom to the synchronization time (cf. Step 1). Details 13 | //! on this methodology may be found in\n 14 | //! \n 15 | //! T. Kroeger.\n 16 | //! On-Line Trajectory Generation in Robotic Systems.\n 17 | //! Springer Tracts in Advanced Robotics, Vol. 58, Springer, January 2010.\n 18 | //! http://www.springer.com/978-3-642-05174-6\n 19 | //! \n 20 | //! The function is part of the namespace TypeIIRMLMath. 21 | //! 22 | //! \date March 2014 23 | //! 24 | //! \version 1.2.6 25 | //! 26 | //! \author Torsten Kroeger, \n 27 | //! 28 | //! \copyright Copyright (C) 2014 Google, Inc. 29 | //! \n 30 | //! \n 31 | //! GNU Lesser General Public License 32 | //! \n 33 | //! \n 34 | //! This file is part of the Type II Reflexxes Motion Library. 35 | //! \n\n 36 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 37 | //! it and/or modify it under the terms of the GNU Lesser General Public License 38 | //! as published by the Free Software Foundation, either version 3 of the 39 | //! License, or (at your option) any later version. 40 | //! \n\n 41 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 42 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 43 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 44 | //! the GNU Lesser General Public License for more details. 45 | //! \n\n 46 | //! You should have received a copy of the GNU Lesser General Public License 47 | //! along with the Type II Reflexxes Motion Library. If not, see 48 | //! . 49 | // ---------------------------------------------------------- 50 | // For a convenient reading of this file's source code, 51 | // please use a tab width of four characters. 52 | // ---------------------------------------------------------- 53 | 54 | 55 | #ifndef __TypeIIRMLDecisionTree2__ 56 | #define __TypeIIRMLDecisionTree2__ 57 | 58 | 59 | #include 60 | 61 | 62 | namespace TypeIIRMLMath 63 | { 64 | 65 | // ---------------------- Doxygen info ---------------------- 66 | //! \fn void TypeIIRMLDecisionTree2(const double&CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, const double &SynchronizationTime, MotionPolynomials *PolynomialsInternal) 67 | //! 68 | //! \brief 69 | //! This function contains the decision tree of the Step 2 of the 70 | //! Type II On-Line Trajectory Generation algorithm 71 | //! 72 | //! \details 73 | //! This function synchronizes the trajectory for one single degree of 74 | //! freedom to the synchronization time \f$ t_{i}^{\,sync} \f$ and 75 | //! calculates all trajectory parameters. 76 | //! 77 | //! \param CurrentPosition 78 | //! Current position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 79 | //! \f$\ _{k}P_{i} \f$ 80 | //! 81 | //! \param CurrentVelocity 82 | //! Current velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 83 | //! \f$\ _{k}V_{i} \f$ 84 | //! 85 | //! \param TargetPosition 86 | //! Target position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 87 | //! \f$\ _{k}P_{i}^{\,trgt} \f$ 88 | //! 89 | //! \param TargetVelocity 90 | //! Target velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 91 | //! \f$\ _{k}V_{i}^{\,trgt} \f$ 92 | //! 93 | //! \param MaxVelocity 94 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 95 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$ 96 | //! 97 | //! \param MaxAcceleration 98 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 99 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$ 100 | //! 101 | //! \param SynchronizationTime 102 | //! The synchronization time \f$ t_{i}^{\,sync} \f$ that was calculated 103 | //! in Step 1 of the On-Line Trajectory Generation algorithm (in 104 | //! seconds). 105 | //! 106 | //! \param PolynomialsInternal 107 | //! A pointer to a \c MotionPolynomials object (cf. 108 | //! TypeIIRMLMath::MotionPolynomials). All trajectory parameters of the 109 | //! synchronized Type II trajectory will be written into this object. 110 | //! 111 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 112 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 113 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 114 | // ---------------------------------------------------------- 115 | void TypeIIRMLDecisionTree2( const double &CurrentPosition 116 | , const double &CurrentVelocity 117 | , const double &TargetPosition 118 | , const double &TargetVelocity 119 | , const double &MaxVelocity 120 | , const double &MaxAcceleration 121 | , const double &SynchronizationTime 122 | , MotionPolynomials *PolynomialsInternal); 123 | 124 | } // namespace TypeIIRMLMath 125 | 126 | #endif 127 | 128 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLQuicksort.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLQuicksort.h 3 | //! 4 | //! \brief 5 | //! Header file for the Quicksort algorithm 6 | //! 7 | //! \details 8 | //! Header file for the Quicksort algorithm to be used for within in the 9 | //! library of the Type II OTG algorithm. The Quicksort function is part 10 | //! of the namespace TypeIIRMLMath. 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | #ifndef __TypeIIRMLQuicksort__ 45 | #define __TypeIIRMLQuicksort__ 46 | 47 | 48 | 49 | namespace TypeIIRMLMath 50 | { 51 | 52 | // ---------------------- Doxygen info ---------------------- 53 | //! \fn void Quicksort(const int &LeftBound, const int &RightBound, double *ArrayOfValues) 54 | //! 55 | //! \brief 56 | //! Standard implementation of the Quicksort algorithm for \c double values 57 | //! 58 | //! \details 59 | //! This function is used to sort of time values that are calculated during 60 | //! Step 1\n 61 | //! \n 62 | //! \f$ _kt_i^{\,min},\,_kt_i^{\,begin},\,_kt_i^{\,end}\ \forall\ k\ \in\ \left\{1,\,\dots,\,K\right\} \f$\n 63 | //! \n 64 | //! where \f$ K \f$ is the number of degrees of freedom. 65 | //! 66 | //! \param LeftBound 67 | //! Index value for the left border 68 | //! 69 | //! \param RightBound 70 | //! Index value for the right border 71 | //! 72 | //! \param ArrayOfValues 73 | //! A pointer to an array of double values to be sorted 74 | //! 75 | //! \sa TypeIIRMLPosition::Step1() 76 | // ---------------------------------------------------------- 77 | void Quicksort( const int &LeftBound 78 | , const int &RightBound 79 | , double *ArrayOfValues); 80 | 81 | } // namespace TypeIIRMLMath 82 | 83 | #endif 84 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLStep1IntermediateProfiles.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep1IntermediateProfiles.h 3 | //! 4 | //! \brief 5 | //! Header file for intermediate profile segments of Step 1 6 | //! 7 | //! \details 8 | //! Header file for trajectory execution time calculations 9 | //! of intermediate velocity profiles in the first step 10 | //! of the Type II On-Line Trajectory Generation algorithm. 11 | //! All profile functions are part of 12 | //! the namespace TypeIIRMLMath. 13 | //! 14 | //! \date March 2014 15 | //! 16 | //! \version 1.2.6 17 | //! 18 | //! \author Torsten Kroeger, \n 19 | //! 20 | //! \copyright Copyright (C) 2014 Google, Inc. 21 | //! \n 22 | //! \n 23 | //! GNU Lesser General Public License 24 | //! \n 25 | //! \n 26 | //! This file is part of the Type II Reflexxes Motion Library. 27 | //! \n\n 28 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 29 | //! it and/or modify it under the terms of the GNU Lesser General Public License 30 | //! as published by the Free Software Foundation, either version 3 of the 31 | //! License, or (at your option) any later version. 32 | //! \n\n 33 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 34 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 35 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 36 | //! the GNU Lesser General Public License for more details. 37 | //! \n\n 38 | //! You should have received a copy of the GNU Lesser General Public License 39 | //! along with the Type II Reflexxes Motion Library. If not, see 40 | //! . 41 | // ---------------------------------------------------------- 42 | // For a convenient reading of this file's source code, 43 | // please use a tab width of four characters. 44 | // ---------------------------------------------------------- 45 | 46 | 47 | 48 | #ifndef __TypeIIRMLStep1IntermediateProfiles__ 49 | #define __TypeIIRMLStep1IntermediateProfiles__ 50 | 51 | #include 52 | 53 | 54 | namespace TypeIIRMLMath 55 | { 56 | 57 | 58 | // ---------------------- Doxygen info ---------------------- 59 | //! \fn void NegateStep1(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity) 60 | //! 61 | //! \brief 62 | //! Negate input values during Step 1 63 | //! 64 | //! \param ThisCurrentPosition 65 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 66 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 67 | //! The sign of this value will be flipped. 68 | //! 69 | //! \param ThisCurrentVelocity 70 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 71 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 72 | //! The sign of this value will be flipped. 73 | //! 74 | //! \param ThisTargetPosition 75 | //! Pointer to a \c double value: Target position value for DOF \f$ k \f$ 76 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i}^{\,trgt} \f$ 77 | //! The sign of this value will be flipped. 78 | //! 79 | //! \param ThisTargetVelocity 80 | //! Pointer to a \c double value: Target velocity value for DOF \f$ k \f$ 81 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,trgt} \f$ 82 | //! The sign of this value will be flipped. 83 | //! 84 | //! \note 85 | //! This function is used in the Step 1 decision trees 1A, 1B, and 1C. 86 | //! 87 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 88 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 89 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 90 | //! \sa TypeIIRMLMath::NegateStep2() 91 | // ---------------------------------------------------------- 92 | void NegateStep1( double *ThisCurrentPosition 93 | , double *ThisCurrentVelocity 94 | , double *ThisTargetPosition 95 | , double *ThisTargetVelocity ); 96 | 97 | 98 | // ---------------------- Doxygen info ---------------------- 99 | //! \fn void VToVMaxStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration) 100 | //! 101 | //! \brief 102 | //! One intermediate Step 1 trajectory segment: v -> +vmax (NegLin) 103 | //! 104 | //! \param TotalTime 105 | //! Pointer to a \c double value: Time in seconds required for preceding 106 | //! trajectory segments. This value 107 | //! will be increased by the amount of time required time for this 108 | //! trajectory segment. 109 | //! 110 | //! \param ThisCurrentPosition 111 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 112 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 113 | //! This value will be changed by the function in order to intermediately 114 | //! reach an acceleration value of zero. 115 | //! 116 | //! \param ThisCurrentVelocity 117 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 118 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 119 | //! This value will be increased in order to intermediately reach an 120 | //! acceleration value of \f$\ _{k}A_{i}^{\,max} \f$. 121 | //! 122 | //! \param MaxVelocity 123 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 124 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$. 125 | //! 126 | //! \param MaxAcceleration 127 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 128 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$. 129 | //! 130 | //! 131 | //! \note 132 | //! This function is used in the Step 1 decision trees 1A, 1B, and 1C. 133 | //! 134 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 135 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 136 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 137 | //! \sa TypeIIRMLMath::VToVMaxStep2() 138 | // ---------------------------------------------------------- 139 | void VToVMaxStep1( double *TotalTime 140 | , double *ThisCurrentPosition 141 | , double *ThisCurrentVelocity 142 | , const double &MaxVelocity 143 | , const double &MaxAcceleration ); 144 | 145 | 146 | // ---------------------- Doxygen info ---------------------- 147 | //! \fn void VToZeroStep1(double *TotalTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration) 148 | //! 149 | //! \brief 150 | //! One intermediate Step 1 trajectory segment: v -> 0 (NegLin) 151 | //! 152 | //! \param TotalTime 153 | //! Pointer to a \c double value: Time in seconds required for preceding 154 | //! trajectory segments. This value 155 | //! will be increased by the amount of time required time for this 156 | //! trajectory segment. 157 | //! 158 | //! \param ThisCurrentPosition 159 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 160 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 161 | //! This value will be changed by the function in order to intermediately 162 | //! reach an acceleration value of zero. 163 | //! 164 | //! \param ThisCurrentVelocity 165 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 166 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 167 | //! This value will be increased in order to intermediately reach an 168 | //! acceleration value of \f$\ _{k}A_{i}^{\,max} \f$. 169 | //! 170 | //! \param MaxAcceleration 171 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 172 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$. 173 | //! 174 | //! \note 175 | //! This function is used in the Step 1 decision trees 1A, 1B, and 1C. 176 | //! 177 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1A() 178 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1B() 179 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree1C() 180 | //! \sa TypeIIRMLMath::VToZeroStep2() 181 | // ---------------------------------------------------------- 182 | void VToZeroStep1( double *TotalTime 183 | , double *ThisCurrentPosition 184 | , double *ThisCurrentVelocity 185 | , const double &MaxAcceleration ); 186 | 187 | 188 | } // namespace TypeIIRMLMath 189 | 190 | #endif 191 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLStep2IntermediateProfiles.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep2IntermediateProfiles.h 3 | //! 4 | //! \brief 5 | //! Header file for intermediate profile segments of Step 2 6 | //! 7 | //! \details 8 | //! Header file for trajectory execution time calculations 9 | //! of intermediate velocity profiles in the second step 10 | //! of the Type II On-Line Trajectory Generation algorithm. 11 | //! All profile functions are part of 12 | //! the namespace TypeIIRMLMath. 13 | //! 14 | //! \date March 2014 15 | //! 16 | //! \version 1.2.6 17 | //! 18 | //! \author Torsten Kroeger, \n 19 | //! 20 | //! \copyright Copyright (C) 2014 Google, Inc. 21 | //! \n 22 | //! \n 23 | //! GNU Lesser General Public License 24 | //! \n 25 | //! \n 26 | //! This file is part of the Type II Reflexxes Motion Library. 27 | //! \n\n 28 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 29 | //! it and/or modify it under the terms of the GNU Lesser General Public License 30 | //! as published by the Free Software Foundation, either version 3 of the 31 | //! License, or (at your option) any later version. 32 | //! \n\n 33 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 34 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 35 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 36 | //! the GNU Lesser General Public License for more details. 37 | //! \n\n 38 | //! You should have received a copy of the GNU Lesser General Public License 39 | //! along with the Type II Reflexxes Motion Library. If not, see 40 | //! . 41 | // ---------------------------------------------------------- 42 | // For a convenient reading of this file's source code, 43 | // please use a tab width of four characters. 44 | // ---------------------------------------------------------- 45 | 46 | 47 | 48 | #ifndef __TypeIIRMLStep2IntermediateTimeProfiles__ 49 | #define __TypeIIRMLStep2IntermediateTimeProfiles__ 50 | 51 | 52 | #include 53 | #include 54 | 55 | 56 | namespace TypeIIRMLMath 57 | { 58 | 59 | 60 | // ---------------------- Doxygen info ---------------------- 61 | //! \fn void NegateStep2(double *ThisCurrentPosition, double *ThisCurrentVelocity, double *ThisTargetPosition, double *ThisTargetVelocity, bool *Inverted) 62 | //! 63 | //! \brief 64 | //! Negate input values 65 | //! 66 | //! \param ThisCurrentPosition 67 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 68 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 69 | //! The sign of this value will be flipped. 70 | //! 71 | //! \param ThisCurrentVelocity 72 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 73 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 74 | //! The sign of this value will be flipped. 75 | //! 76 | //! \param ThisTargetPosition 77 | //! Pointer to a \c double value: Target position value for DOF \f$ k \f$ 78 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i}^{\,trgt} \f$ 79 | //! The sign of this value will be flipped. 80 | //! 81 | //! \param ThisTargetVelocity 82 | //! Pointer to a \c double value: Target velocity value for DOF \f$ k \f$ 83 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,trgt} \f$ 84 | //! The sign of this value will be flipped. 85 | //! 86 | //! \param Inverted 87 | //! Pointer to \c bool value: This bit will be flipped; it indicates, 88 | //! whether the input values have been flipped even or odd times, such 89 | //! that succeeding functions can set-up the trajectory parameters 90 | //! correspondingly. 91 | //! 92 | //! \note 93 | //! This function is used in the Step 2 decision tree. 94 | //! 95 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 96 | //! \sa TypeIIRMLMath::NegateStep1() 97 | // ---------------------------------------------------------- 98 | void NegateStep2( double *ThisCurrentPosition 99 | , double *ThisCurrentVelocity 100 | , double *ThisTargetPosition 101 | , double *ThisTargetVelocity 102 | , bool *Inverted ); 103 | 104 | 105 | 106 | // ---------------------- Doxygen info ---------------------- 107 | //! \fn void VToVMaxStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted) 108 | //! 109 | //! \brief 110 | //! One intermediate Step 2 trajectory segment: v -> vmax (NegLin) 111 | //! 112 | //! \param ThisCurrentTime 113 | //! Pointer to a \c double value: Time in seconds required for preceding 114 | //! trajectory segments. This value 115 | //! will be increased by the amount of time required time for this 116 | //! trajectory segment. 117 | //! 118 | //! \param ThisCurrentPosition 119 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 120 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 121 | //! This value will be changed by the function in order to intermediately 122 | //! reach the maximum velocity value, \f$\ _{k}V_{i}^{\,max} \f$. 123 | //! 124 | //! \param ThisCurrentVelocity 125 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 126 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 127 | //! This value will be increased in order to intermediately reach the 128 | //! maximum velocity value, \f$\ _{k}V_{i}^{\,max} \f$. 129 | //! 130 | //! \param MaxVelocity 131 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 132 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$. 133 | //! 134 | //! \param MaxAcceleration 135 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 136 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$. 137 | //! 138 | //! \param PolynomialsLocal 139 | //! Pointer to a \c MotionPolynomials object, which contains the 140 | //! complete trajectory for one single DOF \f$ k \f$ at instant 141 | //! \f$ T_{i} \f$, \f$ _k{\cal M}_{i}(t) \f$. This function will 142 | //! add one further trajectory segment to this object. 143 | //! 144 | //! \param Inverted 145 | //! A \c bool value that indicates whether the input values have been 146 | //! flipped even or odd times prior to the execution of this function 147 | //! (cf. TypeIIRMLMath::NegatePink()). The trajectory parameters in 148 | //! \c PolynomialsLocal will be set-up correspondingly. 149 | //! 150 | //! \note 151 | //! This function is used in the Step 2 decision tree. 152 | //! 153 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 154 | //! \sa TypeIIRMLMath::VToVMaxStep1() 155 | // ---------------------------------------------------------- 156 | void VToVMaxStep2( double *ThisCurrentTime 157 | , double *ThisCurrentPosition 158 | , double *ThisCurrentVelocity 159 | , const double &MaxVelocity 160 | , const double &MaxAcceleration 161 | , MotionPolynomials *PolynomialsLocal 162 | , const bool &Inverted ); 163 | 164 | 165 | // ---------------------- Doxygen info ---------------------- 166 | //! \fn void VToZeroStep2(double *ThisCurrentTime, double *ThisCurrentPosition, double *ThisCurrentVelocity, const double &MaxAcceleration, MotionPolynomials *PolynomialsLocal, const bool &Inverted) 167 | //! 168 | //! \brief 169 | //! One intermediate Step 2 trajectory segment: v -> 0 (NegLin) 170 | //! 171 | //! \param ThisCurrentTime 172 | //! Pointer to a \c double value: Time in seconds required for preceding 173 | //! trajectory segments. This value 174 | //! will be increased by the amount of time required time for this 175 | //! trajectory segment. 176 | //! 177 | //! \param ThisCurrentPosition 178 | //! Pointer to a \c double value: Current position value for DOF \f$ k \f$ 179 | //! at instant \f$ T_{i} \f$, \f$\ _{k}P_{i} \f$. 180 | //! This value will be changed by the function in order to intermediately 181 | //! reach a velocity value of zero. 182 | //! 183 | //! \param ThisCurrentVelocity 184 | //! Pointer to a \c double value: Current velocity value for DOF \f$ k \f$ 185 | //! at instant \f$ T_{i} \f$, \f$\ _{k}V_{i} \f$ 186 | //! This value will be increased in order to intermediately reach a 187 | //! velocity value of zero. 188 | //! 189 | //! \param MaxAcceleration 190 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 191 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$. 192 | //! 193 | //! \param PolynomialsLocal 194 | //! Pointer to a \c MotionPolynomials object, which contains the 195 | //! complete trajectory for one single DOF \f$ k \f$ at instant 196 | //! \f$ T_{i} \f$, \f$ _k{\cal M}_{i}(t) \f$. This function will 197 | //! add one further trajectory segment to this object. 198 | //! 199 | //! \param Inverted 200 | //! A \c bool value that indicates whether the input values have been 201 | //! flipped even or odd times prior to the execution of this function 202 | //! (cf. TypeIIRMLMath::NegatePink()). The trajectory parameters in 203 | //! \c PolynomialsLocal will be set-up correspondingly. 204 | //! 205 | //! \note 206 | //! This function is used in the Step 2 decision tree. 207 | //! 208 | //! \sa TypeIIRMLMath::TypeIIRMLDecisionTree2() 209 | //! \sa TypeIIRMLMath::VToZeroStep1() 210 | // ---------------------------------------------------------- 211 | void VToZeroStep2( double *ThisCurrentTime 212 | , double *ThisCurrentPosition 213 | , double *ThisCurrentVelocity 214 | , const double &MaxAcceleration 215 | , MotionPolynomials *PolynomialsLocal 216 | , const bool &Inverted ); 217 | 218 | } // namespace TypeIIRMLMath 219 | 220 | #endif 221 | -------------------------------------------------------------------------------- /libreflexxestype2/include/libreflexxestype2/TypeIIRMLStep2WithoutSynchronization.h: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep2WithoutSynchronization.h 3 | //! 4 | //! \brief 5 | //! Header file file for the declaration of the function 6 | //! TypeIIRMLMath::Step2WithoutSynchronization(). 7 | //! 8 | //! \details 9 | //! Header file for the function TypeIIRMLMath::Step2WithoutSynchronization(), 10 | //! which sets up all polynomial parameters in the case of non-synchronized 11 | //! trajectories (cf. RMLFlags::NO_SYNCHRONIZATION). 12 | //! \n 13 | //! The function is part of the namespace TypeIIRMLMath. 14 | //! 15 | //! \date March 2014 16 | //! 17 | //! \version 1.2.6 18 | //! 19 | //! \author Torsten Kroeger, \n 20 | //! 21 | //! \copyright Copyright (C) 2014 Google, Inc. 22 | //! \n 23 | //! \n 24 | //! GNU Lesser General Public License 25 | //! \n 26 | //! \n 27 | //! This file is part of the Type II Reflexxes Motion Library. 28 | //! \n\n 29 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 30 | //! it and/or modify it under the terms of the GNU Lesser General Public License 31 | //! as published by the Free Software Foundation, either version 3 of the 32 | //! License, or (at your option) any later version. 33 | //! \n\n 34 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 35 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 36 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 37 | //! the GNU Lesser General Public License for more details. 38 | //! \n\n 39 | //! You should have received a copy of the GNU Lesser General Public License 40 | //! along with the Type II Reflexxes Motion Library. If not, see 41 | //! . 42 | // ---------------------------------------------------------- 43 | // For a convenient reading of this file's source code, 44 | // please use a tab width of four characters. 45 | // ---------------------------------------------------------- 46 | 47 | 48 | #ifndef __TypeIIRMLStep2WithoutSynchronization__ 49 | #define __TypeIIRMLStep2WithoutSynchronization__ 50 | 51 | 52 | #include 53 | #include 54 | #include 55 | 56 | 57 | namespace TypeIIRMLMath 58 | { 59 | 60 | // ---------------------- Doxygen info ---------------------- 61 | //! \fn void Step2WithoutSynchronization(const double &CurrentPosition, const double &CurrentVelocity, const double &TargetPosition, const double &TargetVelocity, const double &MaxVelocity, const double &MaxAcceleration, const TypeIIRMLMath::Step1_Profile &UsedProfile, const double &MinimumExecutionTime, MotionPolynomials *PolynomialsInternal) 62 | //! 63 | //! \brief 64 | //! This function contains sets of trajectory parameters (i.e., all 65 | //! polynomial coefficients) in the case of non-synchronized trajectories. 66 | //! 67 | //! \details 68 | //! Based on all input values for one selected degree of freedom and on the 69 | //! velocity profile that was applied in Step 1A, all trajectory 70 | //! parameters, that is, all polynomial coefficients, are set-up. This 71 | //! method is only applied in the case of non-synchronized trajectories 72 | //! (cf. RMLFlags::NO_SYNCHRONIZATION). 73 | //! 74 | //! \param CurrentPosition 75 | //! Current position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 76 | //! \f$\ _{k}P_{i} \f$ 77 | //! 78 | //! \param CurrentVelocity 79 | //! Current velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 80 | //! \f$\ _{k}V_{i} \f$ 81 | //! 82 | //! \param TargetPosition 83 | //! Target position value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 84 | //! \f$\ _{k}P_{i}^{\,trgt} \f$ 85 | //! 86 | //! \param TargetVelocity 87 | //! Target velocity value for DOF \f$ k \f$ at instant \f$ T_{i} \f$, 88 | //! \f$\ _{k}V_{i}^{\,trgt} \f$ 89 | //! 90 | //! \param MaxVelocity 91 | //! Maximum allowed velocity value for DOF \f$ k \f$ at instant 92 | //! \f$ T_{i} \f$, \f$\ _{k}V_{i}^{\,max} \f$ 93 | //! 94 | //! \param MaxAcceleration 95 | //! Maximum allowed acceleration value for DOF \f$ k \f$ at instant 96 | //! \f$ T_{i} \f$, \f$\ _{k}A_{i}^{\,max} \f$ 97 | //! 98 | //! \param UsedProfile 99 | //! The ID of the used acceleration profile in Step 1A (cf. 100 | //! RMLMath::Step1_Profile). 101 | //! 102 | //! \param MinimumExecutionTime 103 | //! Minimum execution time \f$ t_{i}^{\,min} \f$ for the current degree of 104 | //! freedom \f$ k \f$ (i.e., the result of profiles of the Step 1A 105 | //! 1A decision tree. 106 | //! 107 | //! \param PolynomialsInternal 108 | //! A pointer to a \c MotionPolynomials object (cf. 109 | //! TypeIIRMLMath::MotionPolynomials). All trajectory parameters of the 110 | //! synchronized Type II trajectory will be written into this object. 111 | //! 112 | //! \sa RMLFlags::NO_SYNCHRONIZATION 113 | // ---------------------------------------------------------- 114 | void Step2WithoutSynchronization( const double &CurrentPosition 115 | , const double &CurrentVelocity 116 | , const double &TargetPosition 117 | , const double &TargetVelocity 118 | , const double &MaxVelocity 119 | , const double &MaxAcceleration 120 | , const TypeIIRMLMath::Step1_Profile &UsedProfile 121 | , const double &MinimumExecutionTime 122 | , MotionPolynomials *PolynomialsInternal); 123 | 124 | } // namespace TypeIIRMLMath 125 | 126 | #endif 127 | 128 | -------------------------------------------------------------------------------- /libreflexxestype2/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | libreflexxestype2 5 | 0.0.0 6 | Reflexxes Type II trajectory smoothing 7 | Andy Zelenak 8 | Proprietary 9 | 10 | ament_cmake 11 | 12 | 13 | ament_cmake 14 | 15 | 16 | -------------------------------------------------------------------------------- /libreflexxestype2/src/RMLPositionSampleApplications/01_RMLPositionSampleApplication.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file 01_RMLPositionSampleApplication.cpp 3 | //! 4 | //! \brief 5 | //! Test application number 1 for the Reflexxes Motion Libraries 6 | //! (basic position-based interface) 7 | //! 8 | //! \date March 2014 9 | //! 10 | //! \version 1.2.6 11 | //! 12 | //! \author Torsten Kroeger, \n 13 | //! 14 | //! \copyright Copyright (C) 2014 Google, Inc. 15 | //! \n 16 | //! \n 17 | //! GNU Lesser General Public License 18 | //! \n 19 | //! \n 20 | //! This file is part of the Type II Reflexxes Motion Library. 21 | //! \n\n 22 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 23 | //! it and/or modify it under the terms of the GNU Lesser General Public License 24 | //! as published by the Free Software Foundation, either version 3 of the 25 | //! License, or (at your option) any later version. 26 | //! \n\n 27 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 28 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 29 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 30 | //! the GNU Lesser General Public License for more details. 31 | //! \n\n 32 | //! You should have received a copy of the GNU Lesser General Public License 33 | //! along with the Type II Reflexxes Motion Library. If not, see 34 | //! . 35 | // ---------------------------------------------------------- 36 | // For a convenient reading of this file's source code, 37 | // please use a tab width of four characters. 38 | // ---------------------------------------------------------- 39 | 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | //************************************************************************* 51 | // defines 52 | 53 | #define CYCLE_TIME_IN_SECONDS 0.001 54 | #define NUMBER_OF_DOFS 3 55 | 56 | 57 | //************************************************************************* 58 | // Main function to run the process that contains the test application 59 | // 60 | // This function contains source code to get started with the Type II 61 | // Reflexxes Motion Library. Only a minimum amount of functionality is 62 | // contained in this program: a simple trajectory for a 63 | // three-degree-of-freedom system is executed. This code snippet 64 | // directly corresponds to the example trajectories shown in the 65 | // documentation. 66 | //************************************************************************* 67 | int main() 68 | { 69 | // ******************************************************************** 70 | // Variable declarations and definitions 71 | 72 | int ResultValue = 0 ; 73 | 74 | ReflexxesAPI *RML = NULL ; 75 | 76 | RMLPositionInputParameters *IP = NULL ; 77 | 78 | RMLPositionOutputParameters *OP = NULL ; 79 | 80 | RMLPositionFlags Flags ; 81 | 82 | // ******************************************************************** 83 | // Creating all relevant objects of the Type II Reflexxes Motion Library 84 | 85 | RML = new ReflexxesAPI( NUMBER_OF_DOFS 86 | , CYCLE_TIME_IN_SECONDS ); 87 | 88 | IP = new RMLPositionInputParameters( NUMBER_OF_DOFS ); 89 | 90 | OP = new RMLPositionOutputParameters( NUMBER_OF_DOFS ); 91 | 92 | // ******************************************************************** 93 | // Set-up a timer with a period of one millisecond 94 | // (not implemented in this example in order to keep it simple) 95 | // ******************************************************************** 96 | 97 | printf("-------------------------------------------------------\n" ); 98 | printf("Reflexxes Motion Libraries \n" ); 99 | printf("Example: 01_RMLPositionSampleApplication \n\n"); 100 | printf("This example demonstrates the most basic use of the \n" ); 101 | printf("Reflexxes API (class ReflexxesAPI) using the position- \n" ); 102 | printf("based Type II Online Trajectory Generation algorithm. \n\n"); 103 | printf("Copyright (C) 2014 Google, Inc. \n" ); 104 | printf("-------------------------------------------------------\n" ); 105 | 106 | // ******************************************************************** 107 | // Set-up the input parameters 108 | 109 | // In this test program, arbitrary values are chosen. If executed on a 110 | // real robot or mechanical system, the position is read and stored in 111 | // an RMLPositionInputParameters::CurrentPositionVector vector object. 112 | // For the very first motion after starting the controller, velocities 113 | // and acceleration are commonly set to zero. The desired target state 114 | // of motion and the motion constraints depend on the robot and the 115 | // current task/application. 116 | // The internal data structures make use of native C data types 117 | // (e.g., IP->CurrentPositionVector->VecData is a pointer to 118 | // an array of NUMBER_OF_DOFS double values), such that the Reflexxes 119 | // Library can be used in a universal way. 120 | 121 | IP->CurrentPositionVector->VecData [0] = 100.0 ; 122 | IP->CurrentPositionVector->VecData [1] = 0.0 ; 123 | IP->CurrentPositionVector->VecData [2] = 50.0 ; 124 | 125 | IP->CurrentVelocityVector->VecData [0] = 100.0 ; 126 | IP->CurrentVelocityVector->VecData [1] = -220.0 ; 127 | IP->CurrentVelocityVector->VecData [2] = -50.0 ; 128 | 129 | IP->CurrentAccelerationVector->VecData [0] = -150.0 ; 130 | IP->CurrentAccelerationVector->VecData [1] = 250.0 ; 131 | IP->CurrentAccelerationVector->VecData [2] = -50.0 ; 132 | 133 | IP->MaxVelocityVector->VecData [0] = 300.0 ; 134 | IP->MaxVelocityVector->VecData [1] = 100.0 ; 135 | IP->MaxVelocityVector->VecData [2] = 300.0 ; 136 | 137 | IP->MaxAccelerationVector->VecData [0] = 300.0 ; 138 | IP->MaxAccelerationVector->VecData [1] = 200.0 ; 139 | IP->MaxAccelerationVector->VecData [2] = 100.0 ; 140 | 141 | IP->MaxJerkVector->VecData [0] = 400.0 ; 142 | IP->MaxJerkVector->VecData [1] = 300.0 ; 143 | IP->MaxJerkVector->VecData [2] = 200.0 ; 144 | 145 | IP->TargetPositionVector->VecData [0] = -600.0 ; 146 | IP->TargetPositionVector->VecData [1] = -200.0 ; 147 | IP->TargetPositionVector->VecData [2] = -350.0 ; 148 | 149 | IP->TargetVelocityVector->VecData [0] = 50.0 ; 150 | IP->TargetVelocityVector->VecData [1] = -50.0 ; 151 | IP->TargetVelocityVector->VecData [2] = -200.0 ; 152 | 153 | IP->SelectionVector->VecData [0] = true ; 154 | IP->SelectionVector->VecData [1] = true ; 155 | IP->SelectionVector->VecData [2] = true ; 156 | 157 | // ******************************************************************** 158 | // Starting the control loop 159 | 160 | while (ResultValue != ReflexxesAPI::RML_FINAL_STATE_REACHED) 161 | { 162 | 163 | // **************************************************************** 164 | // Wait for the next timer tick 165 | // (not implemented in this example in order to keep it simple) 166 | // **************************************************************** 167 | 168 | // Calling the Reflexxes OTG algorithm 169 | ResultValue = RML->RMLPosition( *IP 170 | , OP 171 | , Flags ); 172 | 173 | if (ResultValue < 0) 174 | { 175 | printf("An error occurred (%d).\n", ResultValue ); 176 | break; 177 | } 178 | 179 | // **************************************************************** 180 | // Here, the new state of motion, that is 181 | // 182 | // - OP->NewPositionVector 183 | // - OP->NewVelocityVector 184 | // - OP->NewAccelerationVector 185 | // 186 | // can be used as input values for lower level controllers. In the 187 | // most simple case, a position controller in actuator space is 188 | // used, but the computed state can be applied to many other 189 | // controllers (e.g., Cartesian impedance controllers, 190 | // operational space controllers). 191 | // **************************************************************** 192 | 193 | // **************************************************************** 194 | // Feed the output values of the current control cycle back to 195 | // input values of the next control cycle 196 | 197 | *IP->CurrentPositionVector = *OP->NewPositionVector ; 198 | *IP->CurrentVelocityVector = *OP->NewVelocityVector ; 199 | *IP->CurrentAccelerationVector = *OP->NewAccelerationVector ; 200 | } 201 | 202 | // ******************************************************************** 203 | // Deleting the objects of the Reflexxes Motion Library end terminating 204 | // the process 205 | 206 | delete RML ; 207 | delete IP ; 208 | delete OP ; 209 | 210 | exit(EXIT_SUCCESS) ; 211 | } 212 | -------------------------------------------------------------------------------- /libreflexxestype2/src/RMLVelocitySampleApplications/04_RMLVelocitySampleApplication.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file 04_RMLVelocitySampleApplication.cpp 3 | //! 4 | //! \brief 5 | //! Test application number 1 for the Reflexxes Motion Libraries 6 | //! (basic velocity-based interface) 7 | //! 8 | //! \date March 2014 9 | //! 10 | //! \version 1.2.6 11 | //! 12 | //! \author Torsten Kroeger, \n 13 | //! 14 | //! \copyright Copyright (C) 2014 Google, Inc. 15 | //! \n 16 | //! \n 17 | //! GNU Lesser General Public License 18 | //! \n 19 | //! \n 20 | //! This file is part of the Type II Reflexxes Motion Library. 21 | //! \n\n 22 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 23 | //! it and/or modify it under the terms of the GNU Lesser General Public License 24 | //! as published by the Free Software Foundation, either version 3 of the 25 | //! License, or (at your option) any later version. 26 | //! \n\n 27 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 28 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 29 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 30 | //! the GNU Lesser General Public License for more details. 31 | //! \n\n 32 | //! You should have received a copy of the GNU Lesser General Public License 33 | //! along with the Type II Reflexxes Motion Library. If not, see 34 | //! . 35 | // ---------------------------------------------------------- 36 | // For a convenient reading of this file's source code, 37 | // please use a tab width of four characters. 38 | // ---------------------------------------------------------- 39 | 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | //************************************************************************* 51 | // defines 52 | 53 | #define CYCLE_TIME_IN_SECONDS 0.001 54 | #define NUMBER_OF_DOFS 3 55 | 56 | 57 | //************************************************************************* 58 | // Main function to run the process that contains the test application 59 | // 60 | // This function contains source code to get started with the Type II 61 | // Reflexxes Motion Library. Only a minimum amount of functionality is 62 | // contained in this program: a simple trajectory for a 63 | // three-degree-of-freedom system is executed. This code snippet 64 | // directly corresponds to the example trajectories shown in the 65 | // documentation. 66 | //************************************************************************* 67 | int main() 68 | { 69 | // ******************************************************************** 70 | // Variable declarations and definitions 71 | 72 | int ResultValue = 0 ; 73 | 74 | ReflexxesAPI *RML = NULL ; 75 | 76 | RMLVelocityInputParameters *IP = NULL ; 77 | 78 | RMLVelocityOutputParameters *OP = NULL ; 79 | 80 | RMLVelocityFlags Flags ; 81 | 82 | // ******************************************************************** 83 | // Creating all relevant objects of the Type II Reflexxes Motion Library 84 | 85 | RML = new ReflexxesAPI( NUMBER_OF_DOFS 86 | , CYCLE_TIME_IN_SECONDS ); 87 | 88 | IP = new RMLVelocityInputParameters( NUMBER_OF_DOFS ); 89 | 90 | OP = new RMLVelocityOutputParameters( NUMBER_OF_DOFS ); 91 | 92 | // ******************************************************************** 93 | // Set-up a timer with a period of one millisecond 94 | // (not implemented in this example in order to keep it simple) 95 | // ******************************************************************** 96 | 97 | printf("-------------------------------------------------------\n" ); 98 | printf("Reflexxes Motion Libraries \n" ); 99 | printf("Example: 04_RMLVelocitySampleApplication \n\n"); 100 | printf("This example demonstrates the most basic use of the \n" ); 101 | printf("Reflexxes API (class ReflexxesAPI) using the velocity- \n" ); 102 | printf("based Type II Online Trajectory Generation algorithm. \n\n"); 103 | printf("Copyright (C) 2014 Google, Inc. \n" ); 104 | printf("-------------------------------------------------------\n" ); 105 | 106 | // ******************************************************************** 107 | // Set-up the input parameters 108 | 109 | // In this test program, arbitrary values are chosen. If executed on a 110 | // real robot or mechanical system, the position is read and stored in 111 | // an RMLVelocityInputParameters::CurrentPositionVector vector object. 112 | // For the very first motion after starting the controller, velocities 113 | // and acceleration are commonly set to zero. The desired target state 114 | // of motion and the motion constraints depend on the robot and the 115 | // current task/application. 116 | // The internal data structures make use of native C data types 117 | // (e.g., IP->CurrentPositionVector->VecData is a pointer to 118 | // an array of NUMBER_OF_DOFS double values), such that the Reflexxes 119 | // Library can be used in a universal way. 120 | 121 | IP->CurrentPositionVector->VecData [0] = -200.0 ; 122 | IP->CurrentPositionVector->VecData [1] = 100.0 ; 123 | IP->CurrentPositionVector->VecData [2] = -300.0 ; 124 | 125 | IP->CurrentVelocityVector->VecData [0] = -150.0 ; 126 | IP->CurrentVelocityVector->VecData [1] = 100.0 ; 127 | IP->CurrentVelocityVector->VecData [2] = 50.0 ; 128 | 129 | IP->CurrentAccelerationVector->VecData [0] = 350.0 ; 130 | IP->CurrentAccelerationVector->VecData [1] = -500.0 ; 131 | IP->CurrentAccelerationVector->VecData [2] = 0.0 ; 132 | 133 | IP->MaxAccelerationVector->VecData [0] = 500.0 ; 134 | IP->MaxAccelerationVector->VecData [1] = 500.0 ; 135 | IP->MaxAccelerationVector->VecData [2] = 1000.0 ; 136 | 137 | IP->MaxJerkVector->VecData [0] = 1000.0 ; 138 | IP->MaxJerkVector->VecData [1] = 700.0 ; 139 | IP->MaxJerkVector->VecData [2] = 500.0 ; 140 | 141 | IP->TargetVelocityVector->VecData [0] = 150.0 ; 142 | IP->TargetVelocityVector->VecData [1] = 75.0 ; 143 | IP->TargetVelocityVector->VecData [2] = 100.0 ; 144 | 145 | IP->SelectionVector->VecData [0] = true ; 146 | IP->SelectionVector->VecData [1] = true ; 147 | IP->SelectionVector->VecData [2] = true ; 148 | 149 | // ******************************************************************** 150 | // Starting the control loop 151 | 152 | while (ResultValue != ReflexxesAPI::RML_FINAL_STATE_REACHED) 153 | { 154 | 155 | // **************************************************************** 156 | // Wait for the next timer tick 157 | // (not implemented in this example in order to keep it simple) 158 | // **************************************************************** 159 | 160 | // Calling the Reflexxes OTG algorithm 161 | ResultValue = RML->RMLVelocity( *IP 162 | , OP 163 | , Flags ); 164 | 165 | if (ResultValue < 0) 166 | { 167 | printf("An error occurred (%d).\n", ResultValue ); 168 | break; 169 | } 170 | 171 | // **************************************************************** 172 | // Here, the new state of motion, that is 173 | // 174 | // - OP->NewPositionVector 175 | // - OP->NewVelocityVector 176 | // - OP->NewAccelerationVector 177 | // 178 | // can be used as input values for lower level controllers. In the 179 | // most simple case, a position controller in actuator space is 180 | // used, but the computed state can be applied to many other 181 | // controllers (e.g., Cartesian impedance controllers, 182 | // operational space controllers). 183 | // **************************************************************** 184 | 185 | // **************************************************************** 186 | // Feed the output values of the current control cycle back to 187 | // input values of the next control cycle 188 | 189 | *IP->CurrentPositionVector = *OP->NewPositionVector ; 190 | *IP->CurrentVelocityVector = *OP->NewVelocityVector ; 191 | *IP->CurrentAccelerationVector = *OP->NewAccelerationVector ; 192 | } 193 | 194 | // ******************************************************************** 195 | // Deleting the objects of the Reflexxes Motion Library end terminating 196 | // the process 197 | 198 | delete RML ; 199 | delete IP ; 200 | delete OP ; 201 | 202 | exit(EXIT_SUCCESS) ; 203 | } 204 | -------------------------------------------------------------------------------- /libreflexxestype2/src/RMLVelocitySampleApplications/06_RMLVelocitySampleApplication.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file 06_RMLVelocitySampleApplication.cpp 3 | //! 4 | //! \brief 5 | //! Test application number 3 for the Reflexxes Motion Libraries 6 | //! (basic velocity-based interface) 7 | //! 8 | //! \date March 2014 9 | //! 10 | //! \version 1.2.6 11 | //! 12 | //! \author Torsten Kroeger, \n 13 | //! 14 | //! \copyright Copyright (C) 2014 Google, Inc. 15 | //! \n 16 | //! \n 17 | //! GNU Lesser General Public License 18 | //! \n 19 | //! \n 20 | //! This file is part of the Type II Reflexxes Motion Library. 21 | //! \n\n 22 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 23 | //! it and/or modify it under the terms of the GNU Lesser General Public License 24 | //! as published by the Free Software Foundation, either version 3 of the 25 | //! License, or (at your option) any later version. 26 | //! \n\n 27 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 28 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 29 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 30 | //! the GNU Lesser General Public License for more details. 31 | //! \n\n 32 | //! You should have received a copy of the GNU Lesser General Public License 33 | //! along with the Type II Reflexxes Motion Library. If not, see 34 | //! . 35 | // ---------------------------------------------------------- 36 | // For a convenient reading of this file's source code, 37 | // please use a tab width of four characters. 38 | // ---------------------------------------------------------- 39 | 40 | 41 | #include 42 | #include 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | //************************************************************************* 51 | // defines 52 | 53 | #define CYCLE_TIME_IN_SECONDS 0.001 54 | #define NUMBER_OF_DOFS 3 55 | 56 | 57 | //************************************************************************* 58 | // Main function to run the process that contains the test application 59 | // 60 | // This function contains source code to get started with the Type II 61 | // Reflexxes Motion Library. Only a minimum amount of functionality is 62 | // contained in this program: a simple trajectory for a 63 | // three-degree-of-freedom system is executed. This code snippet 64 | // directly corresponds to the example trajectories shown in the 65 | // documentation. 66 | //************************************************************************* 67 | int main() 68 | { 69 | // ******************************************************************** 70 | // Variable declarations and definitions 71 | 72 | int ResultValue = 0 ; 73 | 74 | ReflexxesAPI *RML = NULL ; 75 | 76 | RMLVelocityInputParameters *IP = NULL ; 77 | 78 | RMLVelocityOutputParameters *OP = NULL ; 79 | 80 | RMLVelocityFlags Flags ; 81 | 82 | // ******************************************************************** 83 | // Creating all relevant objects of the Type II Reflexxes Motion Library 84 | 85 | RML = new ReflexxesAPI( NUMBER_OF_DOFS 86 | , CYCLE_TIME_IN_SECONDS ); 87 | 88 | IP = new RMLVelocityInputParameters( NUMBER_OF_DOFS ); 89 | 90 | OP = new RMLVelocityOutputParameters( NUMBER_OF_DOFS ); 91 | 92 | // ******************************************************************** 93 | // Set-up a timer with a period of one millisecond 94 | // (not implemented in this example in order to keep it simple) 95 | // ******************************************************************** 96 | 97 | printf("-------------------------------------------------------\n" ); 98 | printf("Reflexxes Motion Libraries \n" ); 99 | printf("Example: 06_RMLVelocitySampleApplication.cpp \n\n"); 100 | printf("This example demonstrates the most basic use of the \n" ); 101 | printf("Reflexxes API (class ReflexxesAPI) using the velocity- \n" ); 102 | printf("based Type II Online Trajectory Generation algorithm. \n\n"); 103 | printf("Copyright (C) 2014 Google, Inc. \n" ); 104 | printf("-------------------------------------------------------\n" ); 105 | 106 | // ******************************************************************** 107 | // Set-up the input parameters 108 | 109 | // In this test program, arbitrary values are chosen. If executed on a 110 | // real robot or mechanical system, the position is read and stored in 111 | // an RMLVelocityInputParameters::CurrentPositionVector vector object. 112 | // For the very first motion after starting the controller, velocities 113 | // and acceleration are commonly set to zero. The desired target state 114 | // of motion and the motion constraints depend on the robot and the 115 | // current task/application. 116 | // The internal data structures make use of native C data types 117 | // (e.g., IP->CurrentPositionVector->VecData is a pointer to 118 | // an array of NUMBER_OF_DOFS double values), such that the Reflexxes 119 | // Library can be used in a universal way. 120 | 121 | IP->CurrentPositionVector->VecData [0] = 100.0 ; 122 | IP->CurrentPositionVector->VecData [1] = 50.0 ; 123 | IP->CurrentPositionVector->VecData [2] = -100.0 ; 124 | 125 | IP->CurrentVelocityVector->VecData [0] = 80.0 ; 126 | IP->CurrentVelocityVector->VecData [1] = 25.0 ; 127 | IP->CurrentVelocityVector->VecData [2] = -50.0 ; 128 | 129 | IP->CurrentAccelerationVector->VecData [0] = -50.0 ; 130 | IP->CurrentAccelerationVector->VecData [1] = -150.0 ; 131 | IP->CurrentAccelerationVector->VecData [2] = 80.0 ; 132 | 133 | IP->MaxAccelerationVector->VecData [0] = 400.0 ; 134 | IP->MaxAccelerationVector->VecData [1] = 300.0 ; 135 | IP->MaxAccelerationVector->VecData [2] = 500.0 ; 136 | 137 | IP->MaxJerkVector->VecData [0] = 500.0 ; 138 | IP->MaxJerkVector->VecData [1] = 400.0 ; 139 | IP->MaxJerkVector->VecData [2] = 300.0 ; 140 | 141 | IP->TargetVelocityVector->VecData [0] = 200.0 ; 142 | IP->TargetVelocityVector->VecData [1] = -150.0 ; 143 | IP->TargetVelocityVector->VecData [2] = -20.0 ; 144 | 145 | IP->SelectionVector->VecData [0] = true ; 146 | IP->SelectionVector->VecData [1] = true ; 147 | IP->SelectionVector->VecData [2] = true ; 148 | 149 | Flags.SynchronizationBehavior = RMLFlags::ONLY_TIME_SYNCHRONIZATION; 150 | 151 | // ******************************************************************** 152 | // Starting the control loop 153 | 154 | while (ResultValue != ReflexxesAPI::RML_FINAL_STATE_REACHED) 155 | { 156 | 157 | // **************************************************************** 158 | // Wait for the next timer tick 159 | // (not implemented in this example in order to keep it simple) 160 | // **************************************************************** 161 | 162 | // Calling the Reflexxes OTG algorithm 163 | ResultValue = RML->RMLVelocity( *IP 164 | , OP 165 | , Flags ); 166 | 167 | if (ResultValue < 0) 168 | { 169 | printf("An error occurred (%d).\n", ResultValue ); 170 | break; 171 | } 172 | 173 | // **************************************************************** 174 | // Here, the new state of motion, that is 175 | // 176 | // - OP->NewPositionVector 177 | // - OP->NewVelocityVector 178 | // - OP->NewAccelerationVector 179 | // 180 | // can be used as input values for lower level controllers. In the 181 | // most simple case, a position controller in actuator space is 182 | // used, but the computed state can be applied to many other 183 | // controllers (e.g., Cartesian impedance controllers, 184 | // operational space controllers). 185 | // **************************************************************** 186 | 187 | // **************************************************************** 188 | // Feed the output values of the current control cycle back to 189 | // input values of the next control cycle 190 | 191 | *IP->CurrentPositionVector = *OP->NewPositionVector ; 192 | *IP->CurrentVelocityVector = *OP->NewVelocityVector ; 193 | *IP->CurrentAccelerationVector = *OP->NewAccelerationVector ; 194 | } 195 | 196 | // ******************************************************************** 197 | // Deleting the objects of the Reflexxes Motion Library end terminating 198 | // the process 199 | 200 | delete RML ; 201 | delete IP ; 202 | delete OP ; 203 | 204 | exit(EXIT_SUCCESS) ; 205 | } 206 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/ReflexxesAPI.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file ReflexxesAPI.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the user interface (API) 6 | //! 7 | //! \details 8 | //! Implementation file for all methods of the class ReflexxesAPI, which 9 | //! constitutes the user API for On-Line Trajectory Generation 10 | //! algorithms. 11 | //! For further information, please refer to the file ReflexxesAPI.h. 12 | //! 13 | //! \date March 2014 14 | //! 15 | //! \version 1.2.6 16 | //! 17 | //! \author Torsten Kroeger, \n 18 | //! 19 | //! \copyright Copyright (C) 2014 Google, Inc. 20 | //! \n 21 | //! \n 22 | //! GNU Lesser General Public License 23 | //! \n 24 | //! \n 25 | //! This file is part of the Type II Reflexxes Motion Library. 26 | //! \n\n 27 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 28 | //! it and/or modify it under the terms of the GNU Lesser General Public License 29 | //! as published by the Free Software Foundation, either version 3 of the 30 | //! License, or (at your option) any later version. 31 | //! \n\n 32 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 33 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 34 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 35 | //! the GNU Lesser General Public License for more details. 36 | //! \n\n 37 | //! You should have received a copy of the GNU Lesser General Public License 38 | //! along with the Type II Reflexxes Motion Library. If not, see 39 | //! . 40 | // ---------------------------------------------------------- 41 | // For a convenient reading of this file's source code, 42 | // please use a tab width of four characters. 43 | // ---------------------------------------------------------- 44 | 45 | 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | #include 53 | #include 54 | #include 55 | 56 | 57 | //**************************************************************************** 58 | // ReflexxesAPI() 59 | 60 | ReflexxesAPI::ReflexxesAPI( const unsigned int &DegreesOfFreedom 61 | , const double &CycleTimeInSeconds 62 | , const unsigned int &NumberOfAdditionalThreads) 63 | { 64 | this->NumberOfDOFs = DegreesOfFreedom ; 65 | this->NumberOfOwnThreads = NumberOfAdditionalThreads ; 66 | this->CycleTime = CycleTimeInSeconds ; 67 | 68 | this->RMLPositionObject = (void*) new TypeIIRMLPosition( DegreesOfFreedom 69 | , CycleTimeInSeconds); 70 | 71 | this->RMLVelocityObject = (void*) new TypeIIRMLVelocity( DegreesOfFreedom 72 | , CycleTimeInSeconds); 73 | } 74 | 75 | 76 | //**************************************************************************** 77 | // ~ReflexxesAPI() 78 | 79 | ReflexxesAPI::~ReflexxesAPI(void) 80 | { 81 | delete (TypeIIRMLVelocity*)this->RMLVelocityObject; 82 | delete (TypeIIRMLPosition*)this->RMLPositionObject; 83 | 84 | this->RMLVelocityObject = NULL; 85 | this->RMLPositionObject = NULL; 86 | } 87 | 88 | 89 | //**************************************************************************** 90 | // RMLPosition() 91 | 92 | int ReflexxesAPI::RMLPosition( const RMLPositionInputParameters &InputValues 93 | , RMLPositionOutputParameters *OutputValues 94 | , const RMLPositionFlags &Flags) 95 | { 96 | return(((TypeIIRMLPosition*)(this->RMLPositionObject))->GetNextStateOfMotion( InputValues 97 | , OutputValues 98 | , Flags )); 99 | } 100 | 101 | 102 | //**************************************************************************** 103 | // RMLPositionAtAGivenSampleTime() 104 | 105 | int ReflexxesAPI::RMLPositionAtAGivenSampleTime( const double &TimeValueInSeconds 106 | , RMLPositionOutputParameters *OutputValues) 107 | { 108 | return(((TypeIIRMLPosition*)(this->RMLPositionObject))->GetNextStateOfMotionAtTime( TimeValueInSeconds 109 | , OutputValues )); 110 | } 111 | 112 | 113 | //**************************************************************************** 114 | // RMLVelocity() 115 | 116 | int ReflexxesAPI::RMLVelocity( const RMLVelocityInputParameters &InputValues 117 | , RMLVelocityOutputParameters *OutputValues 118 | , const RMLVelocityFlags &Flags) 119 | { 120 | return(((TypeIIRMLVelocity*)(this->RMLVelocityObject))->GetNextStateOfMotion( InputValues 121 | , OutputValues 122 | , Flags )); 123 | } 124 | 125 | 126 | //**************************************************************************** 127 | // RMLVelocityAtAGivenSampleTime() 128 | 129 | int ReflexxesAPI::RMLVelocityAtAGivenSampleTime( const double &TimeValueInSeconds 130 | , RMLVelocityOutputParameters *OutputValues) 131 | { 132 | return(((TypeIIRMLVelocity*)(this->RMLVelocityObject))->GetNextStateOfMotionAtTime( TimeValueInSeconds 133 | , OutputValues )); 134 | } 135 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLDecisionTree1B.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree1B.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Step 1 decision tree 1B of the Type II 6 | //! On-Line Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file 10 | //! TypeIIRMLDecisionTree1B.h 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | 51 | 52 | //************************************************************************************ 53 | // TypeIIRMLDecisionTree1B() 54 | 55 | void TypeIIRMLMath::TypeIIRMLDecisionTree1B( const double &CurrentPosition 56 | , const double &CurrentVelocity 57 | , const double &TargetPosition 58 | , const double &TargetVelocity 59 | , const double &MaxVelocity 60 | , const double &MaxAcceleration 61 | , double *MaximalExecutionTime) 62 | { 63 | double ThisCurrentPosition = CurrentPosition 64 | , ThisCurrentVelocity = CurrentVelocity 65 | , ThisTargetPosition = TargetPosition 66 | , ThisTargetVelocity = TargetVelocity ; 67 | 68 | *MaximalExecutionTime = 0.0; 69 | 70 | // ******************************************************************** 71 | if (Decision_1B__001(ThisCurrentVelocity)) 72 | { 73 | goto MDecision_1B__002; 74 | } 75 | else 76 | { 77 | NegateStep1( &ThisCurrentPosition 78 | , &ThisCurrentVelocity 79 | , &ThisTargetPosition 80 | , &ThisTargetVelocity ); 81 | 82 | goto MDecision_1B__002; 83 | } 84 | // ******************************************************************** 85 | MDecision_1B__002: 86 | if (Decision_1B__002( ThisCurrentVelocity 87 | , MaxVelocity )) 88 | { 89 | goto MDecision_1B__003; 90 | } 91 | else 92 | { 93 | VToVMaxStep1( MaximalExecutionTime 94 | , &ThisCurrentPosition 95 | , &ThisCurrentVelocity 96 | , MaxVelocity 97 | , MaxAcceleration ); 98 | 99 | goto MDecision_1B__003; 100 | } 101 | // ******************************************************************** 102 | MDecision_1B__003: 103 | if (Decision_1B__003(ThisTargetVelocity)) 104 | { 105 | goto MDecision_1B__004; 106 | } 107 | else 108 | { 109 | *MaximalExecutionTime = RML_INFINITY; 110 | goto END_OF_THIS_FUNCTION; 111 | } 112 | // ******************************************************************** 113 | MDecision_1B__004: 114 | if (Decision_1B__004( ThisCurrentVelocity 115 | , ThisTargetVelocity )) 116 | { 117 | goto MDecision_1B__005; 118 | } 119 | else 120 | { 121 | goto MDecision_1B__007; 122 | } 123 | // ******************************************************************** 124 | MDecision_1B__005: 125 | if (Decision_1B__005( ThisCurrentPosition 126 | , ThisCurrentVelocity 127 | , ThisTargetPosition 128 | , ThisTargetVelocity 129 | , MaxAcceleration )) 130 | { 131 | goto MDecision_1B__006; 132 | } 133 | else 134 | { 135 | *MaximalExecutionTime = RML_INFINITY; 136 | goto END_OF_THIS_FUNCTION; 137 | } 138 | // ******************************************************************** 139 | MDecision_1B__006: 140 | if (Decision_1B__006( ThisCurrentPosition 141 | , ThisCurrentVelocity 142 | , ThisTargetPosition 143 | , ThisTargetVelocity 144 | , MaxAcceleration )) 145 | { 146 | *MaximalExecutionTime = RML_INFINITY; 147 | goto END_OF_THIS_FUNCTION; 148 | } 149 | else 150 | { 151 | *MaximalExecutionTime += ProfileStep1NegLinPosLin( ThisCurrentPosition 152 | , ThisCurrentVelocity 153 | , ThisTargetPosition 154 | , ThisTargetVelocity 155 | , MaxAcceleration ); 156 | goto END_OF_THIS_FUNCTION; 157 | } 158 | // ******************************************************************** 159 | MDecision_1B__007: 160 | if (Decision_1B__007( ThisCurrentPosition 161 | , ThisCurrentVelocity 162 | , ThisTargetPosition 163 | , ThisTargetVelocity 164 | , MaxAcceleration )) 165 | { 166 | *MaximalExecutionTime = RML_INFINITY; 167 | goto END_OF_THIS_FUNCTION; 168 | } 169 | else 170 | { 171 | goto MDecision_1B__006; 172 | } 173 | // ******************************************************************** 174 | END_OF_THIS_FUNCTION: 175 | 176 | return; 177 | } 178 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLDecisionTree1C.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLDecisionTree1C.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Step 1 decision tree 1C of the Type II 6 | //! On-Line Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file 10 | //! TypeIIRMLDecisionTree1C.h 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | //************************************************************************************ 54 | // TypeIIRMLDecisionTree1C() 55 | 56 | void TypeIIRMLMath::TypeIIRMLDecisionTree1C( const double &CurrentPosition 57 | , const double &CurrentVelocity 58 | , const double &TargetPosition 59 | , const double &TargetVelocity 60 | , const double &MaxVelocity 61 | , const double &MaxAcceleration 62 | , double *AlternativeExecutionTime) 63 | { 64 | double ThisCurrentPosition = CurrentPosition 65 | , ThisCurrentVelocity = CurrentVelocity 66 | , ThisTargetPosition = TargetPosition 67 | , ThisTargetVelocity = TargetVelocity ; 68 | 69 | *AlternativeExecutionTime = 0.0; 70 | 71 | // ******************************************************************** 72 | if (Decision_1C__001(ThisCurrentVelocity)) 73 | { 74 | goto MDecision_1C__002; 75 | } 76 | else 77 | { 78 | NegateStep1( &ThisCurrentPosition 79 | , &ThisCurrentVelocity 80 | , &ThisTargetPosition 81 | , &ThisTargetVelocity ); 82 | 83 | goto MDecision_1C__002; 84 | } 85 | // ******************************************************************** 86 | MDecision_1C__002: 87 | if (Decision_1C__002( ThisCurrentVelocity 88 | , MaxVelocity )) 89 | { 90 | goto MDecision_1C__003; 91 | } 92 | else 93 | { 94 | VToVMaxStep1( AlternativeExecutionTime 95 | , &ThisCurrentPosition 96 | , &ThisCurrentVelocity 97 | , MaxVelocity 98 | , MaxAcceleration ); 99 | 100 | goto MDecision_1C__003; 101 | } 102 | // ******************************************************************** 103 | MDecision_1C__003: 104 | 105 | VToZeroStep1( AlternativeExecutionTime 106 | , &ThisCurrentPosition 107 | , &ThisCurrentVelocity 108 | , MaxAcceleration ); 109 | 110 | NegateStep1( &ThisCurrentPosition 111 | , &ThisCurrentVelocity 112 | , &ThisTargetPosition 113 | , &ThisTargetVelocity ); 114 | 115 | 116 | if (Decision_1C__003( ThisCurrentPosition 117 | , ThisCurrentVelocity 118 | , ThisTargetPosition 119 | , ThisTargetVelocity 120 | , MaxVelocity 121 | , MaxAcceleration )) 122 | { 123 | *AlternativeExecutionTime += ProfileStep1PosTriNegLin( ThisCurrentPosition 124 | , ThisCurrentVelocity 125 | , ThisTargetPosition 126 | , ThisTargetVelocity 127 | , MaxAcceleration ); 128 | } 129 | else 130 | { 131 | *AlternativeExecutionTime += ProfileStep1PosTrapNegLin( ThisCurrentPosition 132 | , ThisCurrentVelocity 133 | , ThisTargetPosition 134 | , ThisTargetVelocity 135 | , MaxVelocity 136 | , MaxAcceleration ); 137 | } 138 | // ******************************************************************** 139 | 140 | return; 141 | } 142 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLFallBackStrategy.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLFallBackStrategy.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLPosition.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | 54 | //******************************************************************************************* 55 | // FallBackStrategy() 56 | 57 | void TypeIIRMLPosition::FallBackStrategy( const RMLPositionInputParameters &InputValues 58 | , RMLPositionOutputParameters *OutputValues 59 | , const RMLPositionFlags &InputsFlags) 60 | { 61 | unsigned int i = 0; 62 | 63 | *(this->VelocityInputParameters->SelectionVector) 64 | = *(InputValues.SelectionVector); 65 | *(this->VelocityInputParameters->CurrentPositionVector) 66 | = *(InputValues.CurrentPositionVector); 67 | *(this->VelocityInputParameters->CurrentVelocityVector) 68 | = *(InputValues.CurrentVelocityVector); 69 | *(this->VelocityInputParameters->CurrentAccelerationVector) 70 | = *(InputValues.CurrentAccelerationVector); 71 | *(this->VelocityInputParameters->MaxAccelerationVector) 72 | = *(InputValues.MaxAccelerationVector); 73 | *(this->VelocityInputParameters->MaxJerkVector) 74 | = *(InputValues.MaxJerkVector); 75 | 76 | if (InputsFlags.KeepCurrentVelocityInCaseOfFallbackStrategy) 77 | { 78 | *(this->VelocityInputParameters->TargetVelocityVector) 79 | = *(InputValues.CurrentVelocityVector); 80 | } 81 | else 82 | { 83 | *(this->VelocityInputParameters->TargetVelocityVector) 84 | = *(InputValues.AlternativeTargetVelocityVector); 85 | } 86 | 87 | if (InputsFlags.SynchronizationBehavior == RMLFlags::ONLY_PHASE_SYNCHRONIZATION) 88 | { 89 | this->VelocityFlags.SynchronizationBehavior = RMLFlags::ONLY_PHASE_SYNCHRONIZATION; 90 | } 91 | else 92 | { 93 | this->VelocityFlags.SynchronizationBehavior = RMLFlags::NO_SYNCHRONIZATION; 94 | } 95 | 96 | this->RMLVelocityObject->GetNextStateOfMotion( *(this->VelocityInputParameters) 97 | , this->VelocityOutputParameters 98 | , this->VelocityFlags); 99 | 100 | *(OutputValues->NewPositionVector) 101 | = *(this->VelocityOutputParameters->NewPositionVector); 102 | *(OutputValues->NewVelocityVector) 103 | = *(this->VelocityOutputParameters->NewVelocityVector); 104 | *(OutputValues->NewAccelerationVector) 105 | = *(this->VelocityOutputParameters->NewAccelerationVector); 106 | 107 | OutputValues->SynchronizationTime 108 | = this->VelocityOutputParameters->GetGreatestExecutionTime(); 109 | 110 | OutputValues->TrajectoryIsPhaseSynchronized = false; 111 | 112 | OutputValues->ANewCalculationWasPerformed = true; 113 | 114 | *(OutputValues->MinPosExtremaPositionVectorOnly) 115 | = *(this->VelocityOutputParameters->MinPosExtremaPositionVectorOnly); 116 | *(OutputValues->MaxPosExtremaPositionVectorOnly) 117 | = *(this->VelocityOutputParameters->MaxPosExtremaPositionVectorOnly); 118 | 119 | *(OutputValues->MinExtremaTimesVector) 120 | = *(this->VelocityOutputParameters->MinExtremaTimesVector); 121 | *(OutputValues->MaxExtremaTimesVector) 122 | = *(this->VelocityOutputParameters->MaxExtremaTimesVector); 123 | 124 | for (i = 0; i < this->NumberOfDOFs; i++) 125 | { 126 | *((OutputValues->MinPosExtremaPositionVectorArray)[i]) 127 | = *((this->VelocityOutputParameters->MinPosExtremaPositionVectorArray)[i]); 128 | *((OutputValues->MinPosExtremaVelocityVectorArray)[i]) 129 | = *((this->VelocityOutputParameters->MinPosExtremaVelocityVectorArray)[i]); 130 | *((OutputValues->MinPosExtremaAccelerationVectorArray)[i]) 131 | = *((this->VelocityOutputParameters->MinPosExtremaAccelerationVectorArray)[i]); 132 | 133 | *((OutputValues->MaxPosExtremaPositionVectorArray)[i]) 134 | = *((this->VelocityOutputParameters->MaxPosExtremaPositionVectorArray)[i]); 135 | *((OutputValues->MaxPosExtremaVelocityVectorArray)[i]) 136 | = *((this->VelocityOutputParameters->MaxPosExtremaVelocityVectorArray)[i]); 137 | *((OutputValues->MaxPosExtremaAccelerationVectorArray)[i]) 138 | = *((this->VelocityOutputParameters->MaxPosExtremaAccelerationVectorArray)[i]); 139 | } 140 | } 141 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLPolynomial.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLPolynomial.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for for the class TypeIIRMLMath::TypeIIRMLPolynomial 6 | //! 7 | //! \details 8 | //! Implementation file for the a polynomial class designed 9 | //! for the Type II On-Line Trajectory Generation algorithm. 10 | //! For further information, please refer to the file 11 | //! TypeIIRMLPolynomial.h. 12 | //! 13 | //! \date March 2014 14 | //! 15 | //! \version 1.2.6 16 | //! 17 | //! \author Torsten Kroeger, \n 18 | //! 19 | //! \copyright Copyright (C) 2014 Google, Inc. 20 | //! \n 21 | //! \n 22 | //! GNU Lesser General Public License 23 | //! \n 24 | //! \n 25 | //! This file is part of the Type II Reflexxes Motion Library. 26 | //! \n\n 27 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 28 | //! it and/or modify it under the terms of the GNU Lesser General Public License 29 | //! as published by the Free Software Foundation, either version 3 of the 30 | //! License, or (at your option) any later version. 31 | //! \n\n 32 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 33 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 34 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 35 | //! the GNU Lesser General Public License for more details. 36 | //! \n\n 37 | //! You should have received a copy of the GNU Lesser General Public License 38 | //! along with the Type II Reflexxes Motion Library. If not, see 39 | //! . 40 | // ---------------------------------------------------------- 41 | // For a convenient reading of this file's source code, 42 | // please use a tab width of four characters. 43 | // ---------------------------------------------------------- 44 | 45 | 46 | #include 47 | #include 48 | 49 | 50 | //************************************************************************************ 51 | // Constructor() 52 | 53 | TypeIIRMLMath::TypeIIRMLPolynomial::TypeIIRMLPolynomial() 54 | { 55 | a0 = 0.0; 56 | a1 = 0.0; 57 | a2 = 0.0; 58 | DeltaT = 0.0; 59 | Degree = 0; 60 | } 61 | 62 | 63 | //************************************************************************************ 64 | // Destructor() 65 | 66 | TypeIIRMLMath::TypeIIRMLPolynomial::~TypeIIRMLPolynomial() 67 | {} 68 | 69 | 70 | //************************************************************************************ 71 | // SetCoefficients() 72 | // f(t) = a_2 * (t - DeltaT)^2 + a_1 * (t - DeltaT) + a_0 73 | 74 | void TypeIIRMLMath::TypeIIRMLPolynomial::SetCoefficients( const double &Coeff2 75 | , const double &Coeff1 76 | , const double &Coeff0 77 | , const double &Diff) 78 | { 79 | a0 = Coeff0; 80 | a1 = Coeff1; 81 | a2 = Coeff2; 82 | DeltaT = Diff; 83 | 84 | if (a2 != 0.0) 85 | { 86 | Degree = 2; 87 | return; 88 | } 89 | 90 | if (a1 != 0.0) 91 | { 92 | Degree = 1; 93 | return; 94 | } 95 | 96 | Degree = 0; 97 | return; 98 | } 99 | 100 | 101 | 102 | //************************************************************************************ 103 | // GetCoefficients() 104 | 105 | void TypeIIRMLMath::TypeIIRMLPolynomial::GetCoefficients( double *Coeff2 106 | , double *Coeff1 107 | , double *Coeff0 108 | , double *Diff ) const 109 | { 110 | *Coeff2 = this->a2; 111 | *Coeff1 = this->a1; 112 | *Coeff0 = this->a0; 113 | *Diff = this->DeltaT; 114 | 115 | return; 116 | } 117 | 118 | 119 | 120 | //******************************************************************************************* 121 | // CalculateValue() 122 | // calculates f(t) 123 | 124 | double TypeIIRMLMath::TypeIIRMLPolynomial::CalculateValue(const double &t) const 125 | { 126 | return( ((Degree == 2)? 127 | (a2 * (t - DeltaT) * (t - DeltaT) + a1 * (t - DeltaT) + a0): 128 | ((Degree == 1)? 129 | (a1 * (t - DeltaT) + a0): 130 | (a0)))); 131 | } 132 | 133 | 134 | //******************************************************************************************* 135 | // CalculateRoots() 136 | 137 | void TypeIIRMLMath::TypeIIRMLPolynomial::CalculateRealRoots( unsigned int *NumberOfRoots 138 | , double *Root1 139 | , double *Root2) const 140 | { 141 | if (this->Degree == 2) 142 | { 143 | double b0 = this->a0 / this->a2 144 | , b1 = this->a1 / this->a2 145 | , SquareRootTerm = 0.25 * pow2(b1) - b0; 146 | 147 | if (SquareRootTerm < 0.0) 148 | { 149 | // only complex roots 150 | 151 | *Root1 = 0.0 ; 152 | *Root2 = 0.0 ; 153 | *NumberOfRoots = 0 ; 154 | } 155 | else 156 | { 157 | // Polynomial of degree two: x^2 + b1 x + b2 = 0 158 | 159 | SquareRootTerm = TypeIIRMLMath::RMLSqrt(SquareRootTerm); 160 | 161 | *Root1 = - 0.5 * b1 + SquareRootTerm + this->DeltaT; 162 | *Root2 = - 0.5 * b1 - SquareRootTerm + this->DeltaT; 163 | *NumberOfRoots = 2 ; 164 | } 165 | return; 166 | } 167 | 168 | if (this->Degree == 1) 169 | { 170 | *Root1 = - this->a0 / this->a1 + this->DeltaT; 171 | *Root2 = 0.0 ; 172 | *NumberOfRoots = 1 ; 173 | return; 174 | } 175 | 176 | if (this->Degree == 0) 177 | { 178 | *Root1 = 0.0 ; 179 | *Root2 = 0.0 ; 180 | *NumberOfRoots = 0 ; 181 | } 182 | 183 | return; 184 | } 185 | 186 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLQuicksort.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLQuicksort.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Quicksort algorithm 6 | //! 7 | //! \details 8 | //! Implementation file for the Quicksort algorithm to be used within the 9 | //! library of the Type II On-Line Trajectory Generation algorithm. 10 | //! For further information, please refer to the file TypeIIRMLQuicksort.h. 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | 45 | #include 46 | 47 | 48 | //************************************************************************************ 49 | // Quicksort() 50 | 51 | 52 | void TypeIIRMLMath::Quicksort( const int &LeftBound 53 | , const int &RightBound 54 | , double *ArrayOfValues) 55 | { 56 | int FirstLoopVariable 57 | , SecondLoopVariable; 58 | 59 | double CurrentValue 60 | , HelpVariable; 61 | 62 | CurrentValue = ArrayOfValues[(LeftBound + RightBound) / 2] ; 63 | FirstLoopVariable = LeftBound ; 64 | SecondLoopVariable = RightBound ; 65 | 66 | // try to interexchange elements from the left and from the right 67 | while ( FirstLoopVariable <= SecondLoopVariable ) 68 | { 69 | while( ArrayOfValues[FirstLoopVariable] < CurrentValue ) 70 | { 71 | FirstLoopVariable = FirstLoopVariable + 1; 72 | } 73 | 74 | while ( ArrayOfValues[SecondLoopVariable] > CurrentValue ) 75 | { 76 | SecondLoopVariable = SecondLoopVariable - 1; 77 | } 78 | 79 | if ( FirstLoopVariable <= SecondLoopVariable ) 80 | { 81 | // Interexchange ArrayOfValues[i] and ArrayOfValues[j] 82 | HelpVariable = ArrayOfValues[FirstLoopVariable] ; 83 | ArrayOfValues[FirstLoopVariable] = ArrayOfValues[SecondLoopVariable] ; 84 | ArrayOfValues[SecondLoopVariable] = HelpVariable ; 85 | FirstLoopVariable = FirstLoopVariable + 1 ; 86 | SecondLoopVariable = SecondLoopVariable - 1 ; 87 | } 88 | } 89 | 90 | // Recursive call for both subsections 91 | if ( LeftBound < SecondLoopVariable ) 92 | { 93 | Quicksort( LeftBound 94 | , SecondLoopVariable 95 | , ArrayOfValues ); 96 | } 97 | 98 | if ( FirstLoopVariable < RightBound ) 99 | { 100 | Quicksort( FirstLoopVariable 101 | , RightBound 102 | , ArrayOfValues ); 103 | } 104 | } 105 | 106 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLSetupModifiedSelectionVector.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLSetupModifiedSelectionVector.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLPosition.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | //**************************************************************************** 50 | // SetupModifiedSelectionVector() 51 | 52 | void TypeIIRMLPosition::SetupModifiedSelectionVector(void) 53 | { 54 | unsigned int i = 0; 55 | 56 | *(this->ModifiedSelectionVector) = *(this->CurrentInputParameters->SelectionVector); 57 | 58 | for (i = 0; i < this->NumberOfDOFs; i++) 59 | { 60 | if ((this->CurrentInputParameters->SelectionVector->VecData)[i]) 61 | { 62 | if ( ((this->CurrentInputParameters->TargetVelocityVector->VecData)[i] == 0.0) 63 | && ((this->MinimumExecutionTimes->VecData)[i] <= this->CycleTime)) 64 | { 65 | (this->ModifiedSelectionVector->VecData)[i] = false; 66 | 67 | // This degree of freedom has already reached its target state of motion, which 68 | // is steady, that is, it consists of a zero velocity and zero acceleration value 69 | // right in the target position. To save CPU time (irrelevant for real-time capability) 70 | // and to enable a simple and correct calculation of phase-synchronous trajectories 71 | // the selection vector is modified, and the current state of motion is set to the 72 | // target state of motion. 73 | 74 | (this->CurrentInputParameters->CurrentPositionVector->VecData) [i] 75 | = (this->CurrentInputParameters->TargetPositionVector->VecData) [i]; 76 | (this->CurrentInputParameters->CurrentVelocityVector->VecData) [i] = 0.0; 77 | (this->CurrentInputParameters->CurrentAccelerationVector->VecData) [i] = 0.0; 78 | } 79 | } 80 | } 81 | } 82 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLStep1IntermediateProfiles.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep1IntermediateProfiles.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Step 1 intermediate velocity profiles of 6 | //! the Type II On-Line Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file 10 | //! TypeIIRMLStep1IntermediateProfiles.h. 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | 45 | 46 | #include 47 | 48 | 49 | using namespace TypeIIRMLMath; 50 | 51 | 52 | //**************************************************************************** 53 | // NegateStep1() 54 | 55 | void TypeIIRMLMath::NegateStep1( double *ThisCurrentPosition 56 | , double *ThisCurrentVelocity 57 | , double *ThisTargetPosition 58 | , double *ThisTargetVelocity) 59 | { 60 | *ThisCurrentPosition = -(*ThisCurrentPosition ) ; 61 | *ThisCurrentVelocity = -(*ThisCurrentVelocity ) ; 62 | *ThisTargetPosition = -(*ThisTargetPosition ) ; 63 | *ThisTargetVelocity = -(*ThisTargetVelocity ) ; 64 | 65 | return; 66 | } 67 | 68 | 69 | //**************************************************************************** 70 | // VToVMaxStep1() 71 | 72 | void TypeIIRMLMath::VToVMaxStep1( double *TotalTime 73 | , double *ThisCurrentPosition 74 | , double *ThisCurrentVelocity 75 | , const double &MaxVelocity 76 | , const double &MaxAcceleration) 77 | { 78 | double TimeForCurrentStep = (*ThisCurrentVelocity - MaxVelocity) 79 | / MaxAcceleration; 80 | 81 | *TotalTime += TimeForCurrentStep; 82 | *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity + MaxVelocity) 83 | * TimeForCurrentStep; 84 | *ThisCurrentVelocity = MaxVelocity; 85 | 86 | return; 87 | } 88 | 89 | 90 | //**************************************************************************** 91 | // VToZeroStep1() 92 | 93 | void TypeIIRMLMath::VToZeroStep1( double *TotalTime 94 | , double *ThisCurrentPosition 95 | , double *ThisCurrentVelocity 96 | , const double &MaxAcceleration) 97 | { 98 | double TimeForCurrentStep = *ThisCurrentVelocity / MaxAcceleration; 99 | 100 | *TotalTime += TimeForCurrentStep; 101 | *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity) * TimeForCurrentStep; 102 | *ThisCurrentVelocity = 0.0; 103 | 104 | return; 105 | } 106 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLStep2.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep2.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the method TypeIIRMLPosition::Step2() 6 | //! 7 | //! \details 8 | //! For further information, please refer to the file TypeIIRMLPosition.h. 9 | //! 10 | //! \date March 2014 11 | //! 12 | //! \version 1.2.6 13 | //! 14 | //! \author Torsten Kroeger, \n 15 | //! 16 | //! \copyright Copyright (C) 2014 Google, Inc. 17 | //! \n 18 | //! \n 19 | //! GNU Lesser General Public License 20 | //! \n 21 | //! \n 22 | //! This file is part of the Type II Reflexxes Motion Library. 23 | //! \n\n 24 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 25 | //! it and/or modify it under the terms of the GNU Lesser General Public License 26 | //! as published by the Free Software Foundation, either version 3 of the 27 | //! License, or (at your option) any later version. 28 | //! \n\n 29 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 30 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 31 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 32 | //! the GNU Lesser General Public License for more details. 33 | //! \n\n 34 | //! You should have received a copy of the GNU Lesser General Public License 35 | //! along with the Type II Reflexxes Motion Library. If not, see 36 | //! . 37 | // ---------------------------------------------------------- 38 | // For a convenient reading of this file's source code, 39 | // please use a tab width of four characters. 40 | // ---------------------------------------------------------- 41 | 42 | 43 | #include 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | 51 | using namespace TypeIIRMLMath; 52 | 53 | 54 | //******************************************************************************************* 55 | // Step2 56 | 57 | void TypeIIRMLPosition::Step2(void) 58 | { 59 | unsigned int i = 0; 60 | 61 | if (this->CurrentTrajectoryIsPhaseSynchronized) 62 | { 63 | // As we only calculate the trajectory for one DOF, we do not use multiple threads for the 64 | // Step 2 calculation of the trajectory. 65 | 66 | this->Step2PhaseSynchronization(); 67 | } 68 | else 69 | { 70 | if (this->CurrentTrajectoryIsNotSynchronized) 71 | { 72 | for (i = 0; i < this->NumberOfDOFs; i++) 73 | { 74 | if((this->ModifiedSelectionVector->VecData)[i]) 75 | { 76 | Step2WithoutSynchronization( (this->CurrentInputParameters->CurrentPositionVector->VecData) [i] 77 | , (this->CurrentInputParameters->CurrentVelocityVector->VecData) [i] 78 | , (this->CurrentInputParameters->TargetPositionVector->VecData) [i] 79 | , (this->CurrentInputParameters->TargetVelocityVector->VecData) [i] 80 | , (this->CurrentInputParameters->MaxVelocityVector->VecData) [i] 81 | , (this->CurrentInputParameters->MaxAccelerationVector->VecData) [i] 82 | , (this->UsedStep1AProfiles->VecData) [i] 83 | , (this->MinimumExecutionTimes->VecData) [i] 84 | , &((this->Polynomials)[i]) ); 85 | } 86 | } 87 | } 88 | else 89 | { 90 | for (i = 0; i < this->NumberOfDOFs; i++) 91 | { 92 | if((this->ModifiedSelectionVector->VecData)[i]) 93 | { 94 | TypeIIRMLDecisionTree2( (this->CurrentInputParameters->CurrentPositionVector->VecData) [i] 95 | , (this->CurrentInputParameters->CurrentVelocityVector->VecData) [i] 96 | , (this->CurrentInputParameters->TargetPositionVector->VecData) [i] 97 | , (this->CurrentInputParameters->TargetVelocityVector->VecData) [i] 98 | , (this->CurrentInputParameters->MaxVelocityVector->VecData) [i] 99 | , (this->CurrentInputParameters->MaxAccelerationVector->VecData) [i] 100 | , this->SynchronizationTime 101 | , &((this->Polynomials)[i]) ); 102 | } 103 | } 104 | } 105 | } 106 | 107 | return; 108 | } 109 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLStep2IntermediateProfiles.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep2IntermediateProfiles.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Step 2 intermediate velocity profiles of 6 | //! the Type II On-Line Trajectory Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file 10 | //! TypeIIRMLStep2IntermediateProfiles.h. 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | using namespace TypeIIRMLMath; 50 | 51 | 52 | //**************************************************************************** 53 | // NegateStep2() 54 | 55 | void TypeIIRMLMath::NegateStep2( double *ThisCurrentPosition 56 | , double *ThisCurrentVelocity 57 | , double *ThisTargetPosition 58 | , double *ThisTargetVelocity 59 | , bool *Inverted ) 60 | { 61 | *ThisCurrentPosition = -(*ThisCurrentPosition ) ; 62 | *ThisCurrentVelocity = -(*ThisCurrentVelocity ) ; 63 | *ThisTargetPosition = -(*ThisTargetPosition ) ; 64 | *ThisTargetVelocity = -(*ThisTargetVelocity ) ; 65 | *Inverted = !(*Inverted) ; 66 | 67 | return; 68 | } 69 | 70 | 71 | //**************************************************************************** 72 | // VToVMaxStep2() 73 | 74 | void TypeIIRMLMath::VToVMaxStep2( double *ThisCurrentTime 75 | , double *ThisCurrentPosition 76 | , double *ThisCurrentVelocity 77 | , const double &MaxVelocity 78 | , const double &MaxAcceleration 79 | , MotionPolynomials *PolynomialsLocal 80 | , const bool &Inverted ) 81 | { 82 | double TimeForCurrentStep = (*ThisCurrentVelocity - MaxVelocity) 83 | / MaxAcceleration; 84 | 85 | if(Inverted) 86 | { 87 | PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-(*ThisCurrentVelocity)), (-(*ThisCurrentPosition)), (*ThisCurrentTime)); 88 | PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-(*ThisCurrentVelocity)), (*ThisCurrentTime)); 89 | PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, (*ThisCurrentTime)); 90 | } 91 | else 92 | { 93 | PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (*ThisCurrentVelocity), (*ThisCurrentPosition), (*ThisCurrentTime)); 94 | PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (*ThisCurrentVelocity), (*ThisCurrentTime)); 95 | PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), (*ThisCurrentTime)); 96 | } 97 | 98 | PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = (*ThisCurrentTime) + TimeForCurrentStep; 99 | PolynomialsLocal->ValidPolynomials++; 100 | 101 | //calculate values for the end of the current time interval 102 | *ThisCurrentTime += (TimeForCurrentStep); 103 | *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity + MaxVelocity) 104 | * TimeForCurrentStep; 105 | *ThisCurrentVelocity = MaxVelocity; 106 | 107 | return; 108 | } 109 | 110 | 111 | //**************************************************************************** 112 | // VToZeroStep2() 113 | 114 | void TypeIIRMLMath::VToZeroStep2( double *ThisCurrentTime 115 | , double *ThisCurrentPosition 116 | , double *ThisCurrentVelocity 117 | , const double &MaxAcceleration 118 | , MotionPolynomials *PolynomialsLocal 119 | , const bool &Inverted ) 120 | { 121 | double TimeForCurrentStep = *ThisCurrentVelocity / MaxAcceleration; 122 | 123 | if(Inverted) 124 | { 125 | PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * MaxAcceleration), (-(*ThisCurrentVelocity)), (-(*ThisCurrentPosition)), (*ThisCurrentTime)); 126 | PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, MaxAcceleration , (-(*ThisCurrentVelocity)), (*ThisCurrentTime)); 127 | PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , MaxAcceleration, (*ThisCurrentTime)); 128 | } 129 | else 130 | { 131 | PolynomialsLocal->PositionPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients((0.5 * (-MaxAcceleration)), (*ThisCurrentVelocity), (*ThisCurrentPosition), (*ThisCurrentTime)); 132 | PolynomialsLocal->VelocityPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, (-MaxAcceleration) , (*ThisCurrentVelocity), (*ThisCurrentTime)); 133 | PolynomialsLocal->AccelerationPolynomial[PolynomialsLocal->ValidPolynomials].SetCoefficients(0.0, 0.0 , (-MaxAcceleration), (*ThisCurrentTime)); 134 | } 135 | 136 | PolynomialsLocal->PolynomialTimes[PolynomialsLocal->ValidPolynomials] = (*ThisCurrentTime) + TimeForCurrentStep; 137 | PolynomialsLocal->ValidPolynomials++; 138 | 139 | //calculate values for the end of the current time interval 140 | *ThisCurrentTime += (TimeForCurrentStep); 141 | *ThisCurrentPosition += 0.5 * (*ThisCurrentVelocity) * TimeForCurrentStep; 142 | *ThisCurrentVelocity = 0.0; 143 | 144 | return; 145 | } 146 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLStep2WithoutSynchronization.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep2WithoutSynchronization.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the function 6 | //! TypeIIRMLMath::Step2WithoutSynchronization() 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file 10 | //! TypeIIRMLStep2WithoutSynchronization.h. 11 | //! 12 | //! \date March 2014 13 | //! 14 | //! \version 1.2.6 15 | //! 16 | //! \author Torsten Kroeger, \n 17 | //! 18 | //! \copyright Copyright (C) 2014 Google, Inc. 19 | //! \n 20 | //! \n 21 | //! GNU Lesser General Public License 22 | //! \n 23 | //! \n 24 | //! This file is part of the Type II Reflexxes Motion Library. 25 | //! \n\n 26 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 27 | //! it and/or modify it under the terms of the GNU Lesser General Public License 28 | //! as published by the Free Software Foundation, either version 3 of the 29 | //! License, or (at your option) any later version. 30 | //! \n\n 31 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 32 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 33 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 34 | //! the GNU Lesser General Public License for more details. 35 | //! \n\n 36 | //! You should have received a copy of the GNU Lesser General Public License 37 | //! along with the Type II Reflexxes Motion Library. If not, see 38 | //! . 39 | // ---------------------------------------------------------- 40 | // For a convenient reading of this file's source code, 41 | // please use a tab width of four characters. 42 | // ---------------------------------------------------------- 43 | 44 | 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | #include 52 | 53 | 54 | //**************************************************************************** 55 | // Step2WithoutSynchronization() 56 | 57 | void TypeIIRMLMath::Step2WithoutSynchronization( const double &CurrentPosition 58 | , const double &CurrentVelocity 59 | , const double &TargetPosition 60 | , const double &TargetVelocity 61 | , const double &MaxVelocity 62 | , const double &MaxAcceleration 63 | , const TypeIIRMLMath::Step1_Profile &UsedProfile 64 | , const double &MinimumExecutionTime 65 | , MotionPolynomials *PolynomialsInternal) 66 | { 67 | bool Inverted = false; 68 | 69 | double CurrentTime = 0.0 70 | , ThisCurrentPosition = CurrentPosition 71 | , ThisCurrentVelocity = CurrentVelocity 72 | , ThisTargetPosition = TargetPosition 73 | , ThisTargetVelocity = TargetVelocity ; 74 | 75 | if (!Decision_2___001(ThisCurrentVelocity)) 76 | { 77 | NegateStep2( &ThisCurrentPosition 78 | , &ThisCurrentVelocity 79 | , &ThisTargetPosition 80 | , &ThisTargetVelocity 81 | , &Inverted ); 82 | } 83 | 84 | if (!Decision_2___002( ThisCurrentVelocity 85 | , MaxVelocity )) 86 | { 87 | VToVMaxStep2( &CurrentTime 88 | , &ThisCurrentPosition 89 | , &ThisCurrentVelocity 90 | , MaxVelocity 91 | , MaxAcceleration 92 | , &(*PolynomialsInternal) 93 | , Inverted ); 94 | } 95 | 96 | if ( (UsedProfile == Step1_Profile_NegLinHldPosLin ) 97 | || (UsedProfile == Step1_Profile_NegLinPosLin ) 98 | || (UsedProfile == Step1_Profile_NegTrapPosLin ) 99 | || (UsedProfile == Step1_Profile_NegTriPosLin ) ) 100 | { 101 | VToZeroStep2( &CurrentTime 102 | , &ThisCurrentPosition 103 | , &ThisCurrentVelocity 104 | , MaxAcceleration 105 | , &(*PolynomialsInternal) 106 | , Inverted ); 107 | 108 | NegateStep2( &ThisCurrentPosition 109 | , &ThisCurrentVelocity 110 | , &ThisTargetPosition 111 | , &ThisTargetVelocity 112 | , &Inverted ); 113 | } 114 | 115 | if ( (UsedProfile == Step1_Profile_PosLinHldNegLin ) 116 | || (UsedProfile == Step1_Profile_NegLinHldPosLin ) 117 | || (UsedProfile == Step1_Profile_PosLinNegLin ) 118 | || (UsedProfile == Step1_Profile_NegLinPosLin ) ) 119 | { 120 | ProfileStep2PosLinHldNegLin( CurrentTime 121 | , MinimumExecutionTime 122 | , ThisCurrentPosition 123 | , ThisCurrentVelocity 124 | , ThisTargetPosition 125 | , ThisTargetVelocity 126 | , MaxAcceleration 127 | , &(*PolynomialsInternal) 128 | , Inverted ); 129 | } 130 | else 131 | { 132 | ProfileStep2PosTrapNegLin( CurrentTime 133 | , MinimumExecutionTime 134 | , ThisCurrentPosition 135 | , ThisCurrentVelocity 136 | , ThisTargetPosition 137 | , ThisTargetVelocity 138 | , MaxAcceleration 139 | , &(*PolynomialsInternal) 140 | , Inverted ); 141 | } 142 | 143 | return; 144 | } 145 | 146 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLStep3.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLStep3.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLPosition.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | 50 | 51 | //******************************************************************************************* 52 | // Step3 53 | 54 | int TypeIIRMLPosition::Step3( const double &TimeValueInSeconds 55 | , RMLPositionOutputParameters *OP ) const 56 | { 57 | unsigned int i = 0; 58 | 59 | int j = 0 60 | , ReturnValueForThisMethod = ReflexxesAPI::RML_FINAL_STATE_REACHED; 61 | 62 | for (i = 0; i < this->NumberOfDOFs; i++) 63 | { 64 | if ( (this->ModifiedSelectionVector->VecData)[i] ) 65 | { 66 | j = 0; 67 | 68 | while ( (TimeValueInSeconds > (this->Polynomials)[i].PolynomialTimes[j]) && (j < MAXIMAL_NO_OF_POLYNOMIALS - 1)) 69 | { 70 | j++; 71 | } 72 | 73 | (OP->NewPositionVector->VecData) [i] 74 | = (this->Polynomials)[i].PositionPolynomial[j].CalculateValue(TimeValueInSeconds); 75 | (OP->NewVelocityVector->VecData) [i] 76 | = (this->Polynomials)[i].VelocityPolynomial[j].CalculateValue(TimeValueInSeconds); 77 | (OP->NewAccelerationVector->VecData)[i] 78 | = (this->Polynomials)[i].AccelerationPolynomial[j].CalculateValue(TimeValueInSeconds); 79 | 80 | if ( j < ((this->Polynomials)[i].ValidPolynomials) - 1) 81 | { 82 | ReturnValueForThisMethod = ReflexxesAPI::RML_WORKING; 83 | } 84 | } 85 | else 86 | { 87 | (OP->NewPositionVector->VecData) [i] 88 | = (this->CurrentInputParameters->CurrentPositionVector->VecData)[i]; 89 | (OP->NewVelocityVector->VecData) [i] 90 | = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i]; 91 | (OP->NewAccelerationVector->VecData)[i] 92 | = (this->CurrentInputParameters->CurrentAccelerationVector->VecData)[i]; 93 | } 94 | } 95 | 96 | return(ReturnValueForThisMethod); 97 | } 98 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLVelocityFallBackStrategy.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLVelocityFallBackStrategy.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLVelocity.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | 48 | 49 | using namespace TypeIIRMLMath; 50 | 51 | 52 | //**************************************************************************** 53 | // FallBackStrategy() 54 | 55 | void TypeIIRMLVelocity::FallBackStrategy( const RMLVelocityInputParameters &InputValues 56 | , RMLVelocityOutputParameters *OutputValues ) 57 | { 58 | unsigned int i = 0; 59 | 60 | for (i = 0; i < this->NumberOfDOFs; i++) 61 | { 62 | if (InputValues.SelectionVector->VecData[i]) 63 | { 64 | (OutputValues->NewPositionVector->VecData)[i] 65 | = InputValues.CurrentPositionVector->VecData[i] 66 | + this->CycleTime 67 | * (InputValues.CurrentVelocityVector->VecData)[i]; 68 | 69 | (OutputValues->NewVelocityVector->VecData)[i] 70 | = InputValues.CurrentVelocityVector->VecData[i]; 71 | } 72 | else 73 | { 74 | (OutputValues->NewPositionVector->VecData)[i] 75 | = InputValues.CurrentPositionVector->VecData[i]; 76 | 77 | (OutputValues->NewVelocityVector->VecData)[i] 78 | = InputValues.CurrentVelocityVector->VecData[i]; 79 | } 80 | 81 | OutputValues->ExecutionTimes->VecData[i] = 0.0; 82 | 83 | OutputValues->PositionValuesAtTargetVelocity->VecData[i] 84 | = InputValues.CurrentPositionVector->VecData[i]; 85 | } 86 | 87 | this->SetPositionalExtremsToZero(OutputValues); 88 | 89 | OutputValues->TrajectoryIsPhaseSynchronized = false; 90 | OutputValues->SynchronizationTime = 0.0; 91 | OutputValues->DOFWithTheGreatestExecutionTime = 0; 92 | 93 | return; 94 | } 95 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLVelocityIsPhaseSynchronizationPossible.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLVelocityIsPhaseSynchronizationPossible.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLVelocity.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | #include 49 | #include 50 | #include 51 | 52 | 53 | 54 | 55 | using namespace TypeIIRMLMath; 56 | 57 | 58 | 59 | //**************************************************************************** 60 | // IsPhaseSynchronizationPossible() 61 | 62 | bool TypeIIRMLVelocity::IsPhaseSynchronizationPossible(void) 63 | { 64 | bool Result = true 65 | , SignSwitch = false; 66 | 67 | unsigned int i = 0; 68 | 69 | double LengthOfCurrentVelocityVector = 0.0 70 | , LengthOfTargetVelocityVector = 0.0 71 | , LengthOfReferenceVector = 0.0; 72 | 73 | for (i = 0; i < this->NumberOfDOFs; i++) 74 | { 75 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 76 | { 77 | (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] 78 | = (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i]; 79 | (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] 80 | = (this->CurrentInputParameters->TargetVelocityVector->VecData)[i]; 81 | 82 | LengthOfCurrentVelocityVector += pow2((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]); 83 | LengthOfTargetVelocityVector += pow2((this->PhaseSynchronizationTargetVelocityVector->VecData)[i]); 84 | } 85 | else 86 | { 87 | (this->PhaseSynchronizationCurrentVelocityVector->VecData) [i] = 0.0; 88 | (this->PhaseSynchronizationTargetVelocityVector->VecData) [i] = 0.0; 89 | } 90 | } 91 | 92 | LengthOfCurrentVelocityVector = RMLSqrt(LengthOfCurrentVelocityVector); 93 | LengthOfTargetVelocityVector = RMLSqrt(LengthOfTargetVelocityVector); 94 | 95 | if ( (LengthOfCurrentVelocityVector != POSITIVE_ZERO) && (LengthOfCurrentVelocityVector != 0.0) ) 96 | { 97 | for (i = 0; i < this->NumberOfDOFs; i++) 98 | { 99 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 100 | { 101 | (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] /= LengthOfCurrentVelocityVector; 102 | } 103 | } 104 | } 105 | else 106 | { 107 | for (i = 0; i < this->NumberOfDOFs; i++) 108 | { 109 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 110 | { 111 | (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] = 0.0; 112 | } 113 | } 114 | } 115 | 116 | if ( (LengthOfTargetVelocityVector != POSITIVE_ZERO) && (LengthOfTargetVelocityVector != 0.0) ) 117 | { 118 | for (i = 0; i < this->NumberOfDOFs; i++) 119 | { 120 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 121 | { 122 | (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] /= LengthOfTargetVelocityVector; 123 | } 124 | } 125 | } 126 | else 127 | { 128 | for (i = 0; i < this->NumberOfDOFs; i++) 129 | { 130 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 131 | { 132 | (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] = 0.0; 133 | } 134 | } 135 | } 136 | 137 | // Determine a reference vector. 138 | 139 | LengthOfReferenceVector = ABSOLUTE_PHASE_SYNC_EPSILON; 140 | 141 | if ( ( LengthOfCurrentVelocityVector >= LengthOfTargetVelocityVector ) 142 | && ( LengthOfCurrentVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON )) 143 | { 144 | *(this->PhaseSynchronizationReferenceVector) = *(this->PhaseSynchronizationCurrentVelocityVector); 145 | LengthOfReferenceVector = LengthOfCurrentVelocityVector; 146 | } 147 | 148 | if ( ( LengthOfTargetVelocityVector >= LengthOfCurrentVelocityVector ) 149 | && ( LengthOfTargetVelocityVector >= ABSOLUTE_PHASE_SYNC_EPSILON )) 150 | { 151 | *(this->PhaseSynchronizationReferenceVector) = *(this->PhaseSynchronizationTargetVelocityVector); 152 | LengthOfReferenceVector = LengthOfTargetVelocityVector; 153 | } 154 | 155 | if (LengthOfReferenceVector > ABSOLUTE_PHASE_SYNC_EPSILON) 156 | { 157 | // Switch vector orientations 158 | 159 | SignSwitch = true; 160 | 161 | for (i = 0; i < this->NumberOfDOFs; i++) 162 | { 163 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 164 | { 165 | if ((Sign((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]) 166 | == Sign((this->PhaseSynchronizationReferenceVector->VecData)[i])) 167 | && (fabs((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]) 168 | > ABSOLUTE_PHASE_SYNC_EPSILON)) 169 | { 170 | SignSwitch = false; 171 | break; 172 | } 173 | } 174 | } 175 | 176 | if (SignSwitch) 177 | { 178 | for (i = 0; i < this->NumberOfDOFs; i++) 179 | { 180 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 181 | { 182 | (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i] 183 | = -(this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]; 184 | } 185 | } 186 | } 187 | 188 | SignSwitch = true; 189 | 190 | for (i = 0; i < this->NumberOfDOFs; i++) 191 | { 192 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 193 | { 194 | if ((Sign((this->PhaseSynchronizationTargetVelocityVector->VecData)[i]) 195 | == Sign((this->PhaseSynchronizationReferenceVector->VecData)[i])) 196 | && (fabs((this->PhaseSynchronizationTargetVelocityVector->VecData)[i]) 197 | > ABSOLUTE_PHASE_SYNC_EPSILON)) 198 | { 199 | SignSwitch = false; 200 | break; 201 | } 202 | } 203 | } 204 | 205 | if (SignSwitch) 206 | { 207 | for (i = 0; i < this->NumberOfDOFs; i++) 208 | { 209 | if ((this->PhaseSyncSelectionVector->VecData)[i]) 210 | { 211 | (this->PhaseSynchronizationTargetVelocityVector->VecData)[i] 212 | = -(this->PhaseSynchronizationTargetVelocityVector->VecData)[i]; 213 | } 214 | } 215 | } 216 | 217 | // For the case that all (normalized) are colinear, they now also have the same orientation. 218 | 219 | 220 | for (i = 0; i < this->NumberOfDOFs; i++) 221 | { 222 | if ( (this->PhaseSyncSelectionVector->VecData)[i] ) 223 | { 224 | if ( ((fabs((this->PhaseSynchronizationReferenceVector->VecData)[i] - (this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]) 225 | > ABSOLUTE_PHASE_SYNC_EPSILON ) 226 | && (fabs((this->PhaseSynchronizationCurrentVelocityVector->VecData)[i]) > ABSOLUTE_PHASE_SYNC_EPSILON) ) 227 | || ((fabs((this->PhaseSynchronizationReferenceVector->VecData)[i] - (this->PhaseSynchronizationTargetVelocityVector->VecData)[i]) 228 | > ABSOLUTE_PHASE_SYNC_EPSILON ) 229 | && (fabs((this->PhaseSynchronizationTargetVelocityVector->VecData)[i]) > ABSOLUTE_PHASE_SYNC_EPSILON) ) ) 230 | { 231 | Result = false; 232 | break; 233 | } 234 | } 235 | } 236 | } 237 | else 238 | { 239 | Result = false; 240 | } 241 | 242 | if (!Result) 243 | { 244 | this->PhaseSynchronizationReferenceVector->Set(0.0); 245 | } 246 | 247 | return(Result); 248 | } 249 | -------------------------------------------------------------------------------- /libreflexxestype2/src/TypeIIRML/TypeIIRMLVelocitySetupPhaseSyncSelectionVector.cpp: -------------------------------------------------------------------------------- 1 | // ---------------------- Doxygen info ---------------------- 2 | //! \file TypeIIRMLVelocitySetupPhaseSyncSelectionVector.cpp 3 | //! 4 | //! \brief 5 | //! Implementation file for the Type II On-Line Trajectory 6 | //! Generation algorithm 7 | //! 8 | //! \details 9 | //! For further information, please refer to the file TypeIIRMLVelocity.h. 10 | //! 11 | //! \date March 2014 12 | //! 13 | //! \version 1.2.6 14 | //! 15 | //! \author Torsten Kroeger, \n 16 | //! 17 | //! \copyright Copyright (C) 2014 Google, Inc. 18 | //! \n 19 | //! \n 20 | //! GNU Lesser General Public License 21 | //! \n 22 | //! \n 23 | //! This file is part of the Type II Reflexxes Motion Library. 24 | //! \n\n 25 | //! The Type II Reflexxes Motion Library is free software: you can redistribute 26 | //! it and/or modify it under the terms of the GNU Lesser General Public License 27 | //! as published by the Free Software Foundation, either version 3 of the 28 | //! License, or (at your option) any later version. 29 | //! \n\n 30 | //! The Type II Reflexxes Motion Library is distributed in the hope that it 31 | //! will be useful, but WITHOUT ANY WARRANTY; without even the implied 32 | //! warranty of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See 33 | //! the GNU Lesser General Public License for more details. 34 | //! \n\n 35 | //! You should have received a copy of the GNU Lesser General Public License 36 | //! along with the Type II Reflexxes Motion Library. If not, see 37 | //! . 38 | // ---------------------------------------------------------- 39 | // For a convenient reading of this file's source code, 40 | // please use a tab width of four characters. 41 | // ---------------------------------------------------------- 42 | 43 | 44 | #include 45 | #include 46 | #include 47 | #include 48 | 49 | 50 | using namespace TypeIIRMLMath; 51 | 52 | 53 | 54 | //**************************************************************************** 55 | // SetupPhaseSyncSelectionVector() 56 | 57 | void TypeIIRMLVelocity::SetupPhaseSyncSelectionVector(void) 58 | { 59 | unsigned int i = 0; 60 | 61 | *(this->PhaseSyncSelectionVector) = *(this->CurrentInputParameters->SelectionVector); 62 | 63 | for (i = 0; i < this->NumberOfDOFs; i++) 64 | { 65 | if ( ((this->CurrentInputParameters->SelectionVector->VecData)[i]) 66 | && ( (this->ExecutionTimes->VecData)[i] <= this->CycleTime) 67 | && (IsEpsilonEquality( 0.0 68 | , (this->CurrentInputParameters->CurrentVelocityVector->VecData)[i] 69 | , 0.5 * this->CycleTime * (this->CurrentInputParameters->MaxAccelerationVector->VecData)[i])) 70 | && ((this->CurrentInputParameters->TargetVelocityVector->VecData)[i] == 0.0) ) 71 | { 72 | (this->PhaseSyncSelectionVector->VecData) [i] = false ; 73 | (this->CurrentInputParameters->CurrentVelocityVector->VecData) [i] = 0.0 ; 74 | } 75 | } 76 | } 77 | -------------------------------------------------------------------------------- /reflexxes_wrapper/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(reflexxes_wrapper) 3 | 4 | find_package(ament_cmake REQUIRED) 5 | 6 | include_directories(include) 7 | 8 | install(DIRECTORY include/ DESTINATION include/) 9 | 10 | ament_export_include_directories(include) 11 | 12 | ament_package() 13 | -------------------------------------------------------------------------------- /reflexxes_wrapper/README.txt: -------------------------------------------------------------------------------- 1 | --------------------------------------------------------------- 2 | Example Projects for the Type II Reflexxes Motion Library 3 | --------------------------------------------------------------- 4 | 5 | 6 | *************************************************************** 7 | 1. Directory Contents 8 | *************************************************************** 9 | 10 | - include: Folder for all header files of the Reflexxes API 11 | and the Type II Reflexxes Motion Library 12 | - src: Folder for the source code files of the Type II 13 | Reflexxes Motion Library and the six sample applications 14 | - Linux: Folder with example makefiles for Linux 15 | - MacOS: Folder with example makefiles for Mac OS X 16 | - Windows: Folder with example project files for Microsoft 17 | Windows (Visual Studio 2008 Express) 18 | 19 | 20 | *************************************************************** 21 | 2. Getting Started 22 | *************************************************************** 23 | Change to the directory 'Linux' or 'MacOS', respectively, and 24 | enter 25 | 26 | make clean32 all32 27 | 28 | for 32-bit systems or 29 | 30 | make clean64 all64 31 | 32 | for 64-bit systems, respectively, to check whether all files 33 | compile correctly on your system. Under Microsoft Windows, you 34 | can open the Visual Studio solution file 35 | 'ReflexxesTypeII_ExampleProject.sln' and re-complie all 36 | projects to perform the check on your system. If everything 37 | compiles without error messages, you can take a look at one of 38 | the simple sample applications in the 'src' folder to learn 39 | about the simple and clean Reflexxes API and to use it for 40 | your own applications. In case of problems or issues with this 41 | procedure, please contact us at support@reflexxes.com. 42 | 43 | 44 | *************************************************************** 45 | 3. Documentation 46 | *************************************************************** 47 | 48 | http://www.reflexxes.com/software/typeiirml/v1.2.6 49 | 50 | 51 | *************************************************************** 52 | A. Appendix - Entire Folder Structure 53 | *************************************************************** 54 | 55 | * ReflexxesTypeII 56 | o include Folder for all header files of the Reflexxes API and the source code of the Type II Reflexxes Motion Library 57 | o Linux Folder with example makefiles for Linux 58 | + RMLPositionSampleApplications Folder for the makefile of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 59 | + RMLVelocitySampleApplications Folder for the makefile of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 60 | + TypeIIRML Folder for the makefile of the Type II Reflexxes Motion Library 61 | + x64 Binary files for 64-bit environments 62 | # debug Files with debug information (non-optimized) 63 | * bin Executable files of all sample applications 64 | * lib This folder contains the Reflexxes Type II Motion Library. 65 | * obj Object files 66 | # release Files without debug information (fully optimized) 67 | * bin Executable files of all sample applications 68 | * lib This folder contains the Reflexxes Type II Motion Library. 69 | * obj Object files 70 | + x86 Binary files for 32-bit environments 71 | # debug Files with debug information (non-optimized) 72 | * bin Executable files of all sample applications 73 | * lib This folder contains the Reflexxes Type II Motion Library. 74 | * obj Object files 75 | # release Files without debug information (fully optimized) 76 | * bin Executable files of all sample applications 77 | * lib This folder contains the Reflexxes Type II Motion Library. 78 | * obj Object files 79 | o MacOS Folder with example makefiles for Mac OS X 80 | + RMLPositionSampleApplications Folder for the makefile of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 81 | + RMLVelocitySampleApplications Folder for the makefile of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 82 | + TypeIIRML Folder for the makefile of the Type II Reflexxes Motion Library 83 | + x64 Binary files for 32-bit environments 84 | # debug Files with debug information (non-optimized) 85 | * bin Executable files of all sample applications 86 | * lib This folder contains the Reflexxes Type II Motion Library. 87 | * obj Object files 88 | # release Files without debug information (fully optimized) 89 | * bin Executable files of all sample applications 90 | * lib This folder contains the Reflexxes Type II Motion Library. 91 | * obj Object files 92 | + x86 Binary files for 32-bit environments 93 | # debug Files with debug information (non-optimized) 94 | * bin Executable files of all sample applications 95 | * lib This folder contains the Reflexxes Type II Motion Library. 96 | * obj Object files 97 | # release Files without debug information (fully optimized) 98 | * bin Executable files of all sample applications 99 | * lib This folder contains the Reflexxes Type II Motion Library. 100 | * obj Object files 101 | o src Folder for the source code files of the six sample applications 102 | + RMLPositionSampleApplications Source code of 01_RMLPositionSampleApplication.cpp, 02_RMLPositionSampleApplication.cpp, 03_RMLPositionSampleApplication.cpp, and 07_RMLPositionSampleApplication.cpp 103 | + RMLVelocitySampleApplications Source code of 04_RMLVelocitySampleApplication.cpp, 05_RMLVelocitySampleApplication.cpp, 06_RMLVelocitySampleApplication.cpp, and 08_RMLVelocitySampleApplication.cpp 104 | + TypeIIRML Source code of the Type II Reflexxes Motion Library 105 | o Windows Folder with example project files for Microsoft Windows (Visual Studio 2008 Express) 106 | + Debug Binary files with debug information (non-optimized) 107 | + Release Binary files without debug information (fully optimized) 108 | + 01_RMLPositionSampleApplication Folder for the project file of 01_RMLPositionSampleApplication.cpp 109 | + 02_RMLPositionSampleApplication Folder for the project file of 02_RMLPositionSampleApplication.cpp 110 | + 03_RMLPositionSampleApplication Folder for the project file of 03_RMLPositionSampleApplication.cpp 111 | + 04_RMLVelocitySampleApplication Folder for the project file of 04_RMLVelocitySampleApplication.cpp 112 | + 05_RMLVelocitySampleApplication Folder for the project file of 05_RMLVelocitySampleApplication.cpp 113 | + 06_RMLVelocitySampleApplication Folder for the project file of 06_RMLVelocitySampleApplication.cpp 114 | + 07_RMLPositionSampleApplication Folder for the project file of 07_RMLPositionSampleApplication.cpp 115 | + 08_RMLVelocitySampleApplication Folder for the project file of 08_RMLVelocitySampleApplication.cpp 116 | + TypeIIRML Folder for the project file of the Type II Reflexxes Motion Library 117 | 118 | --------------------------------------------------------------- 119 | Copyright (C) 2014 Google, Inc. -------------------------------------------------------------------------------- /reflexxes_wrapper/package.xml: -------------------------------------------------------------------------------- 1 | 2 | 3 | 4 | reflexxes_wrapper 5 | 0.0.0 6 | Convenience functions for Reflexxes 7 | Andy Zelenak 8 | Proprietary 9 | 10 | ament_cmake 11 | 12 | libreflexxestype2 13 | 14 | 15 | ament_cmake 16 | 17 | 18 | --------------------------------------------------------------------------------