├── .clang-format ├── .gitignore ├── .gitlab-ci.yml ├── .travis.yml ├── CMakeLists.txt ├── Doxyfile ├── LICENSE ├── README.md ├── include └── RTmotion │ ├── algorithm │ ├── math_utils.hpp │ ├── offline_scurve_planner.hpp │ ├── online_scurve_planner.hpp │ ├── planner.hpp │ ├── ruckig_planner.hpp │ ├── scurve_planner.hpp │ ├── time_optimal_trajectory_generation.hpp │ └── trajectory.hpp │ ├── axis.hpp │ ├── fb │ ├── execution_node.hpp │ ├── fb_axis_motion.hpp │ ├── fb_axis_node.hpp │ ├── fb_axis_read.hpp │ ├── fb_base.hpp │ ├── fb_global.hpp │ ├── fb_halt.hpp │ ├── fb_move_absolute.hpp │ ├── fb_move_additive.hpp │ ├── fb_move_relative.hpp │ ├── fb_move_velocity.hpp │ ├── fb_power.hpp │ ├── fb_read_actual_position.hpp │ ├── fb_read_actual_velocity.hpp │ ├── fb_read_axis_error.hpp │ ├── fb_read_command_position.hpp │ ├── fb_read_command_velocity.hpp │ ├── fb_read_status.hpp │ ├── fb_reset.hpp │ └── fb_stop.hpp │ ├── global.hpp │ ├── logging.hpp │ ├── motion_kernel.hpp │ ├── plot │ └── matplotlibcpp.h │ ├── servo.hpp │ └── tool │ └── fb_debug_tool.hpp ├── src ├── CMakeLists.txt ├── algorithm │ ├── README.md │ ├── offline_scurve_planner.cpp │ ├── online_scurve_planner.cpp │ ├── planner.cpp │ ├── ruckig_planner.cpp │ ├── scurve_planner.cpp │ ├── time_optimal_trajectory_generation.cpp │ └── trajectory.cpp ├── axis │ ├── axis.cpp │ └── motion_kernel.cpp ├── demo │ ├── RT_single_axis_move_relative.cpp │ ├── multi-axis.cpp │ └── single_axis_move_relative.cpp ├── fb │ ├── execution_node.cpp │ ├── fb_axis_motion.cpp │ ├── fb_axis_node.cpp │ ├── fb_axis_read.cpp │ ├── fb_base.cpp │ ├── fb_halt.cpp │ ├── fb_move_absolute.cpp │ ├── fb_move_additive.cpp │ ├── fb_move_relative.cpp │ ├── fb_move_velocity.cpp │ ├── fb_power.cpp │ ├── fb_read_actual_position.cpp │ ├── fb_read_actual_velocity.cpp │ ├── fb_read_axis_error.cpp │ ├── fb_read_command_position.cpp │ ├── fb_read_command_velocity.cpp │ ├── fb_read_status.cpp │ ├── fb_reset.cpp │ └── fb_stop.cpp ├── servo │ └── servo.cpp └── tool │ └── fb_debug_tool.cpp └── test ├── CMakeLists.txt ├── function_block_test.cpp ├── get_clock_time_test.cpp ├── offline_scurve_test.cpp ├── online_scurve_test.cpp ├── planner_test.cpp ├── rdtsc_test.cpp └── uranus_test.cpp /.clang-format: -------------------------------------------------------------------------------- 1 | # BSD 3-Clause License 2 | # 3 | # Copyright (c) 2008-2013, Willow Garage, Inc. 4 | # All rights reserved. 5 | # 6 | # Redistribution and use in source and binary forms, with or without 7 | # modification, are permitted provided that the following conditions are met: 8 | # 9 | # * Redistributions of source code must retain the above copyright notice, this 10 | # list of conditions and the following disclaimer. 11 | # 12 | # * Redistributions in binary form must reproduce the above copyright notice, 13 | # this list of conditions and the following disclaimer in the documentation 14 | # and/or other materials provided with the distribution. 15 | # 16 | # * Neither the name of the copyright holder nor the names of its 17 | # contributors may be used to endorse or promote products derived from 18 | # this software without specific prior written permission. 19 | # 20 | # THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" 21 | # AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE 22 | # IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 23 | # DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE 24 | # FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL 25 | # DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR 26 | # SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER 27 | # CAUSED AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, 28 | # OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE 29 | # OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. 30 | 31 | --- 32 | BasedOnStyle: Google 33 | AccessModifierOffset: -2 34 | ConstructorInitializerIndentWidth: 2 35 | AlignEscapedNewlinesLeft: false 36 | AlignTrailingComments: true 37 | AllowAllParametersOfDeclarationOnNextLine: false 38 | AllowShortIfStatementsOnASingleLine: false 39 | AllowShortLoopsOnASingleLine: false 40 | AllowShortFunctionsOnASingleLine: None 41 | AllowShortLoopsOnASingleLine: false 42 | AlwaysBreakTemplateDeclarations: true 43 | AlwaysBreakBeforeMultilineStrings: false 44 | BreakBeforeBinaryOperators: false 45 | BreakBeforeTernaryOperators: false 46 | BreakConstructorInitializersBeforeComma: true 47 | BinPackParameters: true 48 | ColumnLimit: 80 49 | ConstructorInitializerAllOnOneLineOrOnePerLine: true 50 | DerivePointerBinding: false 51 | PointerBindsToType: true 52 | ExperimentalAutoDetectBinPacking: false 53 | IndentCaseLabels: true 54 | MaxEmptyLinesToKeep: 1 55 | NamespaceIndentation: None 56 | ObjCSpaceBeforeProtocolList: true 57 | PenaltyBreakBeforeFirstCallParameter: 19 58 | PenaltyBreakComment: 60 59 | PenaltyBreakString: 100 60 | PenaltyBreakFirstLessLess: 1000 61 | PenaltyExcessCharacter: 1000 62 | PenaltyReturnTypeOnItsOwnLine: 70 63 | SpacesBeforeTrailingComments: 2 64 | Cpp11BracedListStyle: false 65 | Standard: Auto 66 | IndentWidth: 2 67 | TabWidth: 2 68 | UseTab: Never 69 | IndentFunctionDeclarationAfterType: false 70 | SpacesInParentheses: false 71 | SpacesInAngles: false 72 | SpaceInEmptyParentheses: false 73 | SpacesInCStyleCastParentheses: false 74 | SpaceAfterControlStatementKeyword: true 75 | SpaceBeforeAssignmentOperators: true 76 | ContinuationIndentWidth: 4 77 | SortIncludes: false 78 | SpaceAfterCStyleCast: false 79 | 80 | # Configure each individual brace in BraceWrapping 81 | BreakBeforeBraces: Custom 82 | 83 | # Control of individual brace wrapping cases 84 | BraceWrapping: { 85 | AfterClass: 'true' 86 | AfterControlStatement: 'true' 87 | AfterEnum : 'true' 88 | AfterFunction : 'true' 89 | AfterNamespace : 'true' 90 | AfterStruct : 'true' 91 | AfterUnion : 'true' 92 | BeforeCatch : 'true' 93 | BeforeElse : 'true' 94 | IndentBraces : 'false' 95 | } 96 | ... 97 | -------------------------------------------------------------------------------- /.gitignore: -------------------------------------------------------------------------------- 1 | # VScode files 2 | .vscode 3 | 4 | # Build files 5 | build 6 | 7 | # Image files 8 | *.png 9 | 10 | # Generated documentation files 11 | doc 12 | 13 | # Eigen files 14 | *Eigen -------------------------------------------------------------------------------- /.gitlab-ci.yml: -------------------------------------------------------------------------------- 1 | stages: 2 | - build 3 | - deploy 4 | - cleanup 5 | 6 | before_script: 7 | - PLC_HEAD=$(git rev-parse HEAD) 8 | - echo "$PLC_HEAD" 9 | - PLC_URL=$(git remote get-url origin) 10 | - echo $PLC_URL > url.txt 11 | - sed -i "s/gitlab-ci-token/git/1" url.txt 12 | - sed -i "s/git.*@/git@/g" url.txt 13 | - sed -i "s/https/git/1" url.txt 14 | - sed -i "s/com/com:29418/1" url.txt 15 | - PLC_URL=$(cat url.txt) 16 | - echo $PLC_URL 17 | - mkdir -p ${HOME}/.ssh 18 | - eval $(ssh-agent -s) 19 | - ssh-add <(echo "$SSH_PRIVATE_KEY") 20 | - rm -rf ${HOME}/.ssh/known_hosts && touch ${HOME}/.ssh/known_hosts 21 | - ssh-keyscan -p 29418 -t ed25519 'gitlab.devtools.intel.com' >> ${HOME}/.ssh/known_hosts 22 | - cat ${HOME}/.ssh/known_hosts 23 | 24 | # Build environment already set up at /home/gitlab-runner/tgr-build-setup 25 | build-bare-metal: 26 | stage: build 27 | script: 28 | # Pull leatest ECS build setup `tgr-build-setup` 29 | - cd ${HOME}/tgr-build-setup 30 | - git reset --hard 31 | - git pull origin master 32 | # Clear last time change to `meta-plcopen` 33 | - cd ./build/ecs-base-poky/layers/meta-plcopen 34 | - git reset --hard 35 | - cd ./recipes-plcopen/plcopen-motion-control 36 | # Change git source of `plcopen_motion_control` 37 | - number=`sed -n '/SRC_URI/=' ./plcopen-motion-control_internal.inc` 38 | - echo $number 39 | - sed -i "${number}a SRC_URI = \"$PLC_URL;protocol=ssh;destsuffix=git/plcopen_motion_control\"" ./plcopen-motion-control_internal.inc 40 | - sed -i "${number}d" ./plcopen-motion-control_internal.inc 41 | # Change git commit id of `plcopen_motion_control` 42 | - number=`sed -n '/SRCREV/=' ./plcopen-motion-control_internal.inc` 43 | - echo $number 44 | - sed -i "${number}a SRCREV = \"$PLC_HEAD\"" ./plcopen-motion-control_internal.inc 45 | - sed -i "${number}d" ./plcopen-motion-control_internal.inc 46 | # Build image `tgr-image-bare-metal` 47 | - cd ${HOME}/tgr-build-setup/build/ecs-base-poky/poky/ 48 | - source oe-init-build-env ../build/ 49 | - bitbake ecs-image-base-poky 50 | only: 51 | refs: 52 | - master 53 | 54 | # Build environment already set up at /home/gitlab-runner/tgr-build-setup 55 | build-bare-metal-xenomai: 56 | stage: build 57 | script: 58 | # Pull leatest ECS build setup `tgr-build-setup` 59 | - cd ${HOME}/tgr-build-setup 60 | - git reset --hard 61 | - git pull origin master 62 | # Clear last time change to `meta-plcopen` 63 | - cd ./build/ecs-xenomai-poky/layers/meta-plcopen 64 | - git reset --hard 65 | - cd ./recipes-plcopen/plcopen-motion-control 66 | # Change git source of `plcopen_motion_control` 67 | - number=`sed -n '/SRC_URI/=' ./plcopen-motion-control_internal.inc` 68 | - echo $number 69 | - sed -i "${number}a SRC_URI = \"$PLC_URL;protocol=ssh;destsuffix=git/plcopen_motion_control\"" ./plcopen-motion-control_internal.inc 70 | - sed -i "${number}d" ./plcopen-motion-control_internal.inc 71 | # Change git commit id of `plcopen_motion_control` 72 | - number=`sed -n '/SRCREV/=' ./plcopen-motion-control_internal.inc` 73 | - echo $number 74 | - sed -i "${number}a SRCREV = \"$PLC_HEAD\"" ./plcopen-motion-control_internal.inc 75 | - sed -i "${number}d" ./plcopen-motion-control_internal.inc 76 | # Build image `tgr-image-bare-metal-xenomai` 77 | - cd ${HOME}/tgr-build-setup/build/ecs-xenomai-poky/poky/ 78 | - source oe-init-build-env ../build/ 79 | - bitbake ecs-image-xenomai-poky 80 | only: 81 | refs: 82 | - master 83 | 84 | pages: 85 | stage: deploy 86 | before_script: 87 | - sudo apt-get update -qq 88 | - sudo apt-get -qq install doxygen 89 | - sudo apt-get -qq install graphviz 90 | - sudo apt-get -qq install libeigen3-dev 91 | script: 92 | - pwd 93 | - doxygen ./Doxyfile 2>&1 >/dev/null 94 | - mkdir .public 95 | - cp -r build/html/. .public 96 | - mv .public public 97 | - ls public/ 98 | artifacts: 99 | paths: 100 | - public 101 | only: 102 | refs: 103 | - master -------------------------------------------------------------------------------- /.travis.yml: -------------------------------------------------------------------------------- 1 | sudo: required 2 | dist: bionic 3 | services: 4 | - docker 5 | language: cpp 6 | compiler: gcc 7 | 8 | notifications: 9 | email: 10 | recipients: 11 | 12 | addons: 13 | apt: 14 | packages: 15 | - doxygen 16 | 17 | before_install: 18 | - sudo apt-get update -qq 19 | - sudo apt-get -qq install graphviz 20 | - sudo apt-get -qq install libeigen3-dev 21 | - sudo apt-get -qq install python-matplotlib python-numpy python2.7-dev python-tk 22 | - python -c 'import matplotlib.pyplot' 23 | 24 | before_script: 25 | - export CXXFLAGS="-std=c++11" 26 | - cd ~ && git clone https://github.com/google/googletest.git 27 | - cd googletest && mkdir build 28 | - cd build 29 | - cmake .. -DBUILD_SHARED_LIBS=ON 30 | - sudo make install 31 | - echo $PWD 32 | 33 | script: 34 | - cd $TRAVIS_BUILD_DIR 35 | - mkdir build 36 | - cd build 37 | - cmake .. -DTEST=ON 38 | - make 39 | - ./function_block_test 40 | - ./online_scurve_test 41 | - ./offline_scurve_test 42 | - ./planner_test 43 | - cd .. 44 | - rm -rf build 45 | - doxygen ./Doxyfile 46 | 47 | after_success: 48 | - touch build/html/.nojekyll 49 | 50 | deploy: 51 | # Deploy to gh-pages branch 52 | provider: pages 53 | skip-cleanup: true 54 | github-token: $GITHUB_TOKEN 55 | local_dir: ./build/html 56 | on: 57 | branch: master -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.10) 2 | project(RTmotion VERSION 0.1.0 LANGUAGES CXX) 3 | 4 | # Default to C++14 5 | if(NOT CMAKE_CXX_STANDARD) 6 | set(CMAKE_CXX_STANDARD 14) 7 | endif() 8 | 9 | if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") 10 | add_compile_options(-Wall -Wextra -Wpedantic -Wno-unused-parameter -Wno-unused-function -Wno-unused-but-set-variable) 11 | endif() 12 | 13 | # Default options 14 | option(COBALT "Enable compiling with Xenomai" OFF) 15 | option(TEST "Enable unit test" OFF) 16 | option(PLOT "Enable matplotlib c++" OFF) 17 | option(URANUS "Enable test on Uranus code" OFF) 18 | option(TIME "Record time stamps" OFF) 19 | option(DEBUG "Enable debug print" OFF) 20 | 21 | # Default file directory path 22 | set(URANUS_DIR "/usr/local/Uranus" CACHE PATH "Root directory of Uranus.") 23 | set(XENOMAI_DIR "/usr/xenomai" CACHE PATH "Root directory of Xenomai.") 24 | set(XENO_BINDIR "${XENOMAI_DIR}/bin" CACHE PATH "Root directory of Xenomai.") 25 | set(SYSTEM_ROOT "/usr" CACHE PATH "System root directory used to find Python") 26 | 27 | if(TIME) 28 | add_definitions(-DTIME) 29 | add_executable(rdtsc_test test/rdtsc_test.cpp) 30 | add_executable(get_clock_time_test test/get_clock_time_test.cpp) 31 | endif(TIME) 32 | 33 | if(DEBUG) 34 | add_definitions(-DDEBUG) 35 | endif(DEBUG) 36 | 37 | # Set C C++ flags 38 | SET(CMAKE_C_FLAGS "${CMAKE_C_FLAGS} -Wall -O3 -fPIC -ggdb3") 39 | SET(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${CMAKE_C_FLAGS} -std=c++14") 40 | 41 | # Set default install directories 42 | SET(CMAKE_INSTALL_INCLUDEDIR include) 43 | SET(CMAKE_INSTALL_LIBDIR lib) 44 | SET(CMAKE_INSTALL_BINDIR bin) 45 | 46 | # INSTALL_BINDIR is used at command line to define executable output directory 47 | if(NOT INSTALL_BINDIR) 48 | set(INSTALL_BINDIR ${CMAKE_INSTALL_BINDIR}) 49 | endif() 50 | 51 | # Find Eigen3 52 | find_package(Eigen3 REQUIRED) 53 | include_directories( 54 | include 55 | ${EIGEN3_INCLUDE_DIRS} 56 | ) 57 | 58 | find_package(ruckig REQUIRED) 59 | 60 | # Enable matplotlib 61 | if(PLOT) 62 | add_definitions(-DPLOT) 63 | add_definitions(-DWITHOUT_NUMPY) 64 | 65 | find_package(PythonLibs REQUIRED) 66 | message(STATUS "PYTHON_INCLUDE_DIRS: ${PYTHON_INCLUDE_DIRS}") 67 | message(STATUS "PYTHON_LIBRARIES: ${PYTHON_LIBRARIES}") 68 | endif(PLOT) 69 | 70 | # Add Xenomai compile flags 71 | if (COBALT) 72 | execute_process(COMMAND ${XENO_BINDIR}/xeno-config --skin posix --cc OUTPUT_VARIABLE XENO_CC OUTPUT_STRIP_TRAILING_WHITESPACE) 73 | execute_process(COMMAND ${XENO_BINDIR}/xeno-config --skin posix --cflags OUTPUT_VARIABLE XENO_CFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) 74 | execute_process(COMMAND ${XENO_BINDIR}/xeno-config --skin posix --auto-init-solib --ldflags OUTPUT_VARIABLE XENO_LDFLAGS OUTPUT_STRIP_TRAILING_WHITESPACE) 75 | set(CMAKE_CC "${CMAKE_CC} ${XENO_CC}") 76 | set(CMAKE_CXX_FLAGS "${CMAKE_CXX_FLAGS} ${XENO_CFLAGS}") 77 | set(CMAKE_EXE_LINKER_FLAGS "${CMAKE_SHARED_LINKER_FLAGS} ${XENO_LDFLAGS}") 78 | 79 | message(STATUS "XENO_CC: ${XENO_CC}") 80 | message(STATUS "XENO_CFLAGS: ${XENO_CFLAGS}") 81 | message(STATUS "XENO_LDFLAGS: ${XENO_LDFLAGS}") 82 | endif(COBALT) 83 | 84 | add_subdirectory(src) 85 | 86 | if (TEST) 87 | add_subdirectory(test) 88 | endif(TEST) 89 | 90 | install( 91 | DIRECTORY include/${PROJECT_NAME} 92 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} 93 | ) 94 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright 2020 Intel Corporation 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # RTmotion 2 | PLCopen motion library 3 | 4 | ## Documentation 5 | 6 | The documentation can be compiled to `doc` subfolder through the commands: 7 | ```bash 8 | sudo apt install graphviz 9 | 10 | doxygen ./Doxyfile 11 | ``` 12 | 13 | Open `doc/html/index.html` to check the documentation. 14 | 15 | - [S-curve trajectory planning](./src/algorithm/README.md) 16 | 17 | ## Continous Integration 18 | 19 | Travis CI: ![](https://api.travis-ci.com/RoboticsYY/plcopen_motion_control.svg?token=xy7TUUdhEnnR5zpRg3g5&branch=master) 20 | 21 | Gitlab CI: [![pipeline status](https://gitlab.devtools.intel.com/iotg-china-ist/plcopen_motion_control/badges/master/pipeline.svg)](https://gitlab.devtools.intel.com/iotg-china-ist/plcopen_motion_control/-/commits/master) 22 | 23 | ## Install dependency 24 | 25 | ```bash 26 | sudo apt install libeigen3-dev 27 | 28 | # Install ruckig 29 | git clone https://github.com/pantor/ruckig.git 30 | 31 | cd 32 | mkdir -p build && cd build 33 | cmake -DCMAKE_BUILD_TYPE=Release .. 34 | make && sudo make install 35 | ``` 36 | 37 | ## Build 38 | 39 | ```bash 40 | git clone https://gitlab.devtools.intel.com/iotg-china-ist/plcopen_motion_control.git 41 | 42 | cd 43 | 44 | mkdir build && cd build 45 | 46 | # If build on Ubuntu Linux, run the next command 47 | cmake .. 48 | 49 | # If build on PREEMPT Linux, run the next command 50 | cmake .. 51 | 52 | # If build on Xenomai, run the next command 53 | cmake .. -DXENOMAI_DIR=/usr/xenomai/bin/ 54 | 55 | make && sudo make install 56 | 57 | # Try **sudo ldconfig** after installation if meet any problem related to library file missing. 58 | ``` 59 | > Note: Install **googletest** then add `-DTEST=ON -DPLOT=ON -DSYSTEM_ROOT=/usr` at the end of cmake command if need to do Unit Test and plot the result. Check Unit Test below for details. 60 | 61 | ## Run Demo 62 | 63 | ```bash 64 | single_axis_move_relative 65 | ``` 66 | 67 | ## Run Evaluation 68 | 69 | Running the evaluation program using the following commands: 70 | 71 | ```bash 72 | axis_move_cyclic 73 | ``` 74 | 75 | This evaluation program enables a MC_MoveRelative function block running in 1ms real-time cycle. The function block will be re-triggered everytime it finished its task. It can be stopped by `Ctrl+C`. 76 | 77 | ## Unit Test 78 | 79 | - Googletest is used as the test framework. Therefore, if test running is desired, follow the commands below to install `gtest` at first. 80 | ```bash 81 | sudo apt install googletest 82 | ``` 83 | or 84 | 85 | ```bash 86 | export CXXFLAGS="-std=c++11" 87 | cd ~ && git clone https://github.com/google/googletest.git 88 | cd googletest && mkdir build 89 | cd build 90 | cmake .. -DBUILD_SHARED_LIBS=ON 91 | sudo make install 92 | ``` 93 | 94 | In order to build the tests, add the argument `-DTEST=On` to `cmake` commands. 95 | 96 | - To enable the plotting in the test, install the dependencies below. 97 | 98 | ```bash 99 | sudo apt-get install python3-matplotlib python3-numpy python3-dev 100 | ``` 101 | 102 | And add CMake argument `-DPLOT=On`. 103 | 104 | - On-line S-Curve Algorithm Test: 105 | ```bash 106 | /test/online_scurve_test 107 | ``` 108 | 109 | - Trjactory Planner Test: 110 | ```bash 111 | /test/planner_test 112 | ``` 113 | 114 | - Function Block Test: 115 | ```bash 116 | /test/function_block_test 117 | ``` 118 | 119 | ## Link to the library 120 | 121 | Please find all headers files under `/usr/local/include/RTmotion/`. The library file `libRTmotion.so` locates at `/use/local/lib`. 122 | 123 | -------------------------------------------------------------------------------- /include/RTmotion/algorithm/math_utils.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * MathUtils.hpp 3 | * 4 | * Copyright 2020 (C) SYMG(Shanghai) Intelligence System Co.,Ltd 5 | * 6 | * Licensed to the Apache Software Foundation (ASF) under one 7 | * or more contributor license agreements. See the NOTICE file 8 | * distributed with this work for additional information 9 | * regarding copyright ownership. The ASF licenses this file 10 | * to you under the Apache License, Version 2.0 (the 11 | * "License"); you may not use this file except in compliance 12 | * with the License. You may obtain a copy of the License at 13 | * 14 | * http://www.apache.org/licenses/LICENSE-2.0 15 | * 16 | * Unless required by applicable law or agreed to in writing, 17 | * software distributed under the License is distributed on an 18 | * "AS IS" BASIS, WITHOUT WARRANTIES OR CONDITIONS OF ANY 19 | * KIND, either express or implied. See the License for the 20 | * specific language governing permissions and limitations 21 | * under the License. 22 | * 23 | */ 24 | 25 | #ifndef _URANUS_MATHUTILS_HPP_ 26 | #define _URANUS_MATHUTILS_HPP_ 27 | 28 | #include 29 | 30 | namespace Uranus 31 | { 32 | #ifndef __square 33 | #define __square(x) ((x) * (x)) 34 | #endif 35 | 36 | #ifndef __cube 37 | #define __cube(x) (__square(x) * (x)) 38 | #endif 39 | 40 | #ifndef __EPSILON 41 | #define __EPSILON 0.00001 42 | #endif 43 | 44 | #ifndef __iszero 45 | #define __iszero(x) (fabs(x) < __EPSILON) 46 | #endif 47 | 48 | #ifndef __isnotzero 49 | #define __isnotzero(x) (fabs(x) > __EPSILON) 50 | #endif 51 | 52 | #ifndef __iseq 53 | #define __iseq(a, b) __iszero((a) - (b)) 54 | #endif 55 | 56 | #ifndef __isne 57 | #define __isne(a, b) __isnotzero((a) - (b)) 58 | #endif 59 | 60 | #ifndef __isne 61 | #define __isne(a, b) __isnotzero((a) - (b)) 62 | #endif 63 | 64 | #ifndef __isgt 65 | #define __isgt(a, b) ((a) > (b) + __EPSILON) 66 | #endif 67 | 68 | #ifndef __isls 69 | #define __isls(a, b) ((a) < (b)-__EPSILON) 70 | #endif 71 | 72 | inline bool isOpposite(double num1, double num2) 73 | { 74 | if (!num1 || !num2) 75 | return false; 76 | else 77 | return (num1 < 0) != (num2 < 0); 78 | } 79 | 80 | } // namespace Uranus 81 | 82 | #endif /** _URANUS_MATHUTILS_HPP_ **/ -------------------------------------------------------------------------------- /include/RTmotion/algorithm/offline_scurve_planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file offline_scurve_planner.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | using RTmotion::MC_ERROR_CODE; 28 | 29 | namespace trajectory_processing 30 | { 31 | class ScurvePlannerOffLine : public ScurvePlanner 32 | { 33 | public: 34 | ScurvePlannerOffLine() 35 | { 36 | } 37 | ~ScurvePlannerOffLine() = default; 38 | 39 | std::map* time_record_ptr_; 40 | 41 | /** 42 | * @brief Check whether trajectory is feasible. If not return MC_ERROR_CODE 43 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 44 | * a_max, j_max 45 | */ 46 | 47 | MC_ERROR_CODE scurveCheckPosibility(const ScurveCondition& condition); 48 | 49 | /** 50 | * @brief For explanation look at page 79 of 'Trajectory planning for 51 | * automatic machines and robots(2008)' 52 | * @param condition The innput scurve condition, i.e. q0, q1, v0, v1, v_max, 53 | * a_max, j_max 54 | * @param profile The output scurve profile result, i.e. Tj1, Ta, Tj2, Td, Tv 55 | */ 56 | MC_ERROR_CODE computeMaximumSpeedReached(const ScurveCondition& condition, 57 | ScurveProfile& profile); 58 | 59 | /** 60 | * @brief For explanation look at page 79 of 'Trajectory planning for 61 | * automatic machines and robots(2008)' 62 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 63 | * a_max, j_max 64 | * @param profile The output scurve profile result, i.e. Tj1, Ta, Tj2, Td, Tv 65 | */ 66 | MC_ERROR_CODE computeMaximumSpeedNotReached(const ScurveCondition& condition, 67 | ScurveProfile& profile); 68 | 69 | /** 70 | * @brief Trying to achieve requirements with iteratively decreasing maximum 71 | * possible acceleration. Look at 'Trajectory planning for automatic machines 72 | * and robots(2008)' 73 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 74 | * a_max, j_max 75 | * @param profile The ouput scurve profile result, i.e. Tj1, Ta, Tj2, Td, Tv 76 | */ 77 | MC_ERROR_CODE scurveSearchPlanning(const ScurveCondition& condition, 78 | ScurveProfile& profile, double T = 0, 79 | double scale = 0.99, 80 | size_t max_iter = 2000, 81 | double dt_thresh = 0.01, 82 | int timeout = 500); 83 | 84 | /** 85 | * @brief Returns trajectory parameters for a given time, which contains 86 | * acceleration, velocity and position. 87 | * @param t The given timestamp 88 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 89 | * a_max, j_max 90 | * @param profile The input scurve profile, i.e. Tj1, Ta, Tj2, Td, Tv 91 | */ 92 | Eigen::Vector4d getTrajectoryFunc(double t, const ScurveProfile& profile, 93 | const ScurveCondition& condition); 94 | 95 | /** 96 | * @brief Returns trajectory parameters which wraps with sign transforms. 97 | * @param t The given timestamp 98 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 99 | * a_max, j_max 100 | * @param profile The input scurve profile, i.e. Tj1, Ta, Tj2, Td, Tv 101 | */ 102 | Eigen::Vector4d getTrajectoryFunction(double t); 103 | 104 | /** 105 | * @brief Computes s-curve trajectory parameters which are: 106 | * Tj1 --- non-zero constant jerk period while accelerating 107 | * Ta --- total acceleration period time 108 | * Tj2 --- non-zero constant jerk period while decelerating 109 | * Td --- total deceleration time 110 | * Tv --- constant speed time 111 | * @param t The given timestamp 112 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 113 | * a_max, j_max 114 | * @param profile The output scurve profile result, i.e. Tj1, Ta, Tj2, Td, Tv 115 | */ 116 | MC_ERROR_CODE scurveProfileNoOpt(const ScurveCondition& condition, 117 | ScurveProfile& profile); 118 | 119 | /** 120 | * @brief Computes optimal time scurve trajectory or trying to fit it in time 121 | * T. 122 | * @param T Trajectory execution time 123 | */ 124 | MC_ERROR_CODE planTrajectory1D(double T = 0); 125 | 126 | /** 127 | * @brief Computes acceleration phase under conditions. 128 | * @param v Initial velocity or end velocity 129 | * @param v_max Maximum velocity 130 | * @param a_max Maximum acceleration 131 | * @param j_max Maximum jerk 132 | * @param Tj Phase time with accelration from 0 to a_max or a_max to 0 133 | * @param Tad Acceleration phase Tad = 2*Tj if a_max is not reached, Tad > 134 | * 2*Tj if a_max is reached 135 | */ 136 | MC_ERROR_CODE computeAccelerationPhase(double v, double v_max, double a_max, 137 | double j_max, double& Tj, double& Tad); 138 | 139 | virtual MC_ERROR_CODE plan() override 140 | { 141 | profile_.Tv = 0; 142 | profile_.Tj2 = 0; 143 | profile_.Td = 0; 144 | 145 | if (condition_.v1 > condition_.v0) 146 | { 147 | MC_ERROR_CODE res = computeAccelerationPhase( 148 | condition_.v0, condition_.v1, condition_.a_max, condition_.j_max, 149 | profile_.Tj1, profile_.Ta); 150 | return res; 151 | } 152 | else 153 | { 154 | MC_ERROR_CODE res = computeAccelerationPhase( 155 | condition_.v1, condition_.v0, condition_.a_max, condition_.j_max, 156 | profile_.Tj1, profile_.Ta); 157 | condition_.j_max = -condition_.j_max; 158 | return res; 159 | } 160 | } 161 | 162 | virtual Eigen::Vector4d getWaypoint(double t) override 163 | { 164 | return getTrajectoryFunction(t); 165 | } 166 | }; 167 | 168 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/algorithm/online_scurve_planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file online_scurve_planner.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | using RTmotion::MC_ERROR_CODE; 28 | 29 | namespace trajectory_processing 30 | { 31 | typedef enum { 32 | NotYetStarted = 0, 33 | Deceleration = 1, 34 | Acceleration = 2 35 | } StopPhaseMode; 36 | 37 | typedef enum { NoJerk = 0, JerkMin = 1, JerkZero = 2, JerkMax = 3 } StopPhase; 38 | class ScurvePlannerOnLine : public ScurvePlanner 39 | { 40 | public: 41 | ScurvePlannerOnLine() = default; 42 | ~ScurvePlannerOnLine() = default; 43 | 44 | void computeProfile(double jk, double tk, double& ak, double& vk, 45 | double& sk); // Equation (1) 46 | 47 | void computeStopPhaseDccProfile(double& jk, double& hk, double& Tj2a, 48 | double& Tj2b, double& Td); 49 | 50 | void computeStopPhaseAccProfile(double& jk, double& hk, double& Tj2a, 51 | double& Tj2b, double& Td); 52 | 53 | void computeStopPhaseDcc(double t, double& jk, double& ak, double& vk, 54 | double& sk); 55 | 56 | void computeStopPhaseAcc(double t, double& jk, double& ak, double& vk, 57 | double& sk); 58 | 59 | void computeFirstPhaseAcc(double t, double& jk, double& ak, double& vk, 60 | double& sk); // v0 <= Vmax 61 | 62 | void computeFirstPhaseDcc(double t, double& jk, double& ak, double& vk, 63 | double& sk); // v0 > Vmax 64 | 65 | virtual MC_ERROR_CODE plan() override 66 | { 67 | signTransforms(condition_); 68 | return RTmotion::mcErrorCodeGood; 69 | } 70 | 71 | virtual Eigen::Vector4d getWaypoint(double t) override; 72 | 73 | void reset(); 74 | 75 | double sk_, vk_, ak_, jk_, tk_; 76 | double s_init_, v_init_, a_init_, t_init_; 77 | bool triggered_ = false; 78 | StopPhaseMode mode_ = NotYetStarted; 79 | StopPhase phase_ = NoJerk; 80 | }; 81 | 82 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/algorithm/planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file planner.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | 32 | using RTmotion::MC_ERROR_CODE; 33 | 34 | namespace trajectory_processing 35 | { 36 | typedef enum { OFFLine = 0, ONLine = 1, Ruckig = 2} PlannerType; 37 | 38 | class AxisPlanner 39 | { 40 | public: 41 | AxisPlanner(double f) 42 | : ruckig_planner_(f) 43 | { 44 | start_time_ = 0.0; 45 | type_ = Ruckig; 46 | scurve_planner_ = nullptr; 47 | } 48 | 49 | virtual ~AxisPlanner() 50 | { 51 | scurve_planner_ = nullptr; 52 | } 53 | 54 | void setCondition(double start_pos, double end_pos, double start_vel, 55 | double end_vel, double vel_max, double acc_max, 56 | double jerk_max); 57 | 58 | MC_ERROR_CODE planTrajectory(); 59 | 60 | Eigen::Vector4d getTrajectoryPoint(double t); 61 | 62 | MC_ERROR_CODE onReplan(); 63 | 64 | MC_ERROR_CODE onExecution(double t, double* pos_cmd, double* vel_cmd); 65 | 66 | ScurveCondition& getScurveCondition(); 67 | 68 | ScurveProfile& getScurveProfile(); 69 | 70 | void setStartTime(double t); 71 | 72 | private: 73 | ScurvePlanner* scurve_planner_; 74 | ScurvePlannerOnLine online_planner_; 75 | ScurvePlannerOffLine offline_planner_; 76 | RuckigPlanner ruckig_planner_; 77 | double start_time_; 78 | PlannerType type_; 79 | }; 80 | 81 | } // namespace trajectory_processing 82 | -------------------------------------------------------------------------------- /include/RTmotion/algorithm/ruckig_planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file ruckig_planner.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace trajectory_processing 29 | { 30 | 31 | class RuckigPlanner : public ScurvePlanner 32 | { 33 | public: 34 | RuckigPlanner(double f) 35 | : otg_(1/f) 36 | { 37 | } 38 | ~RuckigPlanner() = default; 39 | 40 | virtual RTmotion::MC_ERROR_CODE plan() override 41 | { 42 | signTransforms(condition_); 43 | 44 | // Set input parameters 45 | input_.max_velocity = {condition_.v_max}; 46 | input_.max_acceleration = {condition_.a_max}; 47 | input_.max_jerk = {condition_.j_max}; 48 | 49 | input_.current_position = {condition_.q0}; 50 | input_.current_velocity = {condition_.v0}; 51 | input_.current_acceleration = {condition_.a0}; 52 | 53 | input_.target_position = {condition_.q1}; 54 | input_.target_velocity = {condition_.v1}; 55 | input_.target_acceleration = {condition_.a1}; 56 | 57 | return otg_.validate_input(input_) ? RTmotion::mcErrorCodeGood : RTmotion::mcErrorCode_Scurve_InvalidInput; 58 | } 59 | 60 | virtual Eigen::Vector4d getWaypoint(double t) override; 61 | 62 | ruckig::Ruckig<1> otg_; 63 | ruckig::InputParameter<1> input_; 64 | ruckig::OutputParameter<1> output_; 65 | 66 | }; 67 | 68 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/algorithm/scurve_planner.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file scurve_planner.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | 33 | using RTmotion::MC_ERROR_CODE; 34 | 35 | typedef enum { 36 | POSITION_ID = 0, 37 | SPEED_ID = 1, 38 | ACCELERATION_ID = 2, 39 | JERK_ID = 3 40 | } WaypointIndex; 41 | 42 | namespace trajectory_processing 43 | { 44 | struct ScurveCondition 45 | { 46 | // Condition inputs for planner 47 | double q0 = 0.0; 48 | double q1 = 0.0; 49 | double v0 = 0.0; 50 | double v1 = 0.0; 51 | double a0 = 0.0; 52 | double a1 = 0.0; 53 | double v_max = 0.0; 54 | double a_max = 0.0; 55 | double j_max = 0.0; 56 | }; 57 | 58 | struct ScurveProfile 59 | { 60 | // Time domains for offline scurve planning results 61 | double Tj1 = 0.0; 62 | double Ta = 0.0; 63 | double Tj2 = 0.0; 64 | double Tv = 0.0; 65 | // Time domains for online scurve planning results 66 | double Tj2a = 0.0; 67 | double Tj2c = 0.0; 68 | double Tj2b = 0.0; 69 | double Td = 0.0; 70 | double Th = 0.0; 71 | }; 72 | 73 | class ScurvePlanner 74 | { 75 | public: 76 | ScurvePlanner() = default; 77 | ~ScurvePlanner() = default; 78 | 79 | /** 80 | * @brief Sign transforms for being able to calculate trajectory with q1 < q0. 81 | * Look at 'Trajectory planning for automatic machines and 82 | * robots(2008)' 83 | * @param condition The input scurve condition, i.e. q0, q1, v0, v1, v_max, 84 | * a_max, j_max 85 | */ 86 | void signTransforms(ScurveCondition& condition); 87 | 88 | /** 89 | * @brief Transforms point back to the original sign. 90 | * @param p Reference to Point, i.e. acc, vel, pos 91 | */ 92 | void pointSignTransform(Eigen::Vector4d& p); 93 | 94 | virtual MC_ERROR_CODE plan() = 0; 95 | virtual Eigen::Vector4d getWaypoint(double t) = 0; 96 | 97 | ScurveProfile profile_; 98 | ScurveCondition condition_; 99 | double s_ = 0.0; // Sign transform flag parameter 100 | }; 101 | 102 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/algorithm/time_optimal_trajectory_generation.hpp: -------------------------------------------------------------------------------- 1 | /** 2 | * Copyright (c) 2011-2012, Georgia Tech Research Corporation 3 | * All rights reserved. 4 | * 5 | * Author: Tobias Kunz 6 | * Date: 05/2012 7 | * 8 | * Humanoid Robotics Lab Georgia Institute of Technology 9 | * Director: Mike Stilman http://www.golems.org 10 | * 11 | * Algorithm details and publications: 12 | * http://www.golems.org/node/1570 13 | * 14 | * This file is provided under the following "BSD-style" License: 15 | * Redistribution and use in source and binary forms, with or 16 | * without modification, are permitted provided that the following 17 | * conditions are met: 18 | * * Redistributions of source code must retain the above copyright 19 | * notice, this list of conditions and the following disclaimer. 20 | * * Redistributions in binary form must reproduce the above 21 | * copyright notice, this list of conditions and the following 22 | * disclaimer in the documentation and/or other materials provided 23 | * with the distribution. 24 | * THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND 25 | * CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, 26 | * INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF 27 | * MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE 28 | * DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR 29 | * CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, 30 | * SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT 31 | * LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF 32 | * USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED 33 | * AND ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT 34 | * LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN 35 | * ANY WAY OUT OF THE USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE 36 | * POSSIBILITY OF SUCH DAMAGE. 37 | */ 38 | 39 | #pragma once 40 | 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #include 48 | 49 | namespace trajectory_processing 50 | { 51 | class PathSegment 52 | { 53 | public: 54 | PathSegment(double length = 0.0) : length_(length) 55 | { 56 | } 57 | virtual ~PathSegment() // is required for destructing derived classes 58 | { 59 | } 60 | double getLength() const 61 | { 62 | return length_; 63 | } 64 | virtual Eigen::VectorXd getConfig(double s) const = 0; 65 | virtual Eigen::VectorXd getTangent(double s) const = 0; 66 | virtual Eigen::VectorXd getCurvature(double s) const = 0; 67 | virtual std::list getSwitchingPoints() const = 0; 68 | virtual PathSegment* clone() const = 0; 69 | 70 | double position_; 71 | 72 | protected: 73 | double length_; 74 | }; 75 | 76 | class Path 77 | { 78 | public: 79 | Path(const std::list& path, double max_deviation = 0.0); 80 | Path(const Path& path); 81 | double getLength() const; 82 | Eigen::VectorXd getConfig(double s) const; 83 | Eigen::VectorXd getTangent(double s) const; 84 | Eigen::VectorXd getCurvature(double s) const; 85 | double getNextSwitchingPoint(double s, bool& discontinuity) const; 86 | std::list> getSwitchingPoints() const; 87 | 88 | private: 89 | PathSegment* getPathSegment(double& s) const; 90 | double length_; 91 | std::list> switching_points_; 92 | std::list> path_segments_; 93 | }; 94 | 95 | class Trajectory 96 | { 97 | public: 98 | /// @brief Generates a time-optimal trajectory 99 | Trajectory(const Path& path, const Eigen::VectorXd& max_velocity, 100 | const Eigen::VectorXd& max_acceleration, double time_step = 0.001); 101 | 102 | ~Trajectory(void); 103 | 104 | /** @brief Call this method after constructing the object to make sure the 105 | trajectory generation succeeded without errors. If this method returns 106 | false, all other methods have undefined behavior. **/ 107 | bool isValid() const; 108 | 109 | /// @brief Returns the optimal duration of the trajectory 110 | double getDuration() const; 111 | 112 | /** @brief Return the position/configuration vector for a given point in time 113 | */ 114 | Eigen::VectorXd getPosition(double time) const; 115 | /** @brief Return the velocity vector for a given point in time */ 116 | Eigen::VectorXd getVelocity(double time) const; 117 | /** @brief Return the acceleration vector for a given point in time */ 118 | Eigen::VectorXd getAcceleration(double time) const; 119 | 120 | private: 121 | struct TrajectoryStep 122 | { 123 | TrajectoryStep() 124 | { 125 | } 126 | TrajectoryStep(double path_pos, double path_vel) 127 | : path_pos_(path_pos), path_vel_(path_vel) 128 | { 129 | } 130 | double path_pos_; 131 | double path_vel_; 132 | double time_; 133 | }; 134 | 135 | bool getNextSwitchingPoint(double path_pos, 136 | TrajectoryStep& next_switching_point, 137 | double& before_acceleration, 138 | double& after_acceleration); 139 | bool getNextAccelerationSwitchingPoint(double path_pos, 140 | TrajectoryStep& next_switching_point, 141 | double& before_acceleration, 142 | double& after_acceleration); 143 | bool getNextVelocitySwitchingPoint(double path_pos, 144 | TrajectoryStep& next_switching_point, 145 | double& before_acceleration, 146 | double& after_acceleration); 147 | bool integrateForward(std::list& trajectory, 148 | double acceleration); 149 | void integrateBackward(std::list& start_trajectory, 150 | double path_pos, double path_vel, double acceleration); 151 | double getMinMaxPathAcceleration(double path_position, double path_velocity, 152 | bool max); 153 | double getMinMaxPhaseSlope(double path_position, double path_velocity, 154 | bool max); 155 | double getAccelerationMaxPathVelocity(double path_pos) const; 156 | double getVelocityMaxPathVelocity(double path_pos) const; 157 | double getAccelerationMaxPathVelocityDeriv(double path_pos); 158 | double getVelocityMaxPathVelocityDeriv(double path_pos); 159 | 160 | std::list::const_iterator 161 | getTrajectorySegment(double time) const; 162 | 163 | Path path_; 164 | Eigen::VectorXd max_velocity_; 165 | Eigen::VectorXd max_acceleration_; 166 | unsigned int joint_num_; 167 | bool valid_; 168 | std::list trajectory_; 169 | std::list end_trajectory_; // non-empty only if the 170 | // trajectory generation failed. 171 | 172 | const double time_step_; 173 | 174 | mutable double cached_time_; 175 | mutable std::list::const_iterator cached_trajectory_segment_; 176 | }; 177 | 178 | class TimeOptimalTrajectoryGeneration 179 | { 180 | public: 181 | TimeOptimalTrajectoryGeneration(const double path_tolerance = 0.1, 182 | const double resample_dt = 0.1); 183 | ~TimeOptimalTrajectoryGeneration(); 184 | 185 | bool 186 | computeTimeStamps(RobotTrajectory& trajectory, 187 | const double max_velocity_scaling_factor = 1.0, 188 | const double max_acceleration_scaling_factor = 1.0) const; 189 | 190 | private: 191 | const double path_tolerance_; 192 | const double resample_dt_; 193 | }; 194 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/algorithm/trajectory.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file trajectory.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace trajectory_processing 31 | { 32 | struct VariableBounds 33 | { 34 | double max_velocity_; 35 | double min_velocity_; 36 | double max_acceleration_; 37 | double min_acceleration_; 38 | bool velocity_bounded_; 39 | bool acceleration_bounded_; 40 | }; 41 | 42 | class WayPoint 43 | { 44 | public: 45 | WayPoint() 46 | { 47 | } 48 | ~WayPoint() = default; 49 | 50 | double getVariablePosition(int joint) 51 | { 52 | return position_[joint]; 53 | } 54 | double getVariableVelocity(int joint) 55 | { 56 | return velocity_[joint]; 57 | } 58 | double getVariableAcceleration(int joint) 59 | { 60 | return acceleration_[joint]; 61 | } 62 | void setVariablePosition(int joint, double pos) 63 | { 64 | position_[joint] = pos; 65 | } 66 | void setVariableVelocity(int joint, double vel) 67 | { 68 | velocity_[joint] = vel; 69 | } 70 | void setVariableAcceleration(int joint, double acc) 71 | { 72 | acceleration_[joint] = acc; 73 | } 74 | 75 | private: 76 | std::map position_; 77 | std::map velocity_; 78 | std::map acceleration_; 79 | }; 80 | 81 | class RobotTrajectory 82 | { 83 | public: 84 | RobotTrajectory() 85 | { 86 | } 87 | ~RobotTrajectory() = default; 88 | 89 | bool empty() const 90 | { 91 | return path_.empty(); 92 | } 93 | int getWayPointCount() 94 | { 95 | return path_.size(); 96 | } 97 | WayPoint getWayPoint(size_t index) 98 | { 99 | return path_[index]; 100 | } 101 | double getWayPointTimeStamp(size_t index) 102 | { 103 | return time_stamp_list_[index]; 104 | } 105 | const std::vector& getVariableNames() 106 | { 107 | return joint_names_; 108 | } 109 | const std::vector& getVariableIndexList() 110 | { 111 | return index_list_; 112 | } 113 | unsigned getVariableCount() 114 | { 115 | return joint_names_.size(); 116 | } 117 | VariableBounds getVariableBounds(std::string joint) 118 | { 119 | return variable_bounds_[joint]; 120 | } 121 | void clear() 122 | { 123 | path_.clear(); 124 | time_stamp_list_.clear(); 125 | } 126 | void addSuffixWayPoint(WayPoint waypoint, double time_stamp) 127 | { 128 | path_.push_back(waypoint); 129 | time_stamp_list_.push_back(time_stamp); 130 | } 131 | void addWayPoint(WayPoint waypoint) 132 | { 133 | path_.push_back(waypoint); 134 | } 135 | bool addressTimeStamps() 136 | { 137 | if (path_.size() != time_stamp_list_.size()) 138 | { 139 | printf("Time stamps size don't match waypoints number."); 140 | return false; 141 | } 142 | 143 | for (size_t i = 1; i < time_stamp_list_.size(); i++) 144 | { 145 | time_stamp_list_[i] = time_stamp_list_[i - 1] + time_stamp_list_[i]; 146 | } 147 | 148 | return true; 149 | } 150 | 151 | void setVariableNames(const std::vector& names) 152 | { 153 | joint_names_ = names; 154 | } 155 | void getVariableIndexList(const std::vector& indexes) 156 | { 157 | index_list_ = indexes; 158 | } 159 | void setVariableBounds(const std::map& bounds) 160 | { 161 | variable_bounds_ = bounds; 162 | } 163 | 164 | private: 165 | std::vector path_; 166 | std::vector time_stamp_list_; 167 | std::vector joint_names_; 168 | std::vector index_list_; 169 | std::map variable_bounds_; 170 | }; 171 | 172 | extern std::vector interp_cubic(double t, double T, 173 | std::vector p0_pos, 174 | std::vector p1_pos, 175 | std::vector p0_vel, 176 | std::vector p1_vel); 177 | 178 | extern bool doTraj(std::vector inp_timestamps, 179 | std::vector> inp_positions, 180 | std::vector> inp_velocities, 181 | std::chrono::high_resolution_clock::time_point& t0, 182 | bool& done); 183 | 184 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /include/RTmotion/axis.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file axis.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | namespace RTmotion 31 | { 32 | class FbAxisMotion; 33 | class MotionKernel; 34 | 35 | struct AxisConfig 36 | { 37 | MC_SERVO_CONTROL_MODE mode_ = mcServoControlModePosition; 38 | uint64_t encoder_count_per_unit_ = 1000000; 39 | uint64_t node_buffer_size_ = 100; 40 | bool sw_vel_limit_ = false; 41 | double vel_limit_ = 5000.0; 42 | bool sw_acc_limit_ = false; 43 | double acc_limit_ = 5000.0; 44 | bool sw_range_limit_ = false; 45 | double pos_positive_limit_ = 5000.0; 46 | double pos_negative_limit_ = 5000.0; 47 | double frequency_ = 1000.0; 48 | }; 49 | 50 | class Axis 51 | { 52 | public: 53 | Axis(); 54 | virtual ~Axis(); 55 | 56 | int32_t axisId(); 57 | 58 | std::string axisName(); 59 | 60 | MC_ERROR_CODE setAxisId(int32_t id); 61 | 62 | MC_ERROR_CODE setAxisName(std::string name); 63 | 64 | std::shared_ptr& getServo() 65 | { 66 | return servo_; 67 | }; 68 | 69 | virtual void setServo(std::shared_ptr& servo); 70 | 71 | virtual void setAxisConfig(AxisConfig* config); 72 | 73 | virtual void runCycle(); 74 | 75 | virtual double toUserPos(); 76 | virtual double toUserVel(); 77 | 78 | virtual double toUserPosCmd(); 79 | virtual double toUserVelCmd(); 80 | 81 | virtual double toUserUnit(double x); 82 | virtual int32_t toEncoderUnit(double x); 83 | 84 | virtual void addFBToQueue(FbAxisNode* fb, MC_MOTION_MODE mode); 85 | 86 | virtual BOOL statusHealthy(); 87 | 88 | virtual void powerProcess(); 89 | 90 | virtual bool cmdsProcessing(double frequency); 91 | 92 | virtual void syncMotionKernelResultsToAxis(double duration); 93 | 94 | virtual void updateMotionCmdsToServo(); 95 | 96 | virtual void statusSync(); 97 | 98 | double fixOverFlow(double x); 99 | 100 | MC_AXIS_STATES getAxisState(void); 101 | 102 | MC_ERROR_CODE setAxisState(MC_AXIS_STATES set_state); 103 | 104 | void setPower(bool power_on, bool enable_positive, bool enable_negative); 105 | 106 | void resetError(bool reset); 107 | 108 | bool powerOn(); 109 | bool powerTriggered(); 110 | 111 | MC_ERROR_CODE getAxisError(); 112 | 113 | virtual MC_ERROR_CODE servoErrorToAxisError(MC_ServoErrorCode error_id); 114 | 115 | private: 116 | int32_t axis_id_; 117 | std::string axis_name_; 118 | double axis_pos_; 119 | double axis_vel_; 120 | double axis_acc_; 121 | double axis_jerk_; 122 | double axis_pos_cmd_; 123 | double axis_vel_cmd_; 124 | double axis_tor_cmd_; 125 | double overflow_count_; 126 | MC_AXIS_STATES axis_state_; 127 | std::shared_ptr servo_; 128 | 129 | MotionKernel* motion_kernel_; 130 | 131 | AxisConfig* config_; 132 | 133 | struct timespec time_start_; 134 | struct timespec time_prev_; 135 | 136 | bool power_on_; 137 | bool power_status_; 138 | bool reset_; 139 | bool enable_positive_; 140 | bool enable_negative_; 141 | 142 | bool add_fb_ = false; 143 | uint32_t count_ = 0; 144 | uint32_t add_count_ = 0; 145 | double stamp_ = 0; 146 | 147 | MC_ERROR_CODE axis_error_; 148 | 149 | std::vector node_buffer_; 150 | }; 151 | 152 | typedef std::shared_ptr AXIS_REF; 153 | 154 | } // namespace RTmotion 155 | -------------------------------------------------------------------------------- /include/RTmotion/fb/execution_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file execution_node.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | namespace RTmotion 29 | { 30 | class ExecutionNode 31 | { 32 | public: 33 | 34 | ExecutionNode(double f); 35 | ExecutionNode(FbAxisNode* fb, MC_MOTION_MODE mode, LREAL position, 36 | LREAL velocity, LREAL acceleration, LREAL deceleration, 37 | LREAL jerk, MC_BUFFER_MODE buffer_mode, double f); 38 | virtual ~ExecutionNode() 39 | { 40 | fb_ = nullptr; 41 | } 42 | 43 | void set(FbAxisNode* fb, MC_MOTION_MODE mode, LREAL position, 44 | LREAL velocity, LREAL acceleration, LREAL deceleration, 45 | LREAL jerk, MC_BUFFER_MODE buffer_mode); 46 | 47 | void reset(); 48 | 49 | // Functions to address inputs 50 | void setStartPos(double pos); 51 | void setStartVel(double vel); 52 | void setTerminateCondition(double ref_pos); 53 | void setEndVel(double vel); 54 | void setReplan(); 55 | void setVelocity(LREAL velocity); 56 | 57 | // Functions to address ouputs 58 | MC_BUFFER_MODE getBufferMode(); 59 | MC_MOTION_MODE getMotionMode(); 60 | double getEndPos(); 61 | double getEndVel(); 62 | double getVelocity(); 63 | void getCommands(double* pos_cmd, double* vel_cmd, double duration); 64 | 65 | bool isActive(); 66 | bool isDone(); 67 | bool isAborted(); 68 | bool isError(); 69 | 70 | // Functions for working on different status 71 | virtual void onActive(double t, MC_AXIS_STATES* current_state); 72 | virtual void onExecution(double t); 73 | virtual void onDone(MC_AXIS_STATES* current_state); 74 | virtual void onCommandAborted(); 75 | virtual void onError(MC_ERROR_CODE error_code); 76 | 77 | // Function for checking mission done 78 | virtual bool checkMissionDone(const double axis_pos, const double axis_vel); 79 | 80 | void setPlannerStartTime(double t); 81 | 82 | MC_ERROR_CODE changeAxisStates(MC_AXIS_STATES* current_state, MC_AXIS_STATES set_state); 83 | 84 | FbAxisNode* getAxisNodeInstance(); 85 | 86 | bool taken_ = false; 87 | 88 | protected: 89 | VAR_INPUT LREAL position_; 90 | VAR_INPUT LREAL velocity_; 91 | VAR_INPUT LREAL acceleration_; 92 | VAR_INPUT LREAL deceleration_; 93 | VAR_INPUT LREAL jerk_; 94 | VAR_INPUT MC_BUFFER_MODE buffer_mode_; 95 | 96 | VAR_OUTPUT BOOL done_; 97 | VAR_OUTPUT BOOL busy_; 98 | VAR_OUTPUT BOOL active_; 99 | VAR_OUTPUT BOOL command_aborted_; 100 | VAR_OUTPUT BOOL error_; 101 | VAR_OUTPUT MC_ERROR_CODE error_id_; 102 | 103 | FbAxisNode* fb_; 104 | 105 | // Variables for trajectory planning 106 | double start_pos_; 107 | double end_pos_; 108 | double start_vel_; 109 | double end_vel_; 110 | double pos_cmd_; 111 | double vel_cmd_; 112 | double pos_tmp_; 113 | double vel_tmp_; 114 | bool need_plan_; 115 | MC_MOTION_MODE motion_mode_; 116 | 117 | // Trajectory generator variables 118 | trajectory_processing::AxisPlanner planner_; 119 | }; 120 | 121 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_axis_motion.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_motion.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace RTmotion 31 | { 32 | /** 33 | * @brief Function block base for single axis motion 34 | */ 35 | class FbAxisMotion : public FunctionBlock, public FbAxisNode 36 | { 37 | public: 38 | FbAxisMotion(); 39 | virtual ~FbAxisMotion() = default; 40 | 41 | virtual void runCycle() override; 42 | 43 | // Functions for addressing inputs 44 | virtual void setExecute(BOOL execute); 45 | virtual void setContinuousUpdate(BOOL continuous_update); 46 | virtual void setPosition(LREAL position); 47 | virtual void setVelocity(LREAL); 48 | virtual void setAcceleration(LREAL acceleration); 49 | virtual void setDeceleration(LREAL deceleration); 50 | virtual void setJerk(LREAL jerk); 51 | virtual void setDirection(MC_DIRECTION direction); 52 | virtual void setBufferMode(MC_BUFFER_MODE mode); 53 | 54 | virtual BOOL isError() override; 55 | virtual MC_ERROR_CODE getErrorID() override; 56 | 57 | virtual LREAL getPosition() override; 58 | virtual LREAL getVelocity() override; 59 | virtual LREAL getAcceleration() override; 60 | virtual LREAL getDeceleration() override; 61 | virtual LREAL getJerk() override; 62 | virtual MC_DIRECTION getDirection() override; 63 | virtual MC_BUFFER_MODE getBufferMode() override; 64 | 65 | // Functions for execution 66 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 67 | virtual MC_ERROR_CODE onFallingEdgeExecution() override; 68 | virtual MC_ERROR_CODE onExecution() override; 69 | 70 | protected: 71 | /// Inputs 72 | VAR_INPUT BOOL continuous_update_; 73 | VAR_INPUT LREAL position_; 74 | VAR_INPUT LREAL velocity_; 75 | VAR_INPUT LREAL acceleration_; 76 | VAR_INPUT LREAL deceleration_; 77 | VAR_INPUT LREAL jerk_; 78 | VAR_INPUT MC_DIRECTION direction_; 79 | 80 | /// Flags to control the process 81 | bool triggered_; 82 | }; 83 | } // namespace RTmotion 84 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_axis_node.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_node.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbAxisNode 33 | { 34 | public: 35 | FbAxisNode(); 36 | virtual ~FbAxisNode() = default; 37 | 38 | // Functions for addressing outputs 39 | void syncStatus(BOOL done, BOOL busy, BOOL active, BOOL cmd_aborted, 40 | BOOL error, MC_ERROR_CODE error_id); 41 | 42 | BOOL isEnabled(); 43 | BOOL isDone(); 44 | BOOL isBusy(); 45 | BOOL isActive(); 46 | BOOL isAborted(); 47 | virtual BOOL isError(); 48 | virtual MC_ERROR_CODE getErrorID(); 49 | 50 | virtual LREAL getPosition() = 0; 51 | virtual LREAL getVelocity() = 0; 52 | virtual LREAL getAcceleration() = 0; 53 | virtual LREAL getDeceleration() = 0; 54 | virtual LREAL getJerk() = 0; 55 | virtual MC_DIRECTION getDirection() = 0; 56 | virtual MC_BUFFER_MODE getBufferMode() = 0; 57 | 58 | protected: 59 | VAR_INPUT BOOL execute_; 60 | 61 | /// Outputs 62 | VAR_OUTPUT BOOL done_; 63 | VAR_OUTPUT BOOL busy_; 64 | VAR_OUTPUT BOOL active_; 65 | VAR_OUTPUT BOOL command_aborted_; 66 | VAR_OUTPUT BOOL error_; 67 | VAR_OUTPUT MC_ERROR_CODE error_id_; 68 | VAR_OUTPUT MC_BUFFER_MODE buffer_mode_; 69 | 70 | bool output_flag_; /// Hold the output for another cycle 71 | }; 72 | } // namespace RTmotion 73 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_axis_read.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_read.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbAxisRead : public FunctionBlock 33 | { 34 | public: 35 | FbAxisRead(); 36 | virtual ~FbAxisRead() = default; 37 | 38 | // Functions for addressing inputs 39 | void setExecute(BOOL execute); 40 | 41 | BOOL isEnabled(); 42 | BOOL isValid(); 43 | BOOL isBusy(); 44 | BOOL isError(); 45 | MC_ERROR_CODE getErrorID(); 46 | 47 | virtual LREAL getFloatValue(); 48 | 49 | virtual MC_ERROR_CODE getErrorValue(); 50 | 51 | // Functions for execution 52 | virtual void runCycle() override; 53 | 54 | protected: 55 | /// Inputs 56 | VAR_INPUT BOOL enable_; 57 | 58 | VAR_OUTPUT BOOL valid_; 59 | VAR_OUTPUT BOOL busy_; 60 | VAR_OUTPUT BOOL error_; 61 | VAR_OUTPUT MC_ERROR_CODE error_id_; 62 | }; 63 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_base.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_base.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | #include 30 | 31 | namespace RTmotion 32 | { 33 | class Axis; 34 | typedef std::shared_ptr AXIS_REF; 35 | 36 | class FunctionBlock 37 | { 38 | public: 39 | FunctionBlock(); 40 | virtual ~FunctionBlock() 41 | { 42 | axis_.reset(); 43 | } 44 | 45 | virtual void setAxis(AXIS_REF axis); 46 | 47 | virtual void runCycle() = 0; 48 | 49 | // Functions for working on different status 50 | virtual MC_ERROR_CODE onRisingEdgeExecution(); 51 | 52 | virtual MC_ERROR_CODE onFallingEdgeExecution(); 53 | 54 | virtual MC_ERROR_CODE onExecution(); 55 | 56 | virtual void onError(MC_ERROR_CODE error_code); 57 | 58 | virtual void setTimeRecodPtr(std::map* ptr) 59 | { 60 | time_record_ptr_ = ptr; 61 | }; 62 | 63 | protected: 64 | /// AXIS_REF 65 | VAR_IN_OUT AXIS_REF axis_; 66 | 67 | std::map* time_record_ptr_; 68 | }; 69 | 70 | } // namespace RTmotion 71 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_global.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_global.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | #define VAR_IN_OUT 30 | #define VAR_INPUT 31 | #define VAR_OUTPUT 32 | 33 | // clang-format off 34 | typedef bool BOOL; 35 | typedef uint8_t BYTE; 36 | typedef uint8_t USINT; 37 | typedef int8_t SINT; 38 | typedef uint16_t WORD; 39 | typedef uint16_t UINT; 40 | typedef int16_t INT; 41 | typedef uint32_t DWORD; 42 | typedef uint32_t UDINT; 43 | typedef int32_t DINT; 44 | typedef uint64_t LWORD; 45 | typedef uint64_t ULINT; 46 | typedef int64_t LINT; 47 | typedef float REAL; 48 | typedef double LREAL; 49 | typedef char* STRING; 50 | // clang-format on 51 | 52 | } // namespace RTmotion 53 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_halt.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_halt.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace RTmotion 32 | { 33 | /** 34 | * @brief Function block MC_Halt 35 | */ 36 | class FbHalt : public FbAxisMotion 37 | { 38 | public: 39 | FbHalt(); 40 | ~FbHalt() = default; 41 | 42 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 43 | }; 44 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_move_absolute.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_absolute.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace RTmotion 31 | { 32 | /** 33 | * @brief Function block MC_MoveAbsolute 34 | * \image html MC_MoveRelative.png width=1000cm 35 | * \image html MoveRelative_Example.png width=1000cm 36 | */ 37 | class FbMoveAbsolute : public FbAxisMotion 38 | { 39 | public: 40 | FbMoveAbsolute(); 41 | ~FbMoveAbsolute() = default; 42 | 43 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 44 | }; 45 | } // namespace RTmotion 46 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_move_additive.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_relative.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace RTmotion 31 | { 32 | /** 33 | * @brief Function block MC_MoveAdditive 34 | * \image html MC_MoveAdditive.png width=1000cm 35 | * \image html MoveAdditive_Example.png width=1000cm 36 | */ 37 | class FbMoveAdditive : public FbAxisMotion 38 | { 39 | public: 40 | FbMoveAdditive(); 41 | ~FbMoveAdditive() = default; 42 | 43 | void setDistance(LREAL distance) 44 | { 45 | distance_ = distance; 46 | } 47 | 48 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 49 | 50 | private: 51 | VAR_INPUT LREAL& distance_ = position_; 52 | }; 53 | } // namespace RTmotion 54 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_move_relative.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_relative.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | 28 | #include 29 | 30 | namespace RTmotion 31 | { 32 | /** 33 | * @brief Function block MC_MoveRelative 34 | * \image html MC_MoveRelative.png width=1000cm 35 | * \image html MoveRelative_Example.png width=1000cm 36 | */ 37 | class FbMoveRelative : public FbAxisMotion 38 | { 39 | public: 40 | FbMoveRelative(); 41 | ~FbMoveRelative() = default; 42 | 43 | void setDistance(LREAL distance) 44 | { 45 | distance_ = distance; 46 | } 47 | 48 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 49 | 50 | private: 51 | VAR_INPUT LREAL& distance_ = position_; 52 | }; 53 | } // namespace RTmotion 54 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_move_velocity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_relative.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace RTmotion 32 | { 33 | /** 34 | * @brief Function block MC_MoveVelocity 35 | */ 36 | class FbMoveVelocity : public FbAxisMotion 37 | { 38 | public: 39 | FbMoveVelocity(); 40 | ~FbMoveVelocity() = default; 41 | 42 | BOOL isInVelocity() 43 | { 44 | return in_velocity_; 45 | } 46 | 47 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 48 | 49 | private: 50 | VAR_OUTPUT BOOL& in_velocity_ = done_; 51 | }; 52 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_power.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_power.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbPower : public FunctionBlock 33 | { 34 | public: 35 | FbPower(); 36 | virtual ~FbPower() = default; 37 | 38 | // Functions for addressing inputs 39 | void setExecute(BOOL execute); 40 | void setEnablePositive(BOOL enable_positive); 41 | void setEnableNegative(BOOL enable_negative); 42 | 43 | BOOL isEnabled(); 44 | BOOL getPowerStatus(); 45 | BOOL getPowerStatusValid(); 46 | BOOL isError(); 47 | MC_ERROR_CODE getErrorID(); 48 | 49 | // Functions for execution 50 | virtual void runCycle() override; 51 | 52 | protected: 53 | /// Inputs 54 | VAR_INPUT BOOL enable_; 55 | VAR_INPUT BOOL enable_positive_; 56 | VAR_INPUT BOOL enable_negative_; 57 | }; 58 | } // namespace RTmotion 59 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_actual_position.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_actual_position.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadActualPosition : public FbAxisRead 33 | { 34 | public: 35 | FbReadActualPosition() = default; 36 | virtual ~FbReadActualPosition() = default; 37 | 38 | virtual LREAL getFloatValue() override; 39 | 40 | // Functions for execution 41 | virtual void runCycle() override; 42 | 43 | protected: 44 | 45 | REAL position_; 46 | }; 47 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_actual_velocity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_actual_velocity.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadActualVelocity : public FbAxisRead 33 | { 34 | public: 35 | FbReadActualVelocity() = default; 36 | virtual ~FbReadActualVelocity() = default; 37 | 38 | virtual LREAL getFloatValue() override; 39 | 40 | // Functions for execution 41 | virtual void runCycle() override; 42 | 43 | protected: 44 | 45 | REAL velocity_; 46 | }; 47 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_axis_error.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_axis_error.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadAxisError : public FbAxisRead 33 | { 34 | public: 35 | FbReadAxisError() = default; 36 | virtual ~FbReadAxisError() = default; 37 | 38 | virtual MC_ERROR_CODE getErrorValue() override; 39 | 40 | // Functions for execution 41 | virtual void runCycle() override; 42 | 43 | protected: 44 | MC_ERROR_CODE axis_error_; 45 | }; 46 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_command_position.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_command_position.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadCommandPosition : public FbAxisRead 33 | { 34 | public: 35 | FbReadCommandPosition() = default; 36 | virtual ~FbReadCommandPosition() = default; 37 | 38 | virtual LREAL getFloatValue() override; 39 | 40 | // Functions for execution 41 | virtual void runCycle() override; 42 | 43 | protected: 44 | 45 | REAL position_; 46 | }; 47 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_command_velocity.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_command_velocity.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadCommandVelocity : public FbAxisRead 33 | { 34 | public: 35 | FbReadCommandVelocity() = default; 36 | virtual ~FbReadCommandVelocity() = default; 37 | 38 | virtual LREAL getFloatValue() override; 39 | 40 | // Functions for execution 41 | virtual void runCycle() override; 42 | 43 | protected: 44 | 45 | REAL velocity_; 46 | }; 47 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_read_status.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_status.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReadStatus : public FbAxisRead 33 | { 34 | public: 35 | FbReadStatus() 36 | { 37 | error_stop_ = false; 38 | disabled_ = false; 39 | stopping_ = false; 40 | homing_ = false; 41 | standstill_ = false; 42 | discrete_motion_ = false; 43 | continuous_motion_ = false; 44 | synchronized_motion_ = false; 45 | } 46 | virtual ~FbReadStatus() = default; 47 | 48 | BOOL isErrorStop() {return error_stop_;} 49 | BOOL isDisabled() {return disabled_;} 50 | BOOL isStopping() {return stopping_;} 51 | BOOL isHoming() {return homing_;} 52 | BOOL isStandStill() {return standstill_;} 53 | BOOL isDiscretMotion() {return discrete_motion_;} 54 | BOOL isContinuousMotion() {return continuous_motion_;} 55 | BOOL isSynchronizedMotion() {return synchronized_motion_;} 56 | 57 | // Functions for execution 58 | virtual void runCycle() override; 59 | 60 | protected: 61 | 62 | VAR_OUTPUT BOOL error_stop_; 63 | VAR_OUTPUT BOOL disabled_; 64 | VAR_OUTPUT BOOL stopping_; 65 | VAR_OUTPUT BOOL homing_; 66 | VAR_OUTPUT BOOL standstill_; 67 | VAR_OUTPUT BOOL discrete_motion_; 68 | VAR_OUTPUT BOOL continuous_motion_; 69 | VAR_OUTPUT BOOL synchronized_motion_; 70 | }; 71 | } // namespace RTmotion 72 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_reset.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_reset.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | /** 30 | * @brief Function block base for single axis motion 31 | */ 32 | class FbReset : public FunctionBlock 33 | { 34 | public: 35 | FbReset(); 36 | virtual ~FbReset() = default; 37 | 38 | // Functions for addressing inputs 39 | void setExecute(BOOL execute); 40 | 41 | BOOL isEnabled(); 42 | BOOL isDone(); 43 | BOOL isBusy(); 44 | BOOL isError(); 45 | MC_ERROR_CODE getErrorID(); 46 | 47 | // Functions for execution 48 | virtual void runCycle() override; 49 | 50 | protected: 51 | /// Inputs 52 | VAR_INPUT BOOL enable_; 53 | 54 | VAR_OUTPUT BOOL done_; 55 | VAR_OUTPUT BOOL busy_; 56 | }; 57 | } // namespace RTmotion 58 | -------------------------------------------------------------------------------- /include/RTmotion/fb/fb_stop.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_stop.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | 31 | namespace RTmotion 32 | { 33 | /** 34 | * @brief Function block MC_Stop 35 | */ 36 | class FbStop : public FbAxisMotion 37 | { 38 | public: 39 | FbStop(); 40 | ~FbStop() = default; 41 | 42 | virtual MC_ERROR_CODE onRisingEdgeExecution() override; 43 | 44 | virtual MC_ERROR_CODE onFallingEdgeExecution() override; 45 | }; 46 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/global.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file global.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | #include 29 | 30 | #define __EPSILON 0.01 31 | 32 | #define NSEC_PER_SEC (1000000000L) 33 | #ifndef DIFF_NS 34 | #define DIFF_NS(A, B) (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + ((B).tv_nsec) - (A).tv_nsec) 35 | #endif 36 | #ifdef _WIN32 37 | #include 38 | #else 39 | #include 40 | #endif 41 | 42 | namespace RTmotion 43 | { 44 | /* Servo error code */ 45 | typedef uint32_t MC_ServoErrorCode; 46 | 47 | enum{ 48 | Servo_No_Error = 0, 49 | Servo_Fieldbus_Init_Error = 1, 50 | Servo_Power_Error = 2, 51 | Servo_Powering_On_Error = 3, 52 | Servo_Error_When_Powered_On = 4, 53 | Servo_Powering_Off_Error = 5 54 | }; 55 | 56 | /* Servo control mode */ 57 | typedef enum { 58 | mcServoControlModePosition = 0, 59 | mcServoControlModeVelocity = 1, 60 | mcServoControlModeTorque = 2, 61 | } MC_SERVO_CONTROL_MODE; 62 | 63 | /* MC error code */ 64 | typedef enum { 65 | mcErrorCodeGood = 0x00, // Success 66 | mcErrorCode_Scurve_NotFeasible = 0x10, // Not feasible to compute Scurve 67 | mcErrorCode_Scurve_MaxVelNotReached = 0x21, // Maximum velocity is not reached 68 | mcErrorCode_Scurve_MaxAccNotReached = 0x22, // Maximum acceleration is not reached 69 | mcErrorCode_Scurve_FailToFindMaxAcc = 0x23, // Fail to find max velocity 70 | mcErrorCode_Scurve_InvalidInput = 0x24, // Invalid constraints for Scurve planning 71 | mcErrorCode_AxisStateViolation = 0x30, // General invalid state transition 72 | mcErrorCode_PowerOnOffFromErrorStop = 0x31, // Try to power on at ErrorStop 73 | mcErrorCode_InvalidState_FromStopping = 0x32, // Invalid state transition at Stopping 74 | mcErrorCode_InvalidState_FromErrorStop = 0x33, // Invalid state transition at ErrorStop 75 | mcErrorCode_InvalidState_FromDisabled = 0x34, // Invalid state transition at Disabled 76 | mcErrorCode_Motion_limit_Error = 0x40, // Velocity, Acceleration or Position over Limit 77 | mcErrorCode_Invalid_Direction_Positive = 0x41, // Velocity in forbiden direction 78 | mcErrorCode_Invalid_Direction_Negative = 0x42, // Velocity in forbiden direction 79 | mcErrorCode_Velocity_Over_Limit = 0x43, // Velocity over limit 80 | mcErrorCode_Acceleration_Over_Limit = 0x44, // Acceleration over limit 81 | mcErrorCode_Position_Over_Positive_Limit = 0x45, // Position over positive limit 82 | mcErrorCode_Position_Over_Negative_Limit = 0x46, // Position over negative limit 83 | mcErrorCode_AxisErrorStop = 0x50, // Axis error stopped 84 | mcErrorCode_Servo_No_Error = 0x60, // Servo has no error 85 | mcErrorCode_Servo_Fieldbus_Init_Error = 0x61, // Servo fieldbus initializae error 86 | mcErrorCode_Servo_Power_Error = 0x62, // Servo power error 87 | mcErrorCode_Servo_Powering_On_Error = 0x63, // Servo has error during powering on 88 | mcErrorCode_Servo_Error_When_Powered_On = 0x64, // Servo has error after powered on 89 | mcErrorCode_Servo_Powering_Off_Error = 0x65 // Servo has error during powering off 90 | } MC_ERROR_CODE; 91 | 92 | /** 93 | * Axis states 94 | */ 95 | typedef enum { 96 | /** 97 | * The state ‘Disabled’ describes the initial state of the axis. 98 | * In this state the movement of the axis is not influenced by the FBs. Power 99 | * is off and there is no error 100 | * in the axis. 101 | * If the MC_Power FB is called with ‘Enable’=TRUE while being in ‘Disabled’, 102 | * the state changes to 103 | * ‘Standstill’. The axis feedback is operational before entering the state 104 | * ‘Standstill’. 105 | * Calling MC_Power with ‘Enable’=FALSE in any state except ‘ErrorStop’ 106 | * transfers the axis to the 107 | * state ‘Disabled’, either directly or via any other state. Any on-going 108 | * motion commands on the axis are 109 | * aborted (‘CommandAborted’). 110 | */ 111 | mcDisabled = 0, 112 | 113 | /** 114 | * Power is on, there is no error in the axis, and there are no motion 115 | * commands active on the axis. 116 | */ 117 | mcStandstill = 1, 118 | mcHoming = 2, /// Homing 119 | mcDiscreteMotion = 3, /// DiscreteMotion 120 | mcContinuousMotion = 4, /// ContinuousMotion 121 | mcSynchronizedMotion = 5, /// SynchronizedMotion 122 | mcStopping = 6, /// Stopping 123 | 124 | /** 125 | * ‘ErrorStop’ is valid as highest priority and applicable in case of an 126 | * error. The axis can have either 127 | * power enabled or disabled and can be changed via MC_Power. However, as long 128 | * as the error is pend- 129 | * ing the state remains ‘ErrorStop’. 130 | * The intention of the ‘ErrorStop’ state is that the axis goes to a stop, if 131 | * possible. There is no further 132 | * motion command accepted until a reset has been done from the ‘ErrorStop’ 133 | * state. 134 | * The transition to ‘ErrorStop’ refers to errors from the axis and axis 135 | * control, and not from the Function 136 | * Block instances. These axis errors may also be reflected in the output of 137 | * the Function Blocks ‘FB 138 | * instances errors’. 139 | */ 140 | mcErrorStop = 7, 141 | } MC_AXIS_STATES; 142 | 143 | /* Motion direction */ 144 | typedef enum { 145 | mcPositiveDirection = 1, 146 | mcShortestWay = 2, 147 | mcNegativeDirection = 3, 148 | mcCurrentDirection = 4, 149 | } MC_DIRECTION; 150 | 151 | /* Network error code */ 152 | typedef enum { 153 | mcNetworkGood = 0, 154 | mcNetworkConnectionBreak = 1, 155 | } MC_NETWORK_ERROR_CODE; 156 | 157 | /** 158 | * MC_BUFFER_MODE 159 | * 160 | */ 161 | typedef enum { 162 | /** 163 | * @brief Start FB immediately (default mode). 164 | * The next FB aborts an ongoing motion and the command 165 | * affects the axis immediately. The buffer is cleared. 166 | */ 167 | mcAborting = 0, 168 | /** 169 | * @brief Start FB after current motion has finished. 170 | * The next FB affects the axis as soon as the previous movement is ‘Done’. 171 | * There is no blending. 172 | */ 173 | mcBuffered = 1, 174 | /** 175 | * @brief The velocity is blended with the lowest velocity of both FBs. 176 | * 177 | */ 178 | mcBlendingLow = 179 | 2, /// The velocity is blended with the lowest velocity of both FBs 180 | mcBlendingPrevious = 181 | 3, /// The velocity is blended with the velocity of the first FB 182 | mcBlendingNext = 183 | 4, /// The velocity is blended with velocity of the second FB 184 | mcBlendingHigh = 185 | 5 /// The velocity is blended with highest velocity of both FBs 186 | } MC_BUFFER_MODE; 187 | 188 | typedef enum { 189 | mcNoMoveType = 0, 190 | mcMoveAbsolute = 1, 191 | mcMoveRelative = 2, 192 | mcMoveAdditive = 3, 193 | mcMoveVelocity = 4, 194 | mcHalt = 5, 195 | mcStop = 6 196 | } MC_MOTION_MODE; 197 | 198 | } // namespace RTmotion 199 | -------------------------------------------------------------------------------- /include/RTmotion/logging.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file logging.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | namespace RTmotion 26 | { 27 | #ifdef DEBUG 28 | #define DEBUG_TEST 1 29 | #else 30 | #define DEBUG_TEST 0 31 | #endif 32 | 33 | // clang-format off 34 | #define DEBUG_PRINT(fmt, ...) \ 35 | do { if (DEBUG_TEST) fprintf(stderr, "%s:%d:%s(): \n" fmt, __FILE__, __LINE__, __func__, ##__VA_ARGS__); } while (0) 36 | 37 | #define INFO_PRINT(fmt, ...) \ 38 | do { fprintf(stderr, fmt, ##__VA_ARGS__); } while (0) 39 | // clang-format on 40 | 41 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/motion_kernel.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file motion_kernel.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | #include 27 | #include 28 | 29 | namespace RTmotion 30 | { 31 | class ExecutionNode; 32 | 33 | class MotionKernel 34 | { 35 | public: 36 | MotionKernel(); 37 | 38 | virtual ~MotionKernel(); 39 | 40 | virtual void runCycle(const double t, const double t_prev, const double pos, const double vel, MC_AXIS_STATES* state); 41 | 42 | void addFBToQueue(ExecutionNode* fb, LREAL current_pos, LREAL current_vel); 43 | 44 | std::queue& getQueuedMotions(); 45 | 46 | void getCommands(double* pos_cmd, double* vel_cmd, double duration); 47 | 48 | void setAllFBsAborted(); 49 | 50 | private: 51 | std::queue fb_queue_; 52 | ExecutionNode* fb_hold_ = nullptr; 53 | }; 54 | 55 | } // namespace RTmotion -------------------------------------------------------------------------------- /include/RTmotion/servo.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file servo.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #pragma once 24 | 25 | #include 26 | 27 | namespace RTmotion 28 | { 29 | class Servo 30 | { 31 | public: 32 | Servo(); 33 | virtual ~Servo(); 34 | 35 | virtual MC_ServoErrorCode setPower(bool powerStatus, bool& isDone); 36 | virtual MC_ServoErrorCode setPos(int32_t pos); 37 | virtual MC_ServoErrorCode setVel(int32_t vel); 38 | virtual MC_ServoErrorCode setTorque(double torque); 39 | virtual int32_t pos(void); 40 | virtual int32_t vel(void); 41 | virtual int32_t acc(void); 42 | virtual double torque(void); 43 | virtual bool readVal(int index, double& value); 44 | virtual bool writeVal(int index, double value); 45 | virtual MC_ServoErrorCode resetError(bool& isDone); 46 | virtual void runCycle(double freq); 47 | virtual void emergStop(void); 48 | 49 | private: 50 | class ServoImpl; 51 | ServoImpl* mImpl_; 52 | }; 53 | } // namespace RTmotion 54 | -------------------------------------------------------------------------------- /include/RTmotion/tool/fb_debug_tool.hpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_debug_tool.hpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #ifdef PLOT 30 | #include 31 | namespace plt = matplotlibcpp; 32 | #endif 33 | 34 | std::vector split_string_by_period(std::string name) 35 | { 36 | std::vector tokens; 37 | 38 | std::stringstream ss(name); 39 | 40 | std::string intermediate; 41 | 42 | // Tokenizing w.r.t. period '.' 43 | while (std::getline(ss, intermediate, '.')) 44 | { 45 | tokens.push_back(intermediate); 46 | } 47 | 48 | return tokens; 49 | } 50 | 51 | struct SearchResult 52 | { 53 | bool found; 54 | size_t index; 55 | }; 56 | 57 | struct FB 58 | { 59 | std::string name_; 60 | std::vector digital_names_; 61 | std::vector> digital_states_; 62 | 63 | FB(std::string name) 64 | { 65 | name_ = name; 66 | } 67 | 68 | SearchResult findDigitalItem(std::string item) 69 | { 70 | SearchResult res; 71 | auto it = std::find(digital_names_.begin(), digital_names_.end(), item); 72 | 73 | res.found = (it != digital_names_.end()); 74 | res.index = std::distance(digital_names_.begin(), it); 75 | 76 | return res; 77 | } 78 | }; 79 | 80 | class FBDigitalProfile 81 | { 82 | public: 83 | FBDigitalProfile() = default; 84 | virtual ~FBDigitalProfile() = default; 85 | 86 | SearchResult findFB(std::string name) 87 | { 88 | SearchResult res; 89 | auto it = 90 | std::find_if(fbs_.begin(), fbs_.end(), [name](const FB& item) -> bool { 91 | return item.name_ == name; 92 | }); 93 | 94 | res.found = (it != fbs_.end()); 95 | res.index = std::distance(fbs_.begin(), it); 96 | 97 | return res; 98 | } 99 | 100 | void addFB(std::string name) 101 | { 102 | if (!findFB(name).found) 103 | fbs_.push_back(FB(name)); 104 | } 105 | 106 | bool checkItemName(std::string item, std::string& fb_name, 107 | std::string& var_name) 108 | { 109 | std::vector res = split_string_by_period(item); 110 | 111 | if (res.size() != 2) 112 | { 113 | printf("Invalid item name: '%s', item name should be " 114 | "'.'.\n", 115 | item.c_str()); 116 | return false; 117 | } 118 | 119 | fb_name = res[0]; 120 | var_name = res[1]; 121 | return true; 122 | } 123 | 124 | bool addItemName(std::string item) 125 | { 126 | std::string fb_name, var_name; 127 | if (!checkItemName(item, fb_name, var_name)) 128 | return false; 129 | 130 | SearchResult fb_search = findFB(fb_name); 131 | if (!fb_search.found) // FB not exists 132 | { 133 | printf("Function block '%s' not existing.\n", fb_name.c_str()); 134 | return false; 135 | } 136 | 137 | if (fbs_[fb_search.index].findDigitalItem(var_name).found) // Digital item 138 | // already 139 | // exists 140 | { 141 | printf("Item '%s' already exists.\n", var_name.c_str()); 142 | return false; 143 | } 144 | 145 | fbs_[fb_search.index].digital_names_.push_back(var_name); 146 | fbs_[fb_search.index].digital_states_.push_back(std::vector{}); 147 | printf("Add item '%s' to the debugger of function block '%s'\n", 148 | var_name.c_str(), fb_name.c_str()); 149 | return true; 150 | } 151 | 152 | bool addState(std::string item, int state) 153 | { 154 | std::string fb_name, var_name; 155 | if (!checkItemName(item, fb_name, var_name)) 156 | return false; 157 | 158 | SearchResult fb_search = findFB(fb_name); 159 | if (!fb_search.found) // FB not exists 160 | { 161 | printf("Function block '%s' not existing.", fb_name.c_str()); 162 | return false; 163 | } 164 | 165 | SearchResult var_search = fbs_[fb_search.index].findDigitalItem(var_name); 166 | if (!var_search.found) // Digital item not exists 167 | { 168 | printf("Item '%s' not exists. Please add first.", var_name.c_str()); 169 | return false; 170 | } 171 | 172 | fbs_[fb_search.index].digital_states_[var_search.index].push_back(state); 173 | return true; 174 | } 175 | 176 | void addTime(double time) 177 | { 178 | time_stamps_.push_back(time); 179 | } 180 | 181 | void reset() 182 | { 183 | fbs_.clear(); 184 | time_stamps_.clear(); 185 | } 186 | 187 | size_t getItemCount() 188 | { 189 | size_t n = 0; 190 | for (auto fb : fbs_) 191 | n += fb.digital_names_.size(); 192 | return n; 193 | } 194 | 195 | void plot(std::string file_name = "digital_profile.png", 196 | std::string prefix = "FB", int width = 1280, int height = 1024) 197 | { 198 | #ifdef PLOT 199 | // Set the size of output image to 1280x720 pixels 200 | plt::figure_size(1280, 1024); 201 | 202 | size_t n = getItemCount(); 203 | size_t plot_order = 0; 204 | for (size_t j = 0; j < fbs_.size(); ++j) 205 | { 206 | for (size_t i = 0; i < fbs_[j].digital_names_.size(); ++i) 207 | { 208 | plot_order++; 209 | 210 | plt::subplot(n, 1, plot_order); 211 | if (plot_order == 1) 212 | plt::title("Function Block Profile"); 213 | // Plot line from given time stamp and digital state 214 | plt::named_plot(fbs_[j].digital_names_[i], time_stamps_, 215 | fbs_[j].digital_states_[i], "blue"); 216 | plt::ylabel(prefix + std::to_string(j + 1) + "." + 217 | fbs_[j].digital_names_[i]); 218 | plt::ylim(-0.2, 1.2); 219 | } 220 | plt::xlabel("time"); 221 | } 222 | plt::save(file_name); 223 | #endif 224 | } 225 | 226 | private: 227 | std::vector fbs_; 228 | std::vector time_stamps_; 229 | }; 230 | 231 | class AxisProfile 232 | { 233 | public: 234 | AxisProfile() = default; 235 | virtual ~AxisProfile() = default; 236 | 237 | void addState(double pos, double vel, double time) 238 | { 239 | pos_.push_back(pos); 240 | vel_.push_back(vel); 241 | time_stamps_.push_back(time); 242 | } 243 | 244 | void reset() 245 | { 246 | pos_.clear(); 247 | vel_.clear(); 248 | time_stamps_.clear(); 249 | } 250 | 251 | void plot(std::string file_name = "axis_profile.png") 252 | { 253 | #ifdef PLOT 254 | // Set the size of output image to 1280x360 pixels 255 | plt::figure_size(1280, 360); 256 | // Plot line from given t and pos 257 | plt::subplot(2, 1, 1); 258 | plt::title("Axis Profile"); 259 | plt::named_plot("Position", time_stamps_, pos_, "tab:blue"); 260 | plt::ylabel("Position"); 261 | // Plot line from given t and vel 262 | plt::subplot(2, 1, 2); 263 | plt::named_plot("Velocity", time_stamps_, vel_, "tab:orange"); 264 | plt::ylabel("Velocity"); 265 | plt::xlabel("time"); 266 | // Save the image (file format is determined by the extension) 267 | plt::save(file_name); 268 | #endif 269 | } 270 | 271 | private: 272 | std::vector pos_; 273 | std::vector vel_; 274 | std::vector time_stamps_; 275 | }; -------------------------------------------------------------------------------- /src/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Add source files 2 | aux_source_directory(algorithm SOURCE) 3 | aux_source_directory(axis SOURCE) 4 | aux_source_directory(fb SOURCE) 5 | aux_source_directory(servo SOURCE) 6 | aux_source_directory(tool SOURCE) 7 | 8 | # Create "RTmotion" library 9 | add_library(${PROJECT_NAME} SHARED ${SOURCE}) 10 | set_target_properties(${PROJECT_NAME} PROPERTIES VERSION "${${PROJECT_NAME}_VERSION}") 11 | target_compile_options(${PROJECT_NAME} PRIVATE -Wno-unused-variable) 12 | if(PLOT) 13 | target_include_directories(${PROJECT_NAME} PUBLIC 14 | ${PYTHON_INCLUDE_DIRS} 15 | ) 16 | target_link_libraries(${PROJECT_NAME} ${PYTHON_LIBRARIES} ruckig::ruckig) 17 | else() 18 | target_link_libraries(${PROJECT_NAME} PRIVATE ruckig::ruckig) 19 | endif(PLOT) 20 | 21 | # Create single axis demo executable 22 | add_executable(single_axis_move_relative demo/RT_single_axis_move_relative.cpp) 23 | target_link_libraries(single_axis_move_relative ${PROJECT_NAME} ruckig::ruckig -lpthread) 24 | 25 | add_executable(axis_move_cyclic demo/single_axis_move_relative.cpp) 26 | target_link_libraries(axis_move_cyclic ${PROJECT_NAME} ruckig::ruckig -lpthread) 27 | 28 | add_executable(multi-axis demo/multi-axis.cpp) 29 | target_link_libraries(multi-axis ${PROJECT_NAME} ruckig::ruckig -lpthread) 30 | 31 | install( 32 | TARGETS ${PROJECT_NAME} 33 | LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} 34 | ) 35 | 36 | install( 37 | TARGETS single_axis_move_relative axis_move_cyclic 38 | DESTINATION ${INSTALL_BINDIR} 39 | ) 40 | -------------------------------------------------------------------------------- /src/algorithm/README.md: -------------------------------------------------------------------------------- 1 | # Trajectory Planning 2 | 3 | ## Single Axis Motion Planning 4 | 5 | When driving a motor to an absolute or additve position, the motors always try to accelerate its velocity to its max value and decelerate before it reaches the target position. If the acceleration during this process is not continous or linear piece-wise, some infinite jerk spikes may happen. For this reason, this trajectory may generate efforts and stresses on the mechanical system that may result detrimental or generate undesired 6 | vibrational effects. Therefore, the so called S-Curve trajectory planning is necessary. 7 | 8 | ![](../../image/scurve_profile.png) 9 | 10 | As above figure shows, a complete s-curve process usually contains seven segments with inputs (q0, q1, v0, v1) and constraints (Vmax, Amax, Jmax): 11 | 12 | - Tj1 : time-interval in which the jerk is constant (jmax or jmin) during the acceleration phase; 13 | - Tj2 : time-interval in which the jerk is constant (jmax or jmin) during the deceleration phase; 14 | - Ta : acceleration period; 15 | - Tv : constant velocity period; 16 | - Td : deceleration period; 17 | - T : total duration of the trajectory (= Ta + Tv + Td). 18 | 19 | The s-curve planning module in this project is based on "Trajectory Planning for Automatic Machines and Robots-Springer (2008)" by Luigi Biagiotti, Claudio Melchiorri. 20 | 21 | Some running examples in [test/s_curve_test.cpp](../../test/s_curve_test.cpp): 22 | 23 | - Example 3.9 24 | 25 | * Input: q0 = 0, q1 = 10, v0 = 1, v1 = 0 26 | * Constraints: Vmax = 5, Amax = 10, Jmax = 30 27 | 28 | ![](../../image/Example_3_9.png) 29 | 30 | - Example 3.10 31 | 32 | * Input: q0 = 0, q1 = 10, v0 = 1, v1 = 0 33 | * Constraints: Vmax = 10, Amax = 10, Jmax = 30 34 | 35 | ![](../../image/Example_3_10.png) 36 | 37 | - Example 3.11 38 | 39 | * Input: q0 = 0, q1 = 10, v0 = 7, v1 = 0 40 | * Constraints: Vmax = 10, Amax = 10, Jmax = 30 41 | 42 | ![](../../image/Example_3_11.png) 43 | 44 | - Example 3.12 45 | 46 | * Input: q0 = 0, q1 = 10, v0 = 7.5, v1 = 0 47 | * Constraints: Vmax = 10, Amax = 10, Jmax = 30 48 | 49 | ![](../../image/Example_3_12.png) 50 | 51 | - Example 3.13 52 | 53 | * Input: q0 = 0, q1 = 10, v0 = 0, v1 = 0 54 | * Constraints: Vmax = 10, Amax = 20, Jmax = 30 55 | 56 | ![](../../image/Example_3_13.png) 57 | -------------------------------------------------------------------------------- /src/algorithm/planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file s_curve_planner.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | constexpr double EPSILON = 0.01; 28 | 29 | namespace trajectory_processing 30 | { 31 | void AxisPlanner::setCondition(double start_pos, double end_pos, 32 | double start_vel, double end_vel, double vel_max, 33 | double acc_max, double jerk_max) 34 | { 35 | if (isnan(end_pos)) 36 | { 37 | type_ = OFFLine; 38 | scurve_planner_ = &offline_planner_; 39 | } 40 | else 41 | { 42 | if (type_ == ONLine) 43 | scurve_planner_ = &online_planner_; 44 | 45 | if (type_ == Ruckig) 46 | scurve_planner_ = &ruckig_planner_; 47 | } 48 | 49 | scurve_planner_->condition_.q0 = start_pos; 50 | scurve_planner_->condition_.q1 = end_pos; 51 | scurve_planner_->condition_.v0 = start_vel; 52 | scurve_planner_->condition_.v1 = end_vel; 53 | scurve_planner_->condition_.a0 = 0.0; 54 | scurve_planner_->condition_.a1 = 0.0; 55 | scurve_planner_->condition_.v_max = vel_max; 56 | scurve_planner_->condition_.a_max = acc_max; 57 | scurve_planner_->condition_.j_max = jerk_max; 58 | DEBUG_PRINT( 59 | "AxisPlanner::setCondition:Scurve condition: q0 = %f, q1 = %f, v0 = %f, v1 = %f \n", 60 | scurve_planner_->condition_.q0, scurve_planner_->condition_.q1, 61 | scurve_planner_->condition_.v0, scurve_planner_->condition_.v1); 62 | } 63 | 64 | MC_ERROR_CODE AxisPlanner::planTrajectory() 65 | { 66 | return scurve_planner_->plan(); 67 | } 68 | 69 | Eigen::Vector4d AxisPlanner::getTrajectoryPoint(double t) 70 | { 71 | return scurve_planner_->getWaypoint(t); 72 | } 73 | 74 | MC_ERROR_CODE AxisPlanner::onReplan() 75 | { 76 | MC_ERROR_CODE res = planTrajectory(); 77 | 78 | DEBUG_PRINT( 79 | "AxisPlanner::onReplan:Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 80 | scurve_planner_->profile_.Ta, scurve_planner_->profile_.Tv, scurve_planner_->profile_.Td, 81 | scurve_planner_->profile_.Tj1, scurve_planner_->profile_.Tj2); 82 | return res; 83 | } 84 | 85 | MC_ERROR_CODE AxisPlanner::onExecution(double t, double* pos_cmd, 86 | double* vel_cmd) 87 | { 88 | double d = t - start_time_; 89 | Eigen::Vector4d point = getTrajectoryPoint(d); 90 | *pos_cmd = point[POSITION_ID]; 91 | *vel_cmd = point[SPEED_ID]; 92 | 93 | DEBUG_PRINT("AxisPlanner::onExecution:time: %f, pos_cmd: %f, vel_cmd: %f\n", d, point[POSITION_ID], 94 | point[SPEED_ID]); 95 | return RTmotion::mcErrorCodeGood; 96 | } 97 | 98 | ScurveCondition& AxisPlanner::getScurveCondition() 99 | { 100 | return scurve_planner_->condition_; 101 | } 102 | 103 | ScurveProfile& AxisPlanner::getScurveProfile() 104 | { 105 | return scurve_planner_->profile_; 106 | } 107 | 108 | void AxisPlanner::setStartTime(double t) 109 | { 110 | start_time_ = t; 111 | } 112 | 113 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /src/algorithm/ruckig_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file ruckig_planner.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | using namespace RTmotion; 28 | 29 | constexpr double EPSILON = 0.01; 30 | 31 | namespace trajectory_processing 32 | { 33 | Eigen::Vector4d RuckigPlanner::getWaypoint(double t) 34 | { 35 | otg_.update(input_, output_); 36 | Eigen::Vector4d point; 37 | point[POSITION_ID] = output_.new_position[0]; 38 | point[SPEED_ID] = output_.new_velocity[0]; 39 | point[ACCELERATION_ID] = output_.new_acceleration[0]; 40 | point[JERK_ID] = (output_.new_acceleration[0] - input_.current_acceleration[0]) * t; 41 | 42 | input_.current_position = output_.new_position; 43 | input_.current_velocity = output_.new_velocity; 44 | input_.current_acceleration = output_.new_acceleration; 45 | 46 | // printf("Debug t %f, sk %f, vk %f, ak %f, jk %f.\n", output_.time, point[POSITION_ID], point[SPEED_ID], point[ACCELERATION_ID], point[JERK_ID]); 47 | 48 | pointSignTransform(point); 49 | 50 | return point; 51 | } 52 | } // namespace trajectory_processing 53 | -------------------------------------------------------------------------------- /src/algorithm/scurve_planner.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file scurve_planner.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | using namespace RTmotion; 26 | 27 | constexpr double EPSILON = 0.01; 28 | 29 | namespace trajectory_processing 30 | { 31 | /** 32 | * @brief Get the sign of the number 33 | * @param num The input number 34 | * @return Retrun 1 if num > 0, 0 if num = 0, -1 if num < 0 35 | */ 36 | double sign(double num) 37 | { 38 | double s = 0; 39 | s = (num) > 0 ? 1.0 : s; 40 | s = (num) < 0 ? -1.0 : s; 41 | return s; 42 | } 43 | 44 | void ScurvePlanner::signTransforms(ScurveCondition& condition) 45 | { 46 | // Init condition variables for concise code 47 | double q0 = condition.q0; 48 | double q1 = condition.q1; 49 | double v0 = condition.v0; 50 | double v1 = condition.v1; 51 | double v_max = condition.v_max; 52 | double a_max = condition.a_max; 53 | double j_max = condition.j_max; 54 | 55 | double v_min = -v_max; 56 | double a_min = -a_max; 57 | double j_min = -j_max; 58 | 59 | s_ = isnan(q1 - q0) ? 1.0 : sign(q1 - q0); 60 | double vs1 = (s_ + 1) / 2; 61 | double vs2 = (s_ - 1) / 2; 62 | 63 | condition.q0 = s_ * q0; 64 | condition.q1 = s_ * q1; 65 | condition.v0 = s_ * v0; 66 | condition.v1 = s_ * v1; 67 | condition.v_max = vs1 * v_max + vs2 * v_min; 68 | condition.a_max = vs1 * a_max + vs2 * a_min; 69 | condition.j_max = vs1 * j_max + vs2 * j_min; 70 | } 71 | 72 | void ScurvePlanner::pointSignTransform(Eigen::Vector4d& p) 73 | { 74 | p = s_ * p; 75 | } 76 | } // namespace trajectory_processing 77 | -------------------------------------------------------------------------------- /src/algorithm/trajectory.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file trajectory.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | namespace trajectory_processing 27 | { 28 | std::vector interp_cubic(double t, double T, std::vector p0_pos, 29 | std::vector p1_pos, 30 | std::vector p0_vel, 31 | std::vector p1_vel) 32 | { 33 | /*Returns positions of the joints at time 't' */ 34 | std::vector positions; 35 | for (unsigned int i = 0; i < p0_pos.size(); i++) 36 | { 37 | double a = p0_pos[i]; 38 | double b = p0_vel[i]; 39 | double c = 40 | (-3 * p0_pos[i] + 3 * p1_pos[i] - 2 * T * p0_vel[i] - T * p1_vel[i]) / 41 | pow(T, 2); 42 | double d = (2 * p0_pos[i] - 2 * p1_pos[i] + T * p0_vel[i] + T * p1_vel[i]) / 43 | pow(T, 3); 44 | positions.push_back(a + b * t + c * pow(t, 2) + d * pow(t, 3)); 45 | } 46 | return positions; 47 | } 48 | 49 | bool doTraj(std::vector inp_timestamps, 50 | std::vector> inp_positions, 51 | std::vector> inp_velocities, 52 | std::chrono::high_resolution_clock::time_point& t0, bool& done) 53 | { 54 | std::vector positions; 55 | 56 | auto t = std::chrono::high_resolution_clock::now(); 57 | static unsigned int j = 0; 58 | if (inp_timestamps[inp_timestamps.size() - 1] >= 59 | std::chrono::duration_cast>(t - t0).count()) 60 | { 61 | if (inp_timestamps[j] <= 62 | std::chrono::duration_cast>(t - t0) 63 | .count() && 64 | j < inp_timestamps.size() - 1) 65 | { 66 | j++; 67 | } 68 | positions = interp_cubic( 69 | std::chrono::duration_cast>( 70 | t - 71 | t0).count() - 72 | inp_timestamps[j - 1], 73 | inp_timestamps[j] - inp_timestamps[j - 1], inp_positions[j - 1], 74 | inp_positions[j], inp_velocities[j - 1], inp_velocities[j]); 75 | printf( 76 | "Axis position command sent: %f, at time: %ld \n", positions[0], 77 | std::chrono::duration_cast(t - t0).count()); 78 | // axis_->execPos(positions); 79 | 80 | return true; 81 | } 82 | j = 0; 83 | 84 | done = true; 85 | return true; 86 | } 87 | 88 | } // namespace trajectory_processing -------------------------------------------------------------------------------- /src/axis/motion_kernel.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file motion_kernel.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | namespace RTmotion 27 | { 28 | MotionKernel::MotionKernel() 29 | { 30 | } 31 | 32 | MotionKernel::~MotionKernel() 33 | { 34 | } 35 | 36 | void MotionKernel::runCycle(const double t, const double t_prev, const double pos, const double vel, MC_AXIS_STATES* state) 37 | { 38 | if (!fb_queue_.empty()) 39 | { 40 | ExecutionNode* fb_front = fb_queue_.front(); 41 | DEBUG_PRINT("MotionKernel::runCycle fb_queue_ is not empty %p.\n", (void*)fb_front); 42 | 43 | // Check if the motion kernel mission is done 44 | if (fb_front->checkMissionDone(pos, vel)) 45 | fb_front->onDone(state); 46 | 47 | // If the first FB is aborted or in error, remove it from the execution queue 48 | if (fb_front->isAborted() || fb_front->isError()) 49 | { 50 | DEBUG_PRINT("Abort front FB %p.\n", (void*)fb_front); 51 | fb_front->taken_ = false; 52 | fb_queue_.pop(); 53 | } 54 | 55 | // If the first FB is done, move it to be hold 56 | if (fb_front->isDone()) 57 | { 58 | if (fb_hold_) 59 | { 60 | DEBUG_PRINT("Abort hold FB %p.\n", (void*)fb_hold_); 61 | fb_hold_->taken_ = false; 62 | fb_hold_ = nullptr; 63 | } 64 | fb_hold_ = fb_front; 65 | fb_queue_.pop(); 66 | DEBUG_PRINT("Runtime: %f, FB is done, hold it. FB queue is empty? %d\n", t, fb_queue_.empty()); 67 | } 68 | 69 | // If there is any FB in the queue, activate it 70 | if (!fb_queue_.empty()) 71 | { 72 | ExecutionNode* fb_front = fb_queue_.front(); 73 | DEBUG_PRINT("Front node is active? %d, is enabled? %d.\n", fb_front->isActive(), fb_front->getAxisNodeInstance()->isEnabled()); 74 | if (!fb_front->isActive() && fb_front->getAxisNodeInstance()->isEnabled()) 75 | { 76 | fb_front->onActive(t_prev, state); // Activate the first node from queue 77 | } 78 | 79 | if (fb_front->isActive()) // If the first FB is active, execute it 80 | { 81 | DEBUG_PRINT("FB queue is empty, FB mode is %d.\n", fb_front->getBufferMode()); 82 | if (fb_front->getBufferMode() == mcAborting) 83 | { 84 | DEBUG_PRINT("FB hold is %p.\n", (void*)fb_hold_); 85 | if (fb_hold_) // Since the front FB from queue is ready to execute, remove the hold FB 86 | { 87 | DEBUG_PRINT("Abort hold FB %d.\n", fb_hold_->getMotionMode()); 88 | fb_hold_->onCommandAborted(); 89 | fb_hold_->taken_ = false; 90 | fb_hold_ = nullptr; 91 | } 92 | } 93 | 94 | fb_front->onExecution(t); 95 | } 96 | } 97 | } 98 | } 99 | 100 | void MotionKernel::addFBToQueue(ExecutionNode* node, LREAL current_pos, 101 | LREAL current_vel) 102 | { 103 | node->setReplan(); 104 | DEBUG_PRINT("MotionKernel::addFBToQueue: current_pos: %f, current_vel: %f\n", current_pos, current_vel); 105 | 106 | if (fb_queue_.empty()) // Probably there is a hold node 107 | { 108 | node->setStartPos(current_pos); 109 | node->setStartVel(current_vel); 110 | node->setTerminateCondition(current_pos); 111 | DEBUG_PRINT("MotionKernel::addFBToQueue: fb_queue (size %zu) is empty\n", fb_queue_.size()); 112 | } 113 | else // If queue is not empty, it's not possible to have a hold node 114 | { 115 | ExecutionNode* fb_prev = fb_queue_.back(); 116 | 117 | // If not aborting, then set the end velocity of the previous FB 118 | switch (node->getBufferMode()) 119 | { 120 | case mcBuffered: 121 | break; 122 | case mcAborting: 123 | node->setStartPos(current_pos); 124 | node->setStartVel(current_vel); 125 | node->setTerminateCondition(fb_prev->getEndPos()); 126 | setAllFBsAborted(); 127 | break; 128 | case mcBlendingPrevious: 129 | { 130 | fb_prev->setEndVel(fb_prev->getVelocity()); 131 | fb_prev->setReplan(); 132 | } 133 | break; 134 | case mcBlendingNext: 135 | { 136 | fb_prev->setEndVel(node->getVelocity()); 137 | fb_prev->setReplan(); 138 | } 139 | break; 140 | case mcBlendingHigh: 141 | { 142 | fb_prev->setEndVel( 143 | std::max(fb_prev->getVelocity(), node->getVelocity())); 144 | fb_prev->setReplan(); 145 | } 146 | break; 147 | case mcBlendingLow: 148 | { 149 | fb_prev->setEndVel( 150 | std::min(fb_prev->getVelocity(), node->getVelocity())); 151 | fb_prev->setReplan(); 152 | } 153 | break; 154 | default: 155 | break; 156 | } 157 | 158 | if (node->getBufferMode() != mcAborting) 159 | { 160 | node->setStartPos(fb_prev->getEndPos()); 161 | node->setStartVel(fb_prev->getEndVel()); 162 | node->setTerminateCondition(fb_prev->getEndPos()); 163 | } 164 | } 165 | 166 | fb_queue_.push(node); 167 | } 168 | 169 | std::queue& MotionKernel::getQueuedMotions() 170 | { 171 | return fb_queue_; 172 | } 173 | 174 | void MotionKernel::setAllFBsAborted() 175 | { 176 | while (!fb_queue_.empty()) 177 | { 178 | ExecutionNode* fb = fb_queue_.front(); 179 | fb->onCommandAborted(); 180 | fb->taken_ = false; 181 | fb_queue_.pop(); 182 | } 183 | if (fb_hold_) 184 | { 185 | fb_hold_->onCommandAborted(); 186 | fb_hold_->taken_ = false; 187 | fb_hold_ = nullptr; 188 | } 189 | } 190 | 191 | void MotionKernel::getCommands(double* pos_cmd, double* vel_cmd, 192 | double duration) 193 | { 194 | if (!fb_queue_.empty()) 195 | { 196 | DEBUG_PRINT("MotionKernel::getCommands: fb_queue_ (size %zu) is not empty\n", fb_queue_.size()); 197 | fb_queue_.front()->getCommands(pos_cmd, vel_cmd, duration); 198 | } 199 | else 200 | { 201 | DEBUG_PRINT("MotionKernel::getCommands: fb_queue_ (size %zu) is empty\n", fb_queue_.size()); 202 | if (fb_hold_) 203 | fb_hold_->getCommands(pos_cmd, vel_cmd, duration); 204 | } 205 | } 206 | 207 | } // namespace RTmotion 208 | -------------------------------------------------------------------------------- /src/demo/RT_single_axis_move_relative.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file single_axis_move_relative.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | #include 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | 43 | #define NSEC_PER_SEC (1000000000L) 44 | #define TIMESPEC2NS(T) ((uint64_t)(T).tv_sec * NSEC_PER_SEC + (T).tv_nsec) 45 | #define DIFF_NS(A, B) \ 46 | (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + ((B).tv_nsec) - (A).tv_nsec) 47 | #define CYCLE_COUNTER_PERSEC(X) (NSEC_PER_SEC / 1000 / X) 48 | 49 | static unsigned int cycle_counter = 0; 50 | static int64_t avg_cycle_time; 51 | static int64_t min_cycle_time; 52 | static int64_t max_cycle_time; 53 | static int64_t min_jitter_time; 54 | static int64_t max_jitter_time; 55 | static int64_t total_cycle_ns; 56 | 57 | static pthread_t log_thread; 58 | static sem_t* log_sem; 59 | static pthread_t cyclic_thread; 60 | static volatile int run = 1; 61 | static uint32_t cycle_time = 250; 62 | 63 | static void* rt_thread(void* cookie) 64 | { 65 | struct timespec next_period; 66 | uint8_t servo_run = 0; 67 | 68 | struct sched_param param = {}; 69 | param.sched_priority = 99; 70 | pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); 71 | 72 | struct timespec startTime, endTime, lastStartTime = {}; 73 | int64_t period_ns = 0, exec_ns = 0, period_min_ns = 1000000, 74 | period_max_ns = 0, exec_min_ns = 1000000, exec_max_ns = 0; 75 | int64_t latency_ns = 0; 76 | int64_t latency_min_ns = 1000000, latency_max_ns = -1000000; 77 | int64_t total_exec_ns = 0; 78 | avg_cycle_time = 0; 79 | min_cycle_time = 1000000; 80 | max_cycle_time = -1000000; 81 | min_jitter_time = 1000000; 82 | max_jitter_time = -1000000; 83 | 84 | /* lib init */ 85 | RTmotion::AXIS_REF axis; 86 | axis = std::make_shared(); 87 | axis->setAxisId(1); 88 | axis->setAxisName("X"); 89 | printf("Axis initialized.\n"); 90 | 91 | std::shared_ptr servo; 92 | servo = std::make_shared(); 93 | axis->setServo(servo); 94 | 95 | RTmotion::FbMoveRelative fb_move_abs; 96 | fb_move_abs.setAxis(axis); 97 | fb_move_abs.setExecute(true); 98 | fb_move_abs.setContinuousUpdate(false); 99 | fb_move_abs.setDistance(3.14); 100 | fb_move_abs.setVelocity(1.57); 101 | fb_move_abs.setAcceleration(3.14); 102 | fb_move_abs.setJerk(5000); 103 | printf("Function block initialized.\n"); 104 | 105 | clock_gettime(CLOCK_MONOTONIC, &next_period); 106 | lastStartTime = next_period; 107 | endTime = next_period; 108 | 109 | while (run) 110 | { 111 | next_period.tv_nsec += cycle_time * 1000; 112 | while (next_period.tv_nsec >= NSEC_PER_SEC) 113 | { 114 | next_period.tv_nsec -= NSEC_PER_SEC; 115 | next_period.tv_sec++; 116 | } 117 | 118 | clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL); 119 | 120 | clock_gettime(CLOCK_MONOTONIC, &startTime); 121 | latency_ns = DIFF_NS(next_period, startTime); 122 | period_ns = DIFF_NS(lastStartTime, startTime); 123 | exec_ns = DIFF_NS(lastStartTime, endTime); 124 | lastStartTime = startTime; 125 | 126 | if (latency_ns > latency_max_ns) 127 | { 128 | latency_max_ns = latency_ns; 129 | } 130 | if (latency_ns < latency_min_ns) 131 | { 132 | latency_min_ns = latency_ns; 133 | } 134 | if (period_ns > period_max_ns) 135 | { 136 | period_max_ns = period_ns; 137 | } 138 | if (period_ns < period_min_ns) 139 | { 140 | period_min_ns = period_ns; 141 | } 142 | if (exec_ns > exec_max_ns) 143 | { 144 | exec_max_ns = exec_ns; 145 | } 146 | if (exec_ns < exec_min_ns) 147 | { 148 | exec_min_ns = exec_ns; 149 | } 150 | total_exec_ns += exec_ns; 151 | 152 | cycle_counter++; 153 | 154 | /* task 1s */ 155 | if (!(cycle_counter % CYCLE_COUNTER_PERSEC(cycle_time))) 156 | { 157 | if (min_cycle_time > exec_min_ns) 158 | min_cycle_time = exec_min_ns; 159 | if (max_cycle_time < exec_max_ns) 160 | max_cycle_time = exec_max_ns; 161 | if (min_jitter_time > latency_min_ns) 162 | min_jitter_time = latency_min_ns; 163 | if (max_jitter_time < latency_max_ns) 164 | max_jitter_time = latency_max_ns; 165 | 166 | period_max_ns = -1000000; 167 | period_min_ns = 1000000; 168 | exec_max_ns = -1000000; 169 | exec_min_ns = 1000000; 170 | latency_max_ns = -1000000; 171 | latency_min_ns = 1000000; 172 | total_cycle_ns += total_exec_ns; 173 | total_exec_ns = 0; 174 | 175 | if (servo_run == 0) 176 | { 177 | cycle_counter = 0; 178 | total_cycle_ns = 0; 179 | min_jitter_time = 1000000; 180 | max_jitter_time = -1000000; 181 | min_cycle_time = 1000000; 182 | max_cycle_time = -1000000; 183 | servo_run = 1; 184 | } 185 | sem_post(log_sem); 186 | } 187 | 188 | /* cycle process */ 189 | axis->runCycle(); 190 | fb_move_abs.runCycle(); 191 | 192 | if (!fb_move_abs.isEnabled()) 193 | { 194 | printf("Enable the function block\n"); 195 | fb_move_abs.setExecute(true); 196 | } 197 | 198 | if (fb_move_abs.isDone()) 199 | { 200 | printf("Function block MC_MoveRelative finished.\n"); 201 | fb_move_abs.setExecute(false); 202 | } 203 | 204 | clock_gettime(CLOCK_MONOTONIC, &endTime); 205 | } 206 | 207 | avg_cycle_time = total_cycle_ns / cycle_counter; 208 | printf("*********************************************\n"); 209 | printf("average cycle time %10.3f\n", (float)avg_cycle_time / 1000); 210 | printf("cycle counter %10d\n", cycle_counter); 211 | printf("cycle time %10.3f ... %10.3f\n", 212 | (float)min_cycle_time / 1000, (float)max_cycle_time / 1000); 213 | printf("jitter time %10.3f ... %10.3f\n", 214 | (float)min_jitter_time / 1000, (float)max_jitter_time / 1000); 215 | printf("*********************************************\n"); 216 | 217 | return NULL; 218 | } 219 | 220 | void signal_handler(int sig) 221 | { 222 | run = 0; 223 | } 224 | 225 | static void setup_sched_parameters(pthread_attr_t* attr, int prio) 226 | { 227 | struct sched_param p; 228 | int ret; 229 | 230 | ret = pthread_attr_init(attr); 231 | if (ret) 232 | error(1, ret, "pthread_attr_init()"); 233 | 234 | ret = pthread_attr_setinheritsched(attr, PTHREAD_EXPLICIT_SCHED); 235 | if (ret) 236 | error(1, ret, "pthread_attr_setinheritsched()"); 237 | 238 | ret = pthread_attr_setschedpolicy(attr, prio ? SCHED_FIFO : SCHED_OTHER); 239 | if (ret) 240 | error(1, ret, "pthread_attr_setschedpolicy()"); 241 | 242 | p.sched_priority = prio; 243 | ret = pthread_attr_setschedparam(attr, &p); 244 | if (ret) 245 | error(1, ret, "pthread_attr_setschedparam()"); 246 | } 247 | 248 | static void* log_proc(void* cookie) 249 | { 250 | while (run) 251 | { 252 | int err; 253 | err = sem_wait(log_sem); 254 | if (err < 0) 255 | { 256 | if (errno != EIDRM) 257 | error(1, errno, "sem_wait()"); 258 | break; 259 | } 260 | 261 | printf("time: %10ld\n", 262 | cycle_counter / CYCLE_COUNTER_PERSEC(cycle_time)); 263 | printf("[CYCLE] min:%10.3f max:%10.3f\n", (float)min_cycle_time / 1000, 264 | (float)max_cycle_time / 1000); 265 | printf("[JITTER] min:%10.3f max:%10.3f\n", 266 | (float)min_jitter_time / 1000, (float)max_jitter_time / 1000); 267 | } 268 | 269 | return NULL; 270 | } 271 | 272 | static void getOptions(int argc, char** argv) 273 | { 274 | int index; 275 | static struct option longOptions[] = { 276 | /* name has_arg flag val */ 277 | { "cycle", required_argument, NULL, 't' }, 278 | { "help", no_argument, NULL, 'h' }, 279 | {} 280 | }; 281 | do 282 | { 283 | index = getopt_long(argc, argv, "c:h", longOptions, NULL); 284 | switch (index) 285 | { 286 | case 'c': 287 | cycle_time = atoi(optarg); 288 | printf("cycle time: %dus\n", cycle_time); 289 | break; 290 | case 'h': 291 | printf("Global options:\n"); 292 | printf(" --cycle -c Set cycle time for microsecond.\n"); 293 | printf(" --help -h Show this help.\n"); 294 | printf("default cycle time:%dus\n", cycle_time); 295 | exit(0); 296 | break; 297 | default: 298 | break; 299 | } 300 | } while (index != -1); 301 | } 302 | 303 | int main(int argc, char* argv[]) 304 | { 305 | int ret; 306 | char sem_name[16]; 307 | pthread_attr_t tattr; 308 | 309 | getOptions(argc, argv); 310 | 311 | signal(SIGTERM, signal_handler); 312 | signal(SIGINT, signal_handler); 313 | 314 | mlockall(MCL_CURRENT | MCL_FUTURE); 315 | 316 | /* create sem */ 317 | snprintf(sem_name, sizeof(sem_name), "/logsem-%d", getpid()); 318 | sem_unlink(sem_name); /* may fail */ 319 | log_sem = sem_open(sem_name, O_CREAT | O_EXCL, 0666, 0); 320 | if (log_sem == SEM_FAILED) 321 | error(1, errno, "sem_open()"); 322 | 323 | printf("starting threading\n"); 324 | /* create log thread */ 325 | setup_sched_parameters(&tattr, 0); 326 | ret = pthread_create(&log_thread, &tattr, log_proc, NULL); 327 | if (ret) 328 | error(1, ret, "pthread_create(latency)"); 329 | pthread_attr_destroy(&tattr); 330 | 331 | /* create cyclic thread */ 332 | setup_sched_parameters(&tattr, 99); 333 | ret = pthread_create(&cyclic_thread, &tattr, rt_thread, NULL); 334 | if (ret) 335 | error(1, ret, "pthread_create(latency)"); 336 | pthread_attr_destroy(&tattr); 337 | 338 | while (run) 339 | sched_yield(); 340 | 341 | usleep(10000); 342 | pthread_cancel(log_thread); 343 | pthread_join(log_thread, NULL); 344 | pthread_cancel(cyclic_thread); 345 | pthread_join(cyclic_thread, NULL); 346 | 347 | sem_close(log_sem); 348 | sem_unlink(sem_name); 349 | 350 | return 0; 351 | } 352 | -------------------------------------------------------------------------------- /src/demo/multi-axis.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file multi-axis.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | #include 46 | 47 | #define CYCLE_US 1000 48 | #define BUFFER_SIZE 1000 49 | #define AXIS_NUM 84 50 | 51 | static volatile int run = 1; 52 | static pthread_t cyclic_thread; 53 | static int64_t* execute_time; 54 | 55 | static double running_time = 1.0; 56 | static unsigned int running_time_t = (unsigned int)(BUFFER_SIZE * running_time); 57 | 58 | static std::map time_stamps; 59 | 60 | void* my_thread(void* arg) 61 | { 62 | RTmotion::AXIS_REF axis[AXIS_NUM]; 63 | for (size_t i = 0; i < AXIS_NUM; i++) 64 | { 65 | axis[i] = std::make_shared(); 66 | axis[i]->setAxisId(1); 67 | axis[i]->setAxisName("X" + i); 68 | } 69 | 70 | std::shared_ptr servo[AXIS_NUM]; 71 | for (size_t i = 0; i < AXIS_NUM; i++) 72 | { 73 | servo[i] = std::make_shared(); 74 | axis[i]->setServo(servo[i]); 75 | } 76 | printf("Axis initialized.\n"); 77 | 78 | RTmotion::FbPower fb_power[AXIS_NUM]; 79 | for (size_t i = 0; i < AXIS_NUM; i++) 80 | { 81 | fb_power[i].setAxis(axis[i]); 82 | fb_power[i].setExecute(true); 83 | fb_power[i].setEnablePositive(true); 84 | fb_power[i].setEnableNegative(true); 85 | } 86 | 87 | RTmotion::FbMoveRelative fb_move_rel[AXIS_NUM]; 88 | for (size_t i = 0; i < AXIS_NUM; i++) 89 | { 90 | fb_move_rel[i].setAxis(axis[i]); 91 | fb_move_rel[i].setContinuousUpdate(false); 92 | fb_move_rel[i].setDistance(500); 93 | fb_move_rel[i].setVelocity(100); 94 | fb_move_rel[i].setAcceleration(100); 95 | fb_move_rel[i].setJerk(5000); 96 | fb_move_rel[i].setBufferMode(RTmotion::mcAborting); 97 | } 98 | printf("Function block initialized.\n"); 99 | 100 | struct timespec next_period, start_time, end_time; 101 | unsigned int cycle_counter = 0; 102 | 103 | struct sched_param param = {}; 104 | param.sched_priority = 99; 105 | pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); 106 | clock_gettime(CLOCK_MONOTONIC, &next_period); 107 | 108 | while (run != 0) 109 | { 110 | next_period.tv_nsec += CYCLE_US * 1000; 111 | while (next_period.tv_nsec >= NSEC_PER_SEC) 112 | { 113 | next_period.tv_nsec -= NSEC_PER_SEC; 114 | next_period.tv_sec++; 115 | } 116 | clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL); 117 | 118 | clock_gettime(CLOCK_MONOTONIC, &start_time); 119 | for (size_t i = 0; i < AXIS_NUM; i++) 120 | { 121 | axis[i]->runCycle(); 122 | fb_power[i].runCycle(); 123 | fb_move_rel[i].runCycle(); 124 | } 125 | clock_gettime(CLOCK_MONOTONIC, &end_time); 126 | 127 | if (cycle_counter < running_time_t) 128 | execute_time[cycle_counter] = DIFF_NS(start_time, end_time); 129 | else 130 | run = 0; 131 | 132 | for (size_t i = 0; i < AXIS_NUM; i++) 133 | { 134 | if (!fb_move_rel[i].isEnabled() && fb_power[i].getPowerStatus()) 135 | fb_move_rel[i].setExecute(true); 136 | 137 | // if (fb_move_rel[i].isDone() && (cycle_counter % 6000 == 0)) 138 | if (cycle_counter % 600 == 0) 139 | fb_move_rel[i].setExecute(false); 140 | } 141 | 142 | cycle_counter++; 143 | } 144 | return NULL; 145 | } 146 | 147 | void signal_handler(int sig) 148 | { 149 | run = 0; 150 | } 151 | 152 | static void getOptions(int argc, char** argv) 153 | { 154 | int index; 155 | static struct option longOptions[] = { 156 | // name has_arg flag val 157 | { "time", required_argument, NULL, 't' }, 158 | { "help", no_argument, NULL, 'h' }, 159 | {} 160 | }; 161 | do 162 | { 163 | index = getopt_long(argc, argv, "t:h", longOptions, NULL); 164 | switch (index) 165 | { 166 | case 't': 167 | running_time = atof(optarg); 168 | running_time_t = (unsigned int)(BUFFER_SIZE * running_time); 169 | printf("Time: Set running time to %d ms\n", running_time_t); 170 | break; 171 | case 'h': 172 | printf("Global options:\n"); 173 | printf(" --time -t Set running time(s).\n"); 174 | printf(" --help -h Show this help.\n"); 175 | exit(0); 176 | break; 177 | } 178 | } while (index != -1); 179 | } 180 | 181 | /**************************************************************************** 182 | * Main function 183 | ***************************************************************************/ 184 | int main(int argc, char* argv[]) 185 | { 186 | getOptions(argc, argv); 187 | execute_time = (int64_t*)malloc(sizeof(int64_t) * int(running_time_t)); 188 | signal(SIGTERM, signal_handler); 189 | signal(SIGINT, signal_handler); 190 | mlockall(MCL_CURRENT | MCL_FUTURE); 191 | 192 | /* Create cyclic RT-thread */ 193 | pthread_attr_t thattr; 194 | pthread_attr_init(&thattr); 195 | pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); 196 | 197 | if (pthread_create(&cyclic_thread, &thattr, &my_thread, NULL)) 198 | { 199 | fprintf(stderr, "pthread_create cyclic task failed\n"); 200 | return 1; 201 | } 202 | 203 | while (run) 204 | { 205 | sched_yield(); 206 | } 207 | 208 | pthread_join(cyclic_thread, NULL); 209 | 210 | // Open file for writing 211 | FILE* fptr; 212 | if ((fptr = fopen("data.log", "wb")) == NULL) 213 | { 214 | printf("Error! opening file: data.log"); 215 | exit(1); // Program exits if the file pointer returns NULL. 216 | } 217 | 218 | // Write cycle running time data 219 | for (size_t i = 0; i < running_time_t; ++i) 220 | fprintf(fptr, "%ld\n", execute_time[i]); 221 | fclose(fptr); 222 | 223 | printf("End of Program\n"); 224 | return 0; 225 | } 226 | 227 | /****************************************************************************/ 228 | -------------------------------------------------------------------------------- /src/demo/single_axis_move_relative.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file single_axis_move_relative.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | 27 | #include 28 | 29 | #include 30 | #include 31 | #include 32 | #include 33 | #include 34 | #include 35 | #include 36 | #include 37 | #include 38 | #include 39 | #include 40 | #include 41 | #include 42 | #include 43 | #include 44 | #include 45 | 46 | #define CYCLE_US 1000 47 | #define BUFFER_SIZE 1000 48 | 49 | static volatile int run = 1; 50 | static pthread_t cyclic_thread; 51 | static int64_t* execute_time; 52 | 53 | static double running_time = 1.0; 54 | static unsigned int running_time_t = (unsigned int)(BUFFER_SIZE * running_time); 55 | 56 | static std::map time_stamps; 57 | 58 | void* my_thread(void* arg) 59 | { 60 | RTmotion::AXIS_REF axis; 61 | axis = std::make_shared(); 62 | axis->setAxisId(1); 63 | axis->setAxisName("X"); 64 | printf("Axis initialized.\n"); 65 | 66 | std::shared_ptr servo; 67 | servo = std::make_shared(); 68 | axis->setServo(servo); 69 | 70 | RTmotion::FbMoveRelative fb_move_abs; 71 | fb_move_abs.setAxis(axis); 72 | fb_move_abs.setContinuousUpdate(false); 73 | fb_move_abs.setDistance(3.14); 74 | fb_move_abs.setVelocity(1.57); 75 | fb_move_abs.setAcceleration(3.14); 76 | fb_move_abs.setJerk(5000); 77 | fb_move_abs.setTimeRecodPtr(&time_stamps); 78 | printf("Function block initialized.\n"); 79 | 80 | struct timespec next_period, start_time, end_time; 81 | unsigned int cycle_counter = 0; 82 | 83 | struct sched_param param = {}; 84 | param.sched_priority = 99; 85 | pthread_setschedparam(pthread_self(), SCHED_FIFO, ¶m); 86 | clock_gettime(CLOCK_MONOTONIC, &next_period); 87 | 88 | while (run != 0) 89 | { 90 | next_period.tv_nsec += CYCLE_US * 1000; 91 | while (next_period.tv_nsec >= NSEC_PER_SEC) 92 | { 93 | next_period.tv_nsec -= NSEC_PER_SEC; 94 | next_period.tv_sec++; 95 | } 96 | clock_nanosleep(CLOCK_MONOTONIC, TIMER_ABSTIME, &next_period, NULL); 97 | 98 | clock_gettime(CLOCK_MONOTONIC, &start_time); 99 | 100 | axis->runCycle(); 101 | fb_move_abs.runCycle(); 102 | 103 | clock_gettime(CLOCK_MONOTONIC, &end_time); 104 | if (cycle_counter < running_time_t) 105 | { 106 | execute_time[cycle_counter] = DIFF_NS(start_time, end_time); 107 | } 108 | else 109 | run = 0; 110 | 111 | if (!fb_move_abs.isEnabled()) 112 | { 113 | // printf("Enable the function block\n"); 114 | fb_move_abs.setExecute(true); 115 | execute_time[++cycle_counter] = 0; 116 | } 117 | 118 | if (fb_move_abs.isDone()) 119 | { 120 | // printf("Function block MC_MoveRelative finished.\n"); 121 | fb_move_abs.setExecute(false); 122 | } 123 | 124 | cycle_counter++; 125 | } 126 | return NULL; 127 | } 128 | 129 | void signal_handler(int sig) 130 | { 131 | run = 0; 132 | } 133 | 134 | static void getOptions(int argc, char** argv) 135 | { 136 | int index; 137 | static struct option longOptions[] = { 138 | // name has_arg flag val 139 | { "time", required_argument, NULL, 't' }, 140 | { "help", no_argument, NULL, 'h' }, 141 | {} 142 | }; 143 | do 144 | { 145 | index = getopt_long(argc, argv, "t:h", longOptions, NULL); 146 | switch (index) 147 | { 148 | case 't': 149 | running_time = atof(optarg); 150 | running_time_t = (unsigned int)(BUFFER_SIZE * running_time); 151 | printf("Time: Set running time to %d ms\n", running_time_t); 152 | break; 153 | case 'h': 154 | printf("Global options:\n"); 155 | printf(" --time -t Set running time(s).\n"); 156 | printf(" --help -h Show this help.\n"); 157 | exit(0); 158 | break; 159 | } 160 | } while (index != -1); 161 | } 162 | 163 | /**************************************************************************** 164 | * Main function 165 | ***************************************************************************/ 166 | int main(int argc, char* argv[]) 167 | { 168 | getOptions(argc, argv); 169 | execute_time = (int64_t*)malloc(sizeof(int64_t) * int(running_time_t)); 170 | signal(SIGTERM, signal_handler); 171 | signal(SIGINT, signal_handler); 172 | mlockall(MCL_CURRENT | MCL_FUTURE); 173 | 174 | /* Create cyclic RT-thread */ 175 | pthread_attr_t thattr; 176 | pthread_attr_init(&thattr); 177 | pthread_attr_setdetachstate(&thattr, PTHREAD_CREATE_JOINABLE); 178 | 179 | if (pthread_create(&cyclic_thread, &thattr, &my_thread, NULL)) 180 | { 181 | fprintf(stderr, "pthread_create cyclic task failed\n"); 182 | return 1; 183 | } 184 | 185 | while (run) 186 | { 187 | sched_yield(); 188 | } 189 | 190 | pthread_join(cyclic_thread, NULL); 191 | 192 | FILE* fptr; 193 | 194 | // Write cycle running time data 195 | if ((fptr = fopen("data.log", "wb")) == NULL) 196 | { 197 | printf("Error! opening file: data.log"); 198 | exit(1); // Program exits if the file pointer returns NULL. 199 | } 200 | 201 | for (size_t i = 0; i < running_time_t; ++i) 202 | { 203 | fprintf(fptr, "%ld\n", execute_time[i]); 204 | } 205 | fclose(fptr); 206 | 207 | // Write time stamps data 208 | if ((fptr = fopen("time_stamps.log", "wb")) == NULL) 209 | { 210 | printf("Error! opening file: time_stamps.log"); 211 | exit(1); // Program exits if the file pointer returns NULL. 212 | } 213 | 214 | for (auto time_stamp : time_stamps) 215 | { 216 | fprintf(fptr, "%s: %f\n", time_stamp.first.c_str(), time_stamp.second); 217 | } 218 | fclose(fptr); 219 | 220 | printf("End of Program\n"); 221 | return 0; 222 | } 223 | 224 | /****************************************************************************/ 225 | -------------------------------------------------------------------------------- /src/fb/fb_axis_motion.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_motion.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbAxisMotion::FbAxisMotion() 28 | : continuous_update_(false) 29 | , position_(0) 30 | , velocity_(0) 31 | , acceleration_(0) 32 | , deceleration_(0) 33 | , jerk_(5000) 34 | , direction_(mcCurrentDirection) 35 | , triggered_(false) 36 | { 37 | } 38 | 39 | void FbAxisMotion::runCycle() 40 | { 41 | if (execute_ && !triggered_) // `Execute` rising edge 42 | { 43 | MC_ERROR_CODE err = onRisingEdgeExecution(); 44 | if (err) 45 | onError(err); 46 | else 47 | { 48 | active_ = done_ = error_ = command_aborted_ = false; 49 | busy_ = true; 50 | } 51 | } 52 | else if (execute_ && triggered_) // `Execute` hold high value 53 | { 54 | MC_ERROR_CODE err = onExecution(); 55 | if (err) 56 | onError(err); 57 | } 58 | else if (!execute_ && triggered_) // `Execute` falling edge 59 | { 60 | MC_ERROR_CODE err = onFallingEdgeExecution(); 61 | if (err) 62 | onError(err); 63 | else 64 | { 65 | if ((done_ || command_aborted_ || error_) && !busy_) 66 | { 67 | done_ = command_aborted_ = error_ = busy_ = active_ = false; 68 | error_id_ = mcErrorCodeGood; 69 | } 70 | } 71 | 72 | output_flag_ = false; 73 | } // `Execute` hold low value 74 | else 75 | { 76 | if (!busy_ && !output_flag_) 77 | { 78 | active_ = done_ = command_aborted_ = error_ = false; 79 | error_id_ = mcErrorCodeGood; 80 | } 81 | output_flag_ = false; 82 | } 83 | 84 | triggered_ = execute_; 85 | } 86 | 87 | MC_ERROR_CODE FbAxisMotion::onRisingEdgeExecution() 88 | { 89 | return mcErrorCodeGood; 90 | } 91 | 92 | MC_ERROR_CODE FbAxisMotion::onFallingEdgeExecution() 93 | { 94 | return mcErrorCodeGood; 95 | } 96 | 97 | MC_ERROR_CODE FbAxisMotion::onExecution() 98 | { 99 | return mcErrorCodeGood; 100 | } 101 | 102 | void FbAxisMotion::setExecute(BOOL execute) 103 | { 104 | execute_ = execute; 105 | } 106 | 107 | void FbAxisMotion::setContinuousUpdate(BOOL continuous_update) 108 | { 109 | continuous_update_ = continuous_update; 110 | } 111 | 112 | void FbAxisMotion::setPosition(LREAL position) 113 | { 114 | position_ = position; 115 | } 116 | 117 | void FbAxisMotion::setVelocity(LREAL velocity) 118 | { 119 | velocity_ = velocity; 120 | } 121 | 122 | void FbAxisMotion::setAcceleration(LREAL acceleration) 123 | { 124 | acceleration_ = acceleration; 125 | } 126 | 127 | void FbAxisMotion::setDeceleration(LREAL deceleration) 128 | { 129 | deceleration_ = deceleration; 130 | } 131 | 132 | void FbAxisMotion::setJerk(LREAL jerk) 133 | { 134 | jerk_ = jerk; 135 | } 136 | 137 | void FbAxisMotion::setDirection(MC_DIRECTION direction) 138 | { 139 | direction_ = direction; 140 | } 141 | 142 | void FbAxisMotion::setBufferMode(MC_BUFFER_MODE mode) 143 | { 144 | buffer_mode_ = mode; 145 | } 146 | 147 | BOOL FbAxisMotion::isError() 148 | { 149 | return axis_->getAxisError() ? (bool)axis_->getAxisError() : error_; 150 | } 151 | 152 | MC_ERROR_CODE FbAxisMotion::getErrorID() 153 | { 154 | return axis_->getAxisError() ? axis_->getAxisError() : error_id_; 155 | } 156 | 157 | LREAL FbAxisMotion::getPosition() 158 | { 159 | return position_; 160 | } 161 | 162 | LREAL FbAxisMotion::getVelocity() 163 | { 164 | return velocity_; 165 | } 166 | 167 | LREAL FbAxisMotion::getAcceleration() 168 | { 169 | return acceleration_; 170 | } 171 | 172 | LREAL FbAxisMotion::getDeceleration() 173 | { 174 | return deceleration_; 175 | } 176 | 177 | LREAL FbAxisMotion::getJerk() 178 | { 179 | return jerk_; 180 | } 181 | 182 | MC_DIRECTION FbAxisMotion::getDirection() 183 | { 184 | return direction_; 185 | } 186 | 187 | MC_BUFFER_MODE FbAxisMotion::getBufferMode() 188 | { 189 | return buffer_mode_; 190 | } 191 | 192 | } // namespace RTmotion 193 | -------------------------------------------------------------------------------- /src/fb/fb_axis_node.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_node.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | 26 | namespace RTmotion 27 | { 28 | FbAxisNode::FbAxisNode() 29 | : execute_(false) 30 | , done_(false) 31 | , busy_(false) 32 | , active_(false) 33 | , command_aborted_(false) 34 | , error_(false) 35 | , error_id_(mcErrorCodeGood) 36 | , buffer_mode_(mcBuffered) 37 | , output_flag_(false) 38 | { 39 | } 40 | 41 | void FbAxisNode::syncStatus(BOOL done, BOOL busy, BOOL active, BOOL cmd_aborted, 42 | BOOL error, MC_ERROR_CODE error_id) 43 | { 44 | done_ = done; 45 | busy_ = busy; 46 | active_ = active; 47 | command_aborted_ = cmd_aborted; 48 | error_ = error; 49 | error_id_ = error_id; 50 | 51 | output_flag_ = !execute_; 52 | DEBUG_PRINT("FbAxisNode::syncStatus %s\n", error_ ? "true" : "false" ); 53 | } 54 | 55 | BOOL FbAxisNode::isEnabled() 56 | { 57 | return execute_; 58 | } 59 | 60 | BOOL FbAxisNode::isDone() 61 | { 62 | return done_; 63 | } 64 | 65 | BOOL FbAxisNode::isBusy() 66 | { 67 | return busy_; 68 | } 69 | 70 | BOOL FbAxisNode::isActive() 71 | { 72 | return active_; 73 | } 74 | 75 | BOOL FbAxisNode::isAborted() 76 | { 77 | return command_aborted_; 78 | } 79 | 80 | BOOL FbAxisNode::isError() 81 | { 82 | DEBUG_PRINT("FbAxisNode::isError %s\n", error_ ? "true" : "false" ); 83 | return error_; 84 | } 85 | 86 | MC_ERROR_CODE FbAxisNode::getErrorID() 87 | { 88 | return error_id_; 89 | } 90 | 91 | } // namespace RTmotion 92 | -------------------------------------------------------------------------------- /src/fb/fb_axis_read.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_axis_read.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbAxisRead::FbAxisRead() 28 | : enable_(false) 29 | , valid_(false) 30 | , busy_(false) 31 | , error_(false) 32 | , error_id_(mcErrorCodeGood) 33 | { 34 | } 35 | 36 | void FbAxisRead::setExecute(BOOL execute) 37 | { 38 | enable_ = execute; 39 | } 40 | 41 | BOOL FbAxisRead::isEnabled() 42 | { 43 | return enable_; 44 | } 45 | 46 | BOOL FbAxisRead::isValid() 47 | { 48 | return valid_; 49 | } 50 | 51 | BOOL FbAxisRead::isBusy() 52 | { 53 | return busy_; 54 | } 55 | 56 | BOOL FbAxisRead::isError() 57 | { 58 | return axis_->getAxisError(); 59 | } 60 | 61 | MC_ERROR_CODE FbAxisRead::getErrorID() 62 | { 63 | return axis_->getAxisError(); 64 | } 65 | 66 | LREAL FbAxisRead::getFloatValue() 67 | { 68 | return 0.0; 69 | } 70 | 71 | MC_ERROR_CODE FbAxisRead::getErrorValue() 72 | { 73 | return mcErrorCodeGood; 74 | } 75 | 76 | void FbAxisRead::runCycle() 77 | { 78 | 79 | } 80 | 81 | } // namespace RTmotion 82 | -------------------------------------------------------------------------------- /src/fb/fb_base.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file function_block.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FunctionBlock::FunctionBlock() : axis_(nullptr) 28 | { 29 | } 30 | 31 | void FunctionBlock::setAxis(AXIS_REF axis) 32 | { 33 | axis_ = axis; 34 | } 35 | 36 | // Functions for working on different status 37 | MC_ERROR_CODE FunctionBlock::onRisingEdgeExecution() 38 | { 39 | return mcErrorCodeGood; 40 | } 41 | 42 | MC_ERROR_CODE FunctionBlock::onFallingEdgeExecution() 43 | { 44 | return mcErrorCodeGood; 45 | } 46 | 47 | MC_ERROR_CODE FunctionBlock::onExecution() 48 | { 49 | return mcErrorCodeGood; 50 | } 51 | 52 | void FunctionBlock::onError(MC_ERROR_CODE error_code) 53 | { 54 | 55 | } 56 | 57 | } // namespace RTmotion -------------------------------------------------------------------------------- /src/fb/fb_halt.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_halt.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbHalt::FbHalt() 28 | { 29 | } 30 | 31 | MC_ERROR_CODE FbHalt::onRisingEdgeExecution() 32 | { 33 | axis_->addFBToQueue(this, mcHalt); 34 | return mcErrorCodeGood; 35 | } 36 | 37 | } // namespace RTmotion -------------------------------------------------------------------------------- /src/fb/fb_move_absolute.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_absolute.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbMoveAbsolute::FbMoveAbsolute() 28 | { 29 | } 30 | 31 | MC_ERROR_CODE FbMoveAbsolute::onRisingEdgeExecution() 32 | { 33 | axis_->addFBToQueue(this, mcMoveAbsolute); 34 | return mcErrorCodeGood; 35 | } 36 | 37 | } // namespace RTmotion 38 | -------------------------------------------------------------------------------- /src/fb/fb_move_additive.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_additive.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbMoveAdditive::FbMoveAdditive() 28 | { 29 | distance_ = 0.0; 30 | } 31 | 32 | MC_ERROR_CODE FbMoveAdditive::onRisingEdgeExecution() 33 | { 34 | axis_->addFBToQueue(this, mcMoveAdditive); 35 | return mcErrorCodeGood; 36 | } 37 | 38 | } // namespace RTmotion 39 | -------------------------------------------------------------------------------- /src/fb/fb_move_relative.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_relative.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbMoveRelative::FbMoveRelative() 28 | { 29 | distance_ = 0.0; 30 | } 31 | 32 | MC_ERROR_CODE FbMoveRelative::onRisingEdgeExecution() 33 | { 34 | axis_->addFBToQueue(this, mcMoveRelative); 35 | return mcErrorCodeGood; 36 | } 37 | 38 | } // namespace RTmotion 39 | -------------------------------------------------------------------------------- /src/fb/fb_move_velocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_move_relative.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbMoveVelocity::FbMoveVelocity() 28 | { 29 | in_velocity_ = false; 30 | } 31 | 32 | MC_ERROR_CODE FbMoveVelocity::onRisingEdgeExecution() 33 | { 34 | axis_->addFBToQueue(this, mcMoveVelocity); 35 | return mcErrorCodeGood; 36 | } 37 | 38 | } // namespace RTmotion 39 | -------------------------------------------------------------------------------- /src/fb/fb_power.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_power.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbPower::FbPower() 28 | : enable_(false) 29 | , enable_positive_(false) 30 | , enable_negative_(false) 31 | { 32 | } 33 | 34 | void FbPower::setExecute(BOOL execute) 35 | { 36 | enable_ = execute; 37 | } 38 | 39 | void FbPower::setEnablePositive(BOOL enable_positive) 40 | { 41 | enable_positive_ = enable_positive; 42 | } 43 | 44 | void FbPower::setEnableNegative(BOOL enable_negative) 45 | { 46 | enable_negative_ = enable_negative; 47 | } 48 | 49 | BOOL FbPower::isEnabled() 50 | { 51 | return axis_->powerTriggered(); 52 | } 53 | 54 | BOOL FbPower::getPowerStatus() 55 | { 56 | return axis_->powerOn(); 57 | } 58 | 59 | BOOL FbPower::getPowerStatusValid() 60 | { 61 | return axis_->powerTriggered(); 62 | } 63 | 64 | BOOL FbPower::isError() 65 | { 66 | return axis_->getAxisError(); 67 | } 68 | 69 | MC_ERROR_CODE FbPower::getErrorID() 70 | { 71 | return axis_->getAxisError(); 72 | } 73 | 74 | void FbPower::runCycle() 75 | { 76 | axis_->setPower(enable_, enable_positive_, enable_negative_); 77 | } 78 | 79 | } // namespace RTmotion 80 | -------------------------------------------------------------------------------- /src/fb/fb_read_actual_position.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_actual_position.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | LREAL FbReadActualPosition::getFloatValue() 28 | { 29 | return position_; 30 | } 31 | 32 | void FbReadActualPosition::runCycle() 33 | { 34 | if (enable_) 35 | { 36 | valid_ = true; 37 | if (axis_->getAxisState() == mcErrorStop) 38 | { 39 | position_ = 0; 40 | error_ = true; 41 | error_id_ = mcErrorCode_AxisErrorStop; 42 | return; 43 | } 44 | 45 | error_ = false; 46 | position_ = axis_->toUserPos(); 47 | } 48 | else 49 | { 50 | valid_ = false; 51 | busy_ = false; 52 | error_ = false; 53 | } 54 | } 55 | 56 | } // namespace RTmotion 57 | -------------------------------------------------------------------------------- /src/fb/fb_read_actual_velocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_actual_velocity.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | LREAL FbReadActualVelocity::getFloatValue() 28 | { 29 | return velocity_; 30 | } 31 | 32 | void FbReadActualVelocity::runCycle() 33 | { 34 | if (enable_) 35 | { 36 | valid_ = true; 37 | if (axis_->getAxisState() == mcErrorStop) 38 | { 39 | velocity_ = 0; 40 | error_ = true; 41 | error_id_ = mcErrorCode_AxisErrorStop; 42 | return; 43 | } 44 | 45 | error_ = false; 46 | velocity_ = axis_->toUserVel(); 47 | } 48 | else 49 | { 50 | valid_ = false; 51 | busy_ = false; 52 | error_ = false; 53 | } 54 | } 55 | 56 | } // namespace RTmotion 57 | -------------------------------------------------------------------------------- /src/fb/fb_read_axis_error.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_actual_position.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | MC_ERROR_CODE FbReadAxisError::getErrorValue() 28 | { 29 | return axis_error_; 30 | } 31 | 32 | void FbReadAxisError::runCycle() 33 | { 34 | if (enable_) 35 | { 36 | valid_ = true; 37 | axis_error_ = axis_->getAxisError(); 38 | } 39 | else 40 | valid_ = false; 41 | } 42 | 43 | } // namespace RTmotion 44 | -------------------------------------------------------------------------------- /src/fb/fb_read_command_position.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_command_position.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | LREAL FbReadCommandPosition::getFloatValue() 28 | { 29 | return position_; 30 | } 31 | 32 | void FbReadCommandPosition::runCycle() 33 | { 34 | if (enable_) 35 | { 36 | valid_ = true; 37 | if (axis_->getAxisState() == mcErrorStop) 38 | { 39 | position_ = 0; 40 | error_ = true; 41 | error_id_ = mcErrorCode_AxisErrorStop; 42 | return; 43 | } 44 | 45 | error_ = false; 46 | position_ = axis_->toUserPosCmd(); 47 | } 48 | else 49 | { 50 | valid_ = false; 51 | busy_ = false; 52 | error_ = false; 53 | } 54 | } 55 | 56 | } // namespace RTmotion 57 | -------------------------------------------------------------------------------- /src/fb/fb_read_command_velocity.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_command_velocity.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | LREAL FbReadCommandVelocity::getFloatValue() 28 | { 29 | return velocity_; 30 | } 31 | 32 | void FbReadCommandVelocity::runCycle() 33 | { 34 | if (enable_) 35 | { 36 | valid_ = true; 37 | if (axis_->getAxisState() == mcErrorStop) 38 | { 39 | velocity_ = 0; 40 | error_ = true; 41 | error_id_ = mcErrorCode_AxisErrorStop; 42 | return; 43 | } 44 | 45 | error_ = false; 46 | velocity_ = axis_->toUserVelCmd(); 47 | } 48 | else 49 | { 50 | valid_ = false; 51 | busy_ = false; 52 | error_ = false; 53 | } 54 | } 55 | 56 | } // namespace RTmotion 57 | -------------------------------------------------------------------------------- /src/fb/fb_read_status.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_read_status.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | void FbReadStatus::runCycle() 28 | { 29 | if (enable_) 30 | { 31 | valid_ = true; 32 | switch(axis_->getAxisState()) 33 | { 34 | case mcErrorStop: 35 | { 36 | error_stop_ = true; 37 | disabled_ = stopping_ = homing_ = standstill_ 38 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 39 | } 40 | break; 41 | case mcDisabled: 42 | { 43 | disabled_ = true; 44 | error_stop_ = stopping_ = homing_ = standstill_ 45 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 46 | } 47 | break; 48 | case mcStopping: 49 | { 50 | stopping_ = true; 51 | error_stop_ = disabled_ = homing_ = standstill_ 52 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 53 | } 54 | break; 55 | case mcHoming: 56 | { 57 | homing_ = true; 58 | error_stop_ = disabled_ = stopping_ = standstill_ 59 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 60 | } 61 | break; 62 | case mcStandstill: 63 | { 64 | standstill_ = true; 65 | error_stop_ = disabled_ = stopping_ = homing_ 66 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 67 | } 68 | break; 69 | case mcDiscreteMotion: 70 | { 71 | discrete_motion_ = true; 72 | error_stop_ = disabled_ = stopping_ = homing_ = standstill_ 73 | = continuous_motion_ = synchronized_motion_ = false; 74 | } 75 | break; 76 | case mcContinuousMotion: 77 | { 78 | continuous_motion_ = true; 79 | error_stop_ = disabled_ = stopping_ = homing_ = standstill_ 80 | = discrete_motion_ = synchronized_motion_ = false; 81 | } 82 | break; 83 | case mcSynchronizedMotion: 84 | { 85 | synchronized_motion_ = true; 86 | error_stop_ = disabled_ = stopping_ = homing_ = standstill_ 87 | = discrete_motion_ = continuous_motion_ = false; 88 | } 89 | break; 90 | default: 91 | break; 92 | } 93 | } 94 | else 95 | { 96 | valid_ = false; 97 | error_stop_ = disabled_ = stopping_ = homing_ = standstill_ 98 | = discrete_motion_ = continuous_motion_ = synchronized_motion_ = false; 99 | } 100 | } 101 | 102 | } // namespace RTmotion 103 | -------------------------------------------------------------------------------- /src/fb/fb_reset.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_reset.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbReset::FbReset() 28 | : enable_(false) 29 | , done_(false) 30 | , busy_(false) 31 | { 32 | } 33 | 34 | void FbReset::setExecute(BOOL execute) 35 | { 36 | enable_ = execute; 37 | busy_ = enable_ && !done_ ? true : false; 38 | done_ = enable_ ? done_ : false; 39 | } 40 | 41 | BOOL FbReset::isEnabled() 42 | { 43 | return enable_; 44 | } 45 | 46 | BOOL FbReset::isDone() 47 | { 48 | return done_; 49 | } 50 | 51 | BOOL FbReset::isBusy() 52 | { 53 | return busy_; 54 | } 55 | 56 | BOOL FbReset::isError() 57 | { 58 | return enable_ ? axis_->getAxisError() : mcErrorCodeGood; 59 | } 60 | 61 | MC_ERROR_CODE FbReset::getErrorID() 62 | { 63 | return enable_ ? axis_->getAxisError() : mcErrorCodeGood; 64 | } 65 | 66 | void FbReset::runCycle() 67 | { 68 | if (!enable_) 69 | return; 70 | 71 | if (!axis_->getAxisError()) 72 | { 73 | done_ = true; 74 | return; 75 | } 76 | 77 | DEBUG_PRINT("FbReset::runCycle, reset error\n"); 78 | axis_->resetError(enable_); 79 | 80 | if (axis_->powerTriggered() && axis_->powerOn()) 81 | if (axis_->getAxisState() == mcStandstill) 82 | done_ = true; 83 | 84 | if (!axis_->powerOn()) 85 | if (axis_->getAxisState() == mcDisabled) 86 | done_ = true; 87 | } 88 | 89 | } // namespace RTmotion 90 | -------------------------------------------------------------------------------- /src/fb/fb_stop.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_stop.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | FbStop::FbStop() 28 | { 29 | buffer_mode_ = mcAborting; 30 | } 31 | 32 | MC_ERROR_CODE FbStop::onRisingEdgeExecution() 33 | { 34 | axis_->addFBToQueue(this, mcStop); 35 | return mcErrorCodeGood; 36 | } 37 | 38 | MC_ERROR_CODE FbStop::onFallingEdgeExecution() 39 | { 40 | if (done_) 41 | return axis_->setAxisState(mcStandstill); 42 | 43 | return mcErrorCodeGood; 44 | } 45 | 46 | } // namespace RTmotion -------------------------------------------------------------------------------- /src/servo/servo.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file servo.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | 25 | namespace RTmotion 26 | { 27 | class Servo::ServoImpl 28 | { 29 | public: 30 | int32_t mSubmitPos = 0; 31 | int32_t mPos = 0; 32 | double mVel = 0; 33 | double mAcc = 0; 34 | }; 35 | 36 | Servo::Servo() 37 | { 38 | mImpl_ = new ServoImpl(); 39 | } 40 | 41 | Servo::~Servo() 42 | { 43 | delete mImpl_; 44 | } 45 | 46 | MC_ServoErrorCode Servo::setPower(bool powerStatus, bool& isDone) 47 | { 48 | isDone = true; 49 | return 0; 50 | } 51 | 52 | MC_ServoErrorCode Servo::setPos(int32_t pos) 53 | { 54 | mImpl_->mSubmitPos = pos; 55 | return 0; 56 | } 57 | 58 | MC_ServoErrorCode Servo::setVel(int32_t vel) 59 | { 60 | mImpl_->mVel = vel; 61 | return 0xFFFFFFFF; 62 | } 63 | 64 | MC_ServoErrorCode Servo::setTorque(double torque) 65 | { 66 | return 0xFFFFFFFF; 67 | } 68 | 69 | int32_t Servo::pos(void) 70 | { 71 | return mImpl_->mPos; 72 | } 73 | 74 | int32_t Servo::vel(void) 75 | { 76 | return mImpl_->mVel; 77 | } 78 | 79 | int32_t Servo::acc(void) 80 | { 81 | return mImpl_->mAcc; 82 | } 83 | 84 | double Servo::torque(void) 85 | { 86 | return 0; 87 | } 88 | 89 | bool Servo::readVal(int index, double& value) 90 | { 91 | return false; 92 | } 93 | 94 | bool Servo::writeVal(int index, double value) 95 | { 96 | return false; 97 | } 98 | 99 | MC_ServoErrorCode Servo::resetError(bool& isDone) 100 | { 101 | isDone = true; 102 | return 0; 103 | } 104 | 105 | void Servo::runCycle(double freq) 106 | { 107 | int32_t posDiff = mImpl_->mSubmitPos - mImpl_->mPos; 108 | double curVel = posDiff * freq; 109 | mImpl_->mAcc = (curVel - mImpl_->mVel) * freq; 110 | mImpl_->mVel = curVel; 111 | mImpl_->mPos = mImpl_->mSubmitPos; 112 | // printf("mImpl_->mSubmitPos: %d, mImpl_->mVel: %f, \n", mImpl_->mSubmitPos, mImpl_->mVel); 113 | } 114 | 115 | void Servo::emergStop(void) 116 | { 117 | mImpl_->mPos = mImpl_->mSubmitPos; 118 | mImpl_->mVel = mImpl_->mAcc = 0; 119 | } 120 | } // namespace RTmotion 121 | -------------------------------------------------------------------------------- /src/tool/fb_debug_tool.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file fb_debug_tool.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | # Find GTEST 2 | find_package(GTest REQUIRED) 3 | message(STATUS "GTEST_INCLUDE_DIRS: ${GTEST_INCLUDE_DIRS}") 4 | message(STATUS "GTEST_BOTH_LIBRARIES: ${GTEST_BOTH_LIBRARIES}") 5 | # Include gtest headers 6 | include_directories( 7 | ${GTEST_INCLUDE_DIRS} 8 | ) 9 | 10 | # Create scurve test executable 11 | add_executable(offline_scurve_test offline_scurve_test.cpp) 12 | target_link_libraries(offline_scurve_test ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES} ${PYTHON_LIBRARIES} -lpthread) 13 | 14 | # Create scurve test executable 15 | add_executable(online_scurve_test online_scurve_test.cpp) 16 | target_link_libraries(online_scurve_test ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES} ${PYTHON_LIBRARIES} ruckig::ruckig -lpthread) 17 | 18 | # Create planner test executable 19 | add_executable(planner_test planner_test.cpp) 20 | target_link_libraries(planner_test ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES} ${PYTHON_LIBRARIES} ruckig::ruckig -lpthread) 21 | 22 | # Create function block test executable 23 | add_executable(function_block_test function_block_test.cpp) 24 | target_link_libraries(function_block_test ${PROJECT_NAME} ${GTEST_BOTH_LIBRARIES} ${PYTHON_LIBRARIES} ruckig::ruckig -lpthread) 25 | 26 | # Install test executables 27 | install(TARGETS offline_scurve_test online_scurve_test planner_test function_block_test 28 | RUNTIME DESTINATION ${INSTALL_BINDIR} 29 | ) 30 | 31 | if(URANUS) 32 | # Find URANUS 33 | find_library( 34 | URANUS_LIBRARIES 35 | NAMES uranus 36 | HINTS ${URANUS_DIR}/build) 37 | if (NOT URANUS_LIBRARIES) 38 | message(ERROR " URANUS library not found under: ${URANUS_DIR}/build") 39 | endif() 40 | 41 | # Include uranus headers 42 | include_directories( 43 | ${URANUS_DIR}/install/include/motion 44 | ${URANUS_DIR}/install/include/fb 45 | ${URANUS_DIR}/install/include/misc 46 | ) 47 | 48 | # Create uranus test executable 49 | add_executable(uranus_test uranus_test.cpp) 50 | target_link_libraries(uranus_test ${PROJECT_NAME} ${URANUS_LIBRARIES} ${GTEST_LIBRARIES}) 51 | 52 | # Install uranus test executable 53 | install(TARGETS uranus_test 54 | RUNTIME DESTINATION ${INSTALL_BINDIR} 55 | ) 56 | endif(URANUS) -------------------------------------------------------------------------------- /test/get_clock_time_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | #include 5 | 6 | #define NSEC_PER_SEC (1000000000L) 7 | #define DIFF_NS(A, B) \ 8 | (((B).tv_sec - (A).tv_sec) * NSEC_PER_SEC + ((B).tv_nsec) - (A).tv_nsec) 9 | 10 | int main() 11 | { 12 | std::map time_stamps; 13 | struct timespec next_period, start_time, end_time; 14 | clock_gettime(CLOCK_MONOTONIC, &start_time); 15 | clock_gettime(CLOCK_MONOTONIC, &next_period); 16 | clock_gettime(CLOCK_MONOTONIC, &end_time); 17 | time_stamps.insert(std::make_pair("test", DIFF_NS(start_time, end_time))); 18 | clock_gettime(CLOCK_MONOTONIC, &end_time); 19 | ; 20 | printf("%d ns\n", DIFF_NS(start_time, end_time)); 21 | } -------------------------------------------------------------------------------- /test/offline_scurve_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file s_curve_test.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "gtest/gtest.h" 27 | 28 | #ifdef PLOT 29 | #include 30 | namespace plt = matplotlibcpp; 31 | #endif 32 | 33 | namespace traj_pro = trajectory_processing; 34 | 35 | // The fixture for testing class Foo. 36 | class ScurveTest : public ::testing::Test 37 | { 38 | protected: 39 | // You can remove any or all of the following functions if their bodies would 40 | // be empty. 41 | 42 | ScurveTest() 43 | { 44 | // You can do set-up work for each test here. 45 | } 46 | 47 | ~ScurveTest() override 48 | { 49 | // You can do clean-up work that doesn't throw exceptions here. 50 | } 51 | 52 | // If the constructor and destructor are not enough for setting up 53 | // and cleaning up each test, you can define the following methods: 54 | 55 | void SetUp() override 56 | { 57 | // Code here will be called immediately after the constructor (right 58 | // before each test). 59 | } 60 | 61 | void TearDown() override 62 | { 63 | // Code here will be called immediately after each test (right 64 | // before the destructor). 65 | } 66 | 67 | #ifdef PLOT 68 | void TrajectoryPlot(double delta_time, std::string file_name = "scurve.png") 69 | { 70 | double T = 71 | planner_.profile_.Ta + planner_.profile_.Tv + planner_.profile_.Td; 72 | double iter_t = 0; 73 | std::vector pos, vel, acc, jerk, t; 74 | while (iter_t < T) 75 | { 76 | Eigen::Vector4d point = planner_.getTrajectoryFunction(iter_t); 77 | pos.push_back(point[POSITION_ID]); 78 | vel.push_back(point[SPEED_ID]); 79 | acc.push_back(point[ACCELERATION_ID]); 80 | jerk.push_back(point[JERK_ID]); 81 | t.push_back(iter_t); 82 | iter_t += delta_time; 83 | } 84 | 85 | // Set the size of output image to 1280x720 pixels 86 | plt::figure_size(1280, 720); 87 | // Plot line from given t and pos. 88 | plt::subplot(4, 1, 1); 89 | plt::title("Scurve Figure"); 90 | plt::named_plot("Position", t, pos, "tab:blue"); 91 | plt::ylabel("Position"); 92 | // Plot line from given t and pos. 93 | plt::subplot(4, 1, 2); 94 | plt::named_plot("Velocity", t, vel, "tab:orange"); 95 | plt::ylabel("Velocity"); 96 | // Plot line from given t and pos. 97 | plt::subplot(4, 1, 3); 98 | plt::named_plot("Acceleration", t, acc, "tab:green"); 99 | plt::ylabel("Acceleration"); 100 | // Plot line from given t and pos. 101 | plt::subplot(4, 1, 4); 102 | plt::named_plot("Jerk", t, jerk, "tab:red"); 103 | plt::xlabel("time"); 104 | plt::ylabel("Jerk"); 105 | // Save the image (file format is determined by the extension) 106 | plt::save(file_name); 107 | } 108 | #endif 109 | 110 | traj_pro::ScurvePlannerOffLine planner_; 111 | }; 112 | 113 | // Tests example 3.9 at page 82 in 'Trajectory planning for automatic machines 114 | // and robots(2008)' Maximum speed reached 115 | TEST_F(ScurveTest, Example_3_9) 116 | { 117 | planner_.condition_.q0 = 0; 118 | planner_.condition_.q1 = 10; 119 | planner_.condition_.v0 = 1; 120 | planner_.condition_.v1 = 0; 121 | planner_.condition_.v_max = 5; 122 | planner_.condition_.a_max = 10; 123 | planner_.condition_.j_max = 30; 124 | 125 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory1D(); 126 | 127 | if (res != RTmotion::mcErrorCodeGood) 128 | printf("Failed to find scurve profile \n"); 129 | 130 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 131 | planner_.profile_.Ta, planner_.profile_.Tv, planner_.profile_.Td, 132 | planner_.profile_.Tj1, planner_.profile_.Tj2); 133 | ASSERT_LT(abs(planner_.profile_.Ta - 0.7333), 0.0001); 134 | ASSERT_LT(abs(planner_.profile_.Tv - 1.1433), 0.0001); 135 | ASSERT_LT(abs(planner_.profile_.Td - 0.8333), 0.0001); 136 | ASSERT_LT(abs(planner_.profile_.Tj1 - 0.3333), 0.0001); 137 | ASSERT_LT(abs(planner_.profile_.Tj2 - 0.3333), 0.0001); 138 | #ifdef PLOT 139 | TrajectoryPlot(0.001, "Example_3_9.png"); 140 | #endif 141 | } 142 | 143 | // Tests example 3.10 at page 83 of 'Trajectory planning for automatic machines 144 | // and robots(2008)' Maximum speed not reached 145 | TEST_F(ScurveTest, Example_3_10) 146 | { 147 | planner_.condition_.q0 = 0; 148 | planner_.condition_.q1 = 10; 149 | planner_.condition_.v0 = 1; 150 | planner_.condition_.v1 = 0; 151 | planner_.condition_.v_max = 10; 152 | planner_.condition_.a_max = 10; 153 | planner_.condition_.j_max = 30; 154 | 155 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory1D(); 156 | 157 | if (res != RTmotion::mcErrorCodeGood) 158 | printf("Failed to find scurve profile \n"); 159 | 160 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 161 | planner_.profile_.Ta, planner_.profile_.Tv, planner_.profile_.Td, 162 | planner_.profile_.Tj1, planner_.profile_.Tj2); 163 | ASSERT_LT(abs(planner_.profile_.Ta - 1.0747), 0.0001); 164 | ASSERT_LT(abs(planner_.profile_.Tv - 0.0), 0.0001); 165 | ASSERT_LT(abs(planner_.profile_.Td - 1.1747), 0.0001); 166 | ASSERT_LT(abs(planner_.profile_.Tj1 - 0.3333), 0.0001); 167 | ASSERT_LT(abs(planner_.profile_.Tj2 - 0.3333), 0.0001); 168 | 169 | Eigen::Vector4d point = planner_.getTrajectoryFunction(planner_.profile_.Ta); 170 | INFO_PRINT("Scurve profile: Vlim = %f \n", point[SPEED_ID]); 171 | ASSERT_LT(abs(point[SPEED_ID] - 8.4136), 0.0001); 172 | #ifdef PLOT 173 | TrajectoryPlot(0.001, "Example_3_10.png"); 174 | #endif 175 | } 176 | 177 | // Tests example 3.11 at page 83 of 'Trajectory planning for automatic machines 178 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 179 | // reached 180 | TEST_F(ScurveTest, Example_3_11) 181 | { 182 | planner_.condition_.q0 = 0; 183 | planner_.condition_.q1 = 10; 184 | planner_.condition_.v0 = 7; 185 | planner_.condition_.v1 = 0; 186 | planner_.condition_.v_max = 10; 187 | planner_.condition_.a_max = 10; 188 | planner_.condition_.j_max = 30; 189 | 190 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory1D(); 191 | 192 | if (res != RTmotion::mcErrorCodeGood) 193 | printf("Failed to find scurve profile \n"); 194 | 195 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 196 | planner_.profile_.Ta, planner_.profile_.Tv, planner_.profile_.Td, 197 | planner_.profile_.Tj1, planner_.profile_.Tj2); 198 | ASSERT_LT(abs(planner_.profile_.Ta - 0.4666), 0.0001); 199 | ASSERT_LT(abs(planner_.profile_.Tv - 0.0), 0.0001); 200 | ASSERT_LT(abs(planner_.profile_.Td - 1.4718), 0.0001); 201 | ASSERT_LT(abs(planner_.profile_.Tj1 - 0.2321), 0.0001); 202 | ASSERT_LT(abs(planner_.profile_.Tj2 - 0.2321), 0.0001); 203 | 204 | Eigen::Vector4d point = planner_.getTrajectoryFunction(planner_.profile_.Ta); 205 | INFO_PRINT("Scurve profile: Vlim = %f \n", point[SPEED_ID]); 206 | ASSERT_LT(abs(point[SPEED_ID] - 8.6329), 0.0001); 207 | 208 | point = planner_.getTrajectoryFunction(planner_.profile_.Tj1); 209 | INFO_PRINT("Scurve profile: Alim_a = %f \n", point[ACCELERATION_ID]); 210 | ASSERT_LT(abs(point[ACCELERATION_ID] - 6.9641), 0.0001); 211 | 212 | point = planner_.getTrajectoryFunction( 213 | planner_.profile_.Ta + planner_.profile_.Tv + planner_.profile_.Tj2); 214 | INFO_PRINT("Scurve profile: Alim_d = %f \n", point[ACCELERATION_ID]); 215 | ASSERT_LT(abs(point[ACCELERATION_ID] - (-6.9641)), 0.0001); 216 | #ifdef PLOT 217 | TrajectoryPlot(0.001, "Example_3_11.png"); 218 | #endif 219 | } 220 | 221 | // Tests example 3.12 at page 85 of 'Trajectory planning for automatic machines 222 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 223 | // reached 224 | TEST_F(ScurveTest, Example_3_12) 225 | { 226 | planner_.condition_.q0 = 0; 227 | planner_.condition_.q1 = 10; 228 | planner_.condition_.v0 = 7.5; 229 | planner_.condition_.v1 = 0; 230 | planner_.condition_.v_max = 10; 231 | planner_.condition_.a_max = 10; 232 | planner_.condition_.j_max = 30; 233 | 234 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory1D(); 235 | 236 | if (res != RTmotion::mcErrorCodeGood) 237 | printf("Failed to find scurve profile \n"); 238 | 239 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 240 | planner_.profile_.Ta, planner_.profile_.Tv, planner_.profile_.Td, 241 | planner_.profile_.Tj1, planner_.profile_.Tj2); 242 | ASSERT_LT(abs(planner_.profile_.Ta - 0.0), 0.0001); 243 | ASSERT_LT(abs(planner_.profile_.Tv - 0.0), 0.0001); 244 | ASSERT_LT(abs(planner_.profile_.Td - 2.6667), 0.0001); 245 | ASSERT_LT(abs(planner_.profile_.Tj1 - 0.0), 0.0001); 246 | ASSERT_LT(abs(planner_.profile_.Tj2 - 0.0973), 0.0001); 247 | 248 | Eigen::Vector4d point = planner_.getTrajectoryFunction(planner_.profile_.Ta); 249 | INFO_PRINT("Scurve profile: Vlim = %f \n", point[SPEED_ID]); 250 | ASSERT_LT(abs(point[SPEED_ID] - 7.5), 0.0001); 251 | 252 | point = planner_.getTrajectoryFunction(planner_.profile_.Tj1); 253 | INFO_PRINT("Scurve profile: Alim_a = %f \n", point[ACCELERATION_ID]); 254 | ASSERT_LT(abs(point[ACCELERATION_ID] - 0.0), 0.0001); 255 | 256 | point = planner_.getTrajectoryFunction( 257 | planner_.profile_.Ta + planner_.profile_.Tv + planner_.profile_.Tj2); 258 | INFO_PRINT("Scurve profile: Alim_d = %f \n", point[ACCELERATION_ID]); 259 | ASSERT_LT(abs(point[ACCELERATION_ID] - (-2.9190)), 0.0001); 260 | #ifdef PLOT 261 | TrajectoryPlot(0.001, "Example_3_12.png"); 262 | #endif 263 | } 264 | 265 | // Tests example 3.13 at page 93 of 'Trajectory planning for automatic machines 266 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 267 | // reached 268 | TEST_F(ScurveTest, Example_3_13) 269 | { 270 | planner_.condition_.q0 = 0; 271 | planner_.condition_.q1 = 10; 272 | planner_.condition_.v0 = 0; 273 | planner_.condition_.v1 = 0; 274 | planner_.condition_.v_max = 10; 275 | planner_.condition_.a_max = 20; 276 | planner_.condition_.j_max = 30; 277 | 278 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory1D(); 279 | 280 | if (res != RTmotion::mcErrorCodeGood) 281 | printf("Failed to find scurve profile \n"); 282 | 283 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 284 | planner_.profile_.Ta, planner_.profile_.Tv, planner_.profile_.Td, 285 | planner_.profile_.Tj1, planner_.profile_.Tj2); 286 | ASSERT_LT(abs(planner_.profile_.Ta - 1.1006), 0.0001); 287 | ASSERT_LT(abs(planner_.profile_.Tv - 0.0), 0.0001); 288 | ASSERT_LT(abs(planner_.profile_.Td - 1.1006), 0.0001); 289 | ASSERT_LE(planner_.profile_.Tj1 * 2, planner_.profile_.Ta); 290 | ASSERT_LT(planner_.profile_.Tj2 * 2, planner_.profile_.Td); 291 | 292 | Eigen::Vector4d point = planner_.getTrajectoryFunction(planner_.profile_.Ta); 293 | INFO_PRINT("Scurve profile: Vlim = %f \n", point[SPEED_ID]); 294 | ASSERT_LT(point[SPEED_ID], planner_.condition_.v_max); 295 | 296 | point = planner_.getTrajectoryFunction(planner_.profile_.Tj1); 297 | INFO_PRINT("Scurve profile: Alim_a = %f \n", point[ACCELERATION_ID]); 298 | ASSERT_LT(point[ACCELERATION_ID], planner_.condition_.a_max); 299 | 300 | point = planner_.getTrajectoryFunction( 301 | planner_.profile_.Ta + planner_.profile_.Tv + planner_.profile_.Tj2); 302 | INFO_PRINT("Scurve profile: Alim_d = %f \n", point[ACCELERATION_ID]); 303 | ASSERT_GT(point[ACCELERATION_ID], -planner_.condition_.a_max); 304 | #ifdef PLOT 305 | TrajectoryPlot(0.001, "Example_3_13.png"); 306 | #endif 307 | } 308 | 309 | int main(int argc, char** argv) 310 | { 311 | ::testing::InitGoogleTest(&argc, argv); 312 | return RUN_ALL_TESTS(); 313 | } 314 | -------------------------------------------------------------------------------- /test/planner_test.cpp: -------------------------------------------------------------------------------- 1 | /* 2 | * Copyright (c) 2020 Intel Corporation 3 | * Licensed under the Apache License, Version 2.0 (the "License"); 4 | * you may not use this file except in compliance with the License. 5 | * You may obtain a copy of the License at 6 | * 7 | * http://www.apache.org/licenses/LICENSE-2.0 8 | * 9 | * Unless required by applicable law or agreed to in writing, software 10 | * distributed under the License is distributed on an "AS IS" BASIS, 11 | * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | * See the License for the specific language governing permissions and 13 | * limitations under the License. 14 | */ 15 | 16 | /** 17 | * @file s_curve_test.cpp 18 | * 19 | * Maintainer: Yu Yan 20 | * 21 | */ 22 | 23 | #include 24 | #include 25 | #include 26 | #include "gtest/gtest.h" 27 | 28 | #ifdef PLOT 29 | #include 30 | namespace plt = matplotlibcpp; 31 | #endif 32 | 33 | namespace traj_pro = trajectory_processing; 34 | 35 | // The fixture for testing class Foo. 36 | class PlannerTest : public ::testing::Test 37 | { 38 | protected: 39 | // You can remove any or all of the following functions if their bodies would 40 | // be empty. 41 | 42 | PlannerTest() 43 | { 44 | // You can do set-up work for each test here. 45 | } 46 | 47 | ~PlannerTest() override 48 | { 49 | // You can do clean-up work that doesn't throw exceptions here. 50 | } 51 | 52 | // If the constructor and destructor are not enough for setting up 53 | // and cleaning up each test, you can define the following methods: 54 | 55 | void SetUp() override 56 | { 57 | // Code here will be called immediately after the constructor (right 58 | // before each test). 59 | } 60 | 61 | void TearDown() override 62 | { 63 | // Code here will be called immediately after each test (right 64 | // before the destructor). 65 | } 66 | 67 | #ifdef PLOT 68 | void TrajectoryPlot(double delta_time, double T = 10, 69 | std::string file_name = "scurve.png") 70 | { 71 | double iter_t = 0; 72 | std::vector pos, vel, acc, jerk, t; 73 | while (iter_t < T) 74 | { 75 | Eigen::Vector4d point = planner_.getTrajectoryPoint(iter_t); 76 | pos.push_back(point[POSITION_ID]); 77 | vel.push_back(point[SPEED_ID]); 78 | acc.push_back(point[ACCELERATION_ID]); 79 | jerk.push_back(point[JERK_ID]); 80 | t.push_back(iter_t); 81 | iter_t += delta_time; 82 | } 83 | 84 | // Set the size of output image to 1280x720 pixels 85 | plt::figure_size(1280, 720); 86 | // Plot line from given t and pos. 87 | plt::subplot(4, 1, 1); 88 | plt::title("Scurve Figure"); 89 | plt::named_plot("Position", t, pos, "tab:blue"); 90 | plt::ylabel("Position"); 91 | // Plot line from given t and pos. 92 | plt::subplot(4, 1, 2); 93 | plt::named_plot("Velocity", t, vel, "tab:orange"); 94 | plt::ylabel("Velocity"); 95 | // Plot line from given t and pos. 96 | plt::subplot(4, 1, 3); 97 | plt::named_plot("Acceleration", t, acc, "tab:green"); 98 | plt::ylabel("Acceleration"); 99 | // Plot line from given t and pos. 100 | plt::subplot(4, 1, 4); 101 | plt::named_plot("Jerk", t, jerk, "tab:red"); 102 | plt::xlabel("time"); 103 | plt::ylabel("Jerk"); 104 | // Save the image (file format is determined by the extension) 105 | plt::save(file_name); 106 | } 107 | #endif 108 | traj_pro::AxisPlanner planner_ = {1000}; 109 | }; 110 | 111 | // Tests velocity profile 112 | TEST_F(PlannerTest, VelocityUp) 113 | { 114 | planner_.setCondition(5, NAN, -1, 10, 5, 10, 30); 115 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 116 | 117 | if (res != RTmotion::mcErrorCodeGood) 118 | printf("Failed to find scurve profile \n"); 119 | 120 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 121 | planner_.getScurveProfile().Ta, planner_.getScurveProfile().Tv, 122 | planner_.getScurveProfile().Td, planner_.getScurveProfile().Tj1, 123 | planner_.getScurveProfile().Tj2); 124 | ASSERT_LT(abs(planner_.getScurveProfile().Ta - 1.4333), 0.0001); 125 | ASSERT_LT(abs(planner_.getScurveProfile().Tv - 0), 0.0001); 126 | ASSERT_LT(abs(planner_.getScurveProfile().Td - 0), 0.0001); 127 | ASSERT_LT(abs(planner_.getScurveProfile().Tj1 - 0.3333), 0.0001); 128 | ASSERT_LT(abs(planner_.getScurveProfile().Tj2 - 0), 0.0001); 129 | #ifdef PLOT 130 | TrajectoryPlot(0.001, 2, "VelocityUp.png"); 131 | #endif 132 | } 133 | 134 | // Tests velocity profile 135 | TEST_F(PlannerTest, VelocityDown) 136 | { 137 | planner_.setCondition(5, NAN, 10, -5, 5, 10, 30); 138 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 139 | 140 | if (res != RTmotion::mcErrorCodeGood) 141 | printf("Failed to find scurve profile \n"); 142 | 143 | INFO_PRINT("Scurve profile: Ta = %f, Tv = %f, Td = %f, Tj1 = %f, Tj2 = %f \n", 144 | planner_.getScurveProfile().Ta, planner_.getScurveProfile().Tv, 145 | planner_.getScurveProfile().Td, planner_.getScurveProfile().Tj1, 146 | planner_.getScurveProfile().Tj2); 147 | ASSERT_LT(abs(planner_.getScurveProfile().Ta - 1.8333), 0.0001); 148 | ASSERT_LT(abs(planner_.getScurveProfile().Tv - 0), 0.0001); 149 | ASSERT_LT(abs(planner_.getScurveProfile().Td - 0), 0.0001); 150 | ASSERT_LT(abs(planner_.getScurveProfile().Tj1 - 0.3333), 0.0001); 151 | ASSERT_LT(abs(planner_.getScurveProfile().Tj2 - 0), 0.0001); 152 | #ifdef PLOT 153 | TrajectoryPlot(0.001, 2, "VelocityDown.png"); 154 | #endif 155 | } 156 | 157 | // Tests example 3.9 at page 82 in 'Trajectory planning for automatic machines 158 | // and robots(2008)' Maximum speed reached 159 | TEST_F(PlannerTest, Example_3_9) 160 | { 161 | planner_.setCondition(0, 10, 1, 0, 5, 10, 30); 162 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 163 | 164 | if (res != RTmotion::mcErrorCodeGood) 165 | printf("Failed to find scurve profile \n"); 166 | 167 | #ifdef PLOT 168 | TrajectoryPlot(0.001, 3, "Example_3_9.png"); 169 | #endif 170 | } 171 | 172 | // Tests example 3.10 at page 83 of 'Trajectory planning for automatic machines 173 | // and robots(2008)' Maximum speed not reached 174 | TEST_F(PlannerTest, Example_3_10) 175 | { 176 | planner_.setCondition(0, 10, 1, 0, 10, 10, 30); 177 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 178 | 179 | if (res != RTmotion::mcErrorCodeGood) 180 | printf("Failed to find scurve profile \n"); 181 | 182 | #ifdef PLOT 183 | TrajectoryPlot(0.001, 3, "Example_3_10.png"); 184 | #endif 185 | } 186 | 187 | // Tests example 3.11 at page 83 of 'Trajectory planning for automatic machines 188 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 189 | // reached 190 | TEST_F(PlannerTest, Example_3_11) 191 | { 192 | planner_.setCondition(0, 10, 7, 0, 10, 10, 30); 193 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 194 | 195 | if (res != RTmotion::mcErrorCodeGood) 196 | printf("Failed to find scurve profile \n"); 197 | 198 | #ifdef PLOT 199 | TrajectoryPlot(0.001, 2, "Example_3_11.png"); 200 | #endif 201 | } 202 | 203 | // Tests example 3.12 at page 85 of 'Trajectory planning for automatic machines 204 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 205 | // reached 206 | TEST_F(PlannerTest, Example_3_12) 207 | { 208 | planner_.setCondition(0, 10, 7.5, 0, 10, 10, 30); 209 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 210 | 211 | if (res != RTmotion::mcErrorCodeGood) 212 | printf("Failed to find scurve profile \n"); 213 | 214 | #ifdef PLOT 215 | TrajectoryPlot(0.001, 2, "Example_3_12.png"); 216 | #endif 217 | } 218 | 219 | // Tests example 3.13 at page 93 of 'Trajectory planning for automatic machines 220 | // and robots(2008)' Maximum speed not reached and maximum acceleration not 221 | // reached 222 | TEST_F(PlannerTest, Example_3_13) 223 | { 224 | planner_.setCondition(0, 10, 0, 0, 10, 20, 30); 225 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 226 | 227 | if (res != RTmotion::mcErrorCodeGood) 228 | printf("Failed to find scurve profile \n"); 229 | 230 | #ifdef PLOT 231 | TrajectoryPlot(0.001, 2.5, "Example_3_13.png"); 232 | #endif 233 | } 234 | 235 | TEST_F(PlannerTest, Test_Negative) 236 | { 237 | planner_.setCondition(10, 0, 1, 0, 5, 10, 30); 238 | RTmotion::MC_ERROR_CODE res = planner_.planTrajectory(); 239 | 240 | if (res != RTmotion::mcErrorCodeGood) 241 | printf("Failed to find scurve profile \n"); 242 | 243 | #ifdef PLOT 244 | TrajectoryPlot(0.001, 3, "Test_Negative.png"); 245 | #endif 246 | } 247 | 248 | int main(int argc, char** argv) 249 | { 250 | ::testing::InitGoogleTest(&argc, argv); 251 | return RUN_ALL_TESTS(); 252 | } 253 | -------------------------------------------------------------------------------- /test/rdtsc_test.cpp: -------------------------------------------------------------------------------- 1 | #include 2 | #include 3 | #include 4 | 5 | #ifdef _WIN32 6 | #include 7 | #else 8 | #include 9 | #endif 10 | 11 | int main() 12 | { 13 | std::map time_stamps; 14 | uint64_t i; 15 | double d; 16 | i = __rdtsc(); 17 | __rdtsc(); 18 | time_stamps.insert(std::make_pair("test", (__rdtsc() - i) / 1.8)); 19 | d = (__rdtsc() - i) / 1.8; 20 | printf("%f ns\n", d); 21 | } --------------------------------------------------------------------------------