├── .gitignore ├── 3rdparty └── libfranka │ ├── LICENSE-APACHE │ ├── NOTICE │ ├── README.md │ ├── examples_common.cpp │ └── include │ └── examples_common.h ├── CMakeLists.txt ├── LICENSE ├── README.md ├── cmake └── orlConfig.cmake.in ├── doxygen └── Doxyfile ├── examples ├── CMakeLists.txt ├── b_spline_example.cpp ├── bezier_example.cpp ├── check_joint_states.cpp ├── circle_example.cpp ├── force_stop_example.cpp ├── gripper_control.cpp ├── impedance_example.cpp ├── pose_generators_example.cpp └── s_curve_example.cpp ├── include └── liborl │ ├── Frame.h │ ├── Payload.h │ ├── Pose.h │ ├── PoseGenerator.h │ ├── Robot.h │ ├── SCurve.h │ ├── SpeedProfile.h │ ├── StopCondition.h │ ├── enums.h │ ├── geometry_math.h │ └── liborl.h ├── src ├── Payload.cpp ├── Pose.cpp ├── PoseGenerator.cpp ├── QuinticPolynomial.hpp ├── Robot.cpp ├── SCurve.cpp ├── SpeedProfile.cpp ├── StopCondition.cpp └── geometry_math.cpp └── test ├── CMakeLists.txt └── test.cpp /.gitignore: -------------------------------------------------------------------------------- 1 | /doxygen/html/ 2 | /doxygen/latex/ 3 | -------------------------------------------------------------------------------- /3rdparty/libfranka/LICENSE-APACHE: -------------------------------------------------------------------------------- 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 | -------------------------------------------------------------------------------- /3rdparty/libfranka/NOTICE: -------------------------------------------------------------------------------- 1 | libfranka 2 | 3 | Copyright 2017 Franka Emika GmbH 4 | 5 | Licensed under the Apache License, Version 2.0 (the "License"); 6 | you may not use this file except in compliance with the License. 7 | You may obtain a copy of the License at 8 | 9 | http://www.apache.org/licenses/LICENSE-2.0 10 | 11 | Unless required by applicable law or agreed to in writing, software 12 | distributed under the License is distributed on an "AS IS" BASIS, 13 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 14 | See the License for the specific language governing permissions and 15 | limitations under the License. 16 | -------------------------------------------------------------------------------- /3rdparty/libfranka/README.md: -------------------------------------------------------------------------------- 1 | This folder includes copied source code from [libfranka](https://github.com/frankaemika/libfranka). 2 | Copyright (c) 2017 Franka Emika GmbH licensed under Apache version 2.0. See LICENSE-APACHE file -------------------------------------------------------------------------------- /3rdparty/libfranka/examples_common.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE-APACHE 3 | #include "examples_common.h" 4 | 5 | #include 6 | #include 7 | #include 8 | 9 | #include 10 | #include 11 | 12 | void setDefaultBehavior(franka::Robot &robot) { 13 | robot.setCollisionBehavior( 14 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 15 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, 16 | {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, 17 | {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); 18 | robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); 19 | robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); 20 | } 21 | 22 | MotionGenerator::MotionGenerator(double speed_factor, const std::array q_goal) 23 | : q_goal_(q_goal.data()) { 24 | dq_max_ *= speed_factor; 25 | ddq_max_start_ *= speed_factor; 26 | ddq_max_goal_ *= speed_factor; 27 | dq_max_sync_.setZero(); 28 | q_start_.setZero(); 29 | delta_q_.setZero(); 30 | t_1_sync_.setZero(); 31 | t_2_sync_.setZero(); 32 | t_f_sync_.setZero(); 33 | q_1_.setZero(); 34 | } 35 | 36 | bool MotionGenerator::calculateDesiredValues(double t, Vector7d *delta_q_d) const { 37 | Vector7i sign_delta_q; 38 | sign_delta_q << delta_q_.cwiseSign().cast(); 39 | Vector7d t_d = t_2_sync_ - t_1_sync_; 40 | Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_; 41 | std::array joint_motion_finished{}; 42 | 43 | for (size_t i = 0; i < 7; i++) { 44 | if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) { 45 | (*delta_q_d)[i] = 0; 46 | joint_motion_finished[i] = true; 47 | } else { 48 | if (t < t_1_sync_[i]) { 49 | (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * 50 | (0.5 * t - t_1_sync_[i]) * std::pow(t, 3.0); 51 | } else if (t >= t_1_sync_[i] && t < t_2_sync_[i]) { 52 | (*delta_q_d)[i] = q_1_[i] + (t - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i]; 53 | } else if (t >= t_2_sync_[i] && t < t_f_sync_[i]) { 54 | (*delta_q_d)[i] = 55 | delta_q_[i] + 0.5 * 56 | (1.0 / std::pow(delta_t_2_sync[i], 3.0) * 57 | (t - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) * 58 | std::pow((t - t_1_sync_[i] - t_d[i]), 3.0) + 59 | (2.0 * t - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) * 60 | dq_max_sync_[i] * sign_delta_q[i]; 61 | } else { 62 | (*delta_q_d)[i] = delta_q_[i]; 63 | joint_motion_finished[i] = true; 64 | } 65 | } 66 | } 67 | return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), 68 | [](bool x) { return x; }); 69 | } 70 | 71 | void MotionGenerator::calculateSynchronizedValues() { 72 | Vector7d dq_max_reach(dq_max_); 73 | Vector7d t_f = Vector7d::Zero(); 74 | Vector7d delta_t_2 = Vector7d::Zero(); 75 | Vector7d t_1 = Vector7d::Zero(); 76 | Vector7d delta_t_2_sync = Vector7d::Zero(); 77 | Vector7i sign_delta_q; 78 | sign_delta_q << delta_q_.cwiseSign().cast(); 79 | 80 | for (size_t i = 0; i < 7; i++) { 81 | if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { 82 | if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) + 83 | 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) { 84 | dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] * 85 | (ddq_max_start_[i] * ddq_max_goal_[i]) / 86 | (ddq_max_start_[i] + ddq_max_goal_[i])); 87 | } 88 | t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i]; 89 | delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i]; 90 | t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i]; 91 | } 92 | } 93 | double max_t_f = t_f.maxCoeff(); 94 | for (size_t i = 0; i < 7; i++) { 95 | if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { 96 | double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]); 97 | double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i]; 98 | double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i]; 99 | double delta = b * b - 4.0 * a * c; 100 | if (delta < 0.0) { 101 | delta = 0.0; 102 | } 103 | dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); 104 | t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i]; 105 | delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i]; 106 | t_f_sync_[i] = 107 | (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]); 108 | t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i]; 109 | q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]); 110 | } 111 | } 112 | } 113 | 114 | franka::JointPositions MotionGenerator::operator()(const franka::RobotState &robot_state, 115 | franka::Duration period) { 116 | time_ += period.toSec(); 117 | 118 | if (time_ == 0.0) { 119 | q_start_ = Vector7d(robot_state.q_d.data()); 120 | delta_q_ = q_goal_ - q_start_; 121 | calculateSynchronizedValues(); 122 | } 123 | 124 | Vector7d delta_q_d; 125 | bool motion_finished = calculateDesiredValues(time_, &delta_q_d); 126 | 127 | std::array joint_positions; 128 | Eigen::VectorXd::Map(&joint_positions[0], 7) = (q_start_ + delta_q_d); 129 | franka::JointPositions output(joint_positions); 130 | output.motion_finished = motion_finished; 131 | return output; 132 | } -------------------------------------------------------------------------------- /3rdparty/libfranka/include/examples_common.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2017 Franka Emika GmbH 2 | // Use of this source code is governed by the Apache-2.0 license, see LICENSE-APACHE 3 | #pragma once 4 | 5 | #include 6 | 7 | #include 8 | 9 | #include 10 | #include 11 | #include 12 | #include 13 | 14 | /** 15 | * @file examples_common.h 16 | * Contains common types and functions for the examples. 17 | */ 18 | 19 | /** 20 | * Sets a default collision behavior, joint impedance, Cartesian impedance, and filter frequency. 21 | * 22 | * @param[in] robot Robot instance to set behavior on. 23 | */ 24 | void setDefaultBehavior(franka::Robot &robot); 25 | 26 | /** 27 | * An example showing how to generate a joint pose motion to a goal position. Adapted from: 28 | * Wisama Khalil and Etienne Dombre. 2002. Modeling, Identification and Control of Robots 29 | * (Kogan Page Science Paper edition). 30 | */ 31 | class MotionGenerator { 32 | public: 33 | /** 34 | * Creates a new MotionGenerator instance for a target q. 35 | * 36 | * @param[in] speed_factor General speed factor in range [0, 1]. 37 | * @param[in] q_goal Target joint positions. 38 | */ 39 | MotionGenerator(double speed_factor, const std::array q_goal); 40 | 41 | /** 42 | * Sends joint position calculations 43 | * 44 | * @param[in] robot_state Current state of the robot. 45 | * @param[in] period Duration of execution. 46 | * 47 | * @return Joint positions for use inside a control loop. 48 | */ 49 | franka::JointPositions operator()(const franka::RobotState &robot_state, franka::Duration period); 50 | 51 | private: 52 | using Vector7d = Eigen::Matrix; 53 | using Vector7i = Eigen::Matrix; 54 | 55 | bool calculateDesiredValues(double t, Vector7d *delta_q_d) const; 56 | 57 | void calculateSynchronizedValues(); 58 | 59 | static constexpr double kDeltaQMotionFinished = 1e-6; 60 | const Vector7d q_goal_; 61 | 62 | Vector7d q_start_; 63 | Vector7d delta_q_; 64 | 65 | Vector7d dq_max_sync_; 66 | Vector7d t_1_sync_; 67 | Vector7d t_2_sync_; 68 | Vector7d t_f_sync_; 69 | Vector7d q_1_; 70 | 71 | double time_ = 0.0; 72 | 73 | Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); 74 | Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 75 | Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); 76 | }; 77 | 78 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5.1) 2 | 3 | project(liborl VERSION 0.3.0) 4 | include(CTest) 5 | 6 | add_compile_options(-fPIC) 7 | 8 | add_compile_options(-Wall -Wextra) 9 | find_package(Eigen3 REQUIRED) 10 | 11 | find_package(Franka REQUIRED) 12 | message("libfranka version: ${Franka_VERSION}") 13 | 14 | add_library(orl SHARED 15 | src/Robot.cpp 16 | 3rdparty/libfranka/examples_common.cpp 17 | src/Pose.cpp src/geometry_math.cpp include/liborl/geometry_math.h src/PoseGenerator.cpp src/Payload.cpp src/StopCondition.cpp include/liborl/StopCondition.h src/SpeedProfile.cpp include/liborl/SpeedProfile.h include/liborl/Frame.h src/SCurve.cpp include/liborl/SCurve.h include/liborl/enums.h) 18 | set_target_properties(orl PROPERTIES VERSION ${PROJECT_VERSION} 19 | ) 20 | target_include_directories(orl 21 | PUBLIC 22 | $ 23 | $ 24 | $ 25 | ) 26 | 27 | target_link_libraries(orl PUBLIC 28 | Franka::Franka 29 | ) 30 | include(GNUInstallDirs) 31 | set(INSTALL_CONFIGDIR ${CMAKE_INSTALL_LIBDIR}/cmake/orl) 32 | install(DIRECTORY include/liborl/ 33 | DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/liborl 34 | FILES_MATCHING PATTERN "*.h*" 35 | ) 36 | 37 | install(DIRECTORY include/ DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}) 38 | 39 | install(TARGETS orl 40 | EXPORT orlTargets 41 | LIBRARY DESTINATION lib 42 | ARCHIVE DESTINATION lib 43 | RUNTIME DESTINATION bin 44 | INCLUDES DESTINATION include 45 | ) 46 | include(CMakePackageConfigHelpers) 47 | write_basic_package_version_file( 48 | orlConfigVersion.cmake 49 | VERSION ${PACKAGE_VERSION} 50 | COMPATIBILITY AnyNewerVersion 51 | ) 52 | configure_package_config_file(cmake/orlConfig.cmake.in 53 | ${CMAKE_CURRENT_BINARY_DIR}/orlConfig.cmake 54 | INSTALL_DESTINATION ${INSTALL_CONFIGDIR}) 55 | install(FILES "${CMAKE_CURRENT_BINARY_DIR}/orlConfig.cmake" 56 | "${CMAKE_CURRENT_BINARY_DIR}/orlConfigVersion.cmake" 57 | DESTINATION lib/cmake/orl 58 | ) 59 | install(EXPORT orlTargets 60 | FILE orlTargets.cmake 61 | NAMESPACE orl:: 62 | DESTINATION lib/cmake/orl 63 | ) 64 | add_subdirectory(examples) 65 | if (BUILD_TESTING) 66 | enable_testing() 67 | add_subdirectory(test) 68 | endif () 69 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | EUROPEAN UNION PUBLIC LICENCE v. 1.2 2 | EUPL © the European Union 2007, 2016 3 | 4 | This European Union Public Licence (the ‘EUPL’) applies to the Work (as defined 5 | below) which is provided under the terms of this Licence. Any use of the Work, 6 | other than as authorised under this Licence is prohibited (to the extent such 7 | use is covered by a right of the copyright holder of the Work). 8 | 9 | The Work is provided under the terms of this Licence when the Licensor (as 10 | defined below) has placed the following notice immediately following the 11 | copyright notice for the Work: 12 | 13 | Licensed under the EUPL 14 | 15 | or has expressed by any other means his willingness to license under the EUPL. 16 | 17 | 1. Definitions 18 | 19 | In this Licence, the following terms have the following meaning: 20 | 21 | - ‘The Licence’: this Licence. 22 | 23 | - ‘The Original Work’: the work or software distributed or communicated by the 24 | Licensor under this Licence, available as Source Code and also as Executable 25 | Code as the case may be. 26 | 27 | - ‘Derivative Works’: the works or software that could be created by the 28 | Licensee, based upon the Original Work or modifications thereof. This Licence 29 | does not define the extent of modification or dependence on the Original Work 30 | required in order to classify a work as a Derivative Work; this extent is 31 | determined by copyright law applicable in the country mentioned in Article 15. 32 | 33 | - ‘The Work’: the Original Work or its Derivative Works. 34 | 35 | - ‘The Source Code’: the human-readable form of the Work which is the most 36 | convenient for people to study and modify. 37 | 38 | - ‘The Executable Code’: any code which has generally been compiled and which is 39 | meant to be interpreted by a computer as a program. 40 | 41 | - ‘The Licensor’: the natural or legal person that distributes or communicates 42 | the Work under the Licence. 43 | 44 | - ‘Contributor(s)’: any natural or legal person who modifies the Work under the 45 | Licence, or otherwise contributes to the creation of a Derivative Work. 46 | 47 | - ‘The Licensee’ or ‘You’: any natural or legal person who makes any usage of 48 | the Work under the terms of the Licence. 49 | 50 | - ‘Distribution’ or ‘Communication’: any act of selling, giving, lending, 51 | renting, distributing, communicating, transmitting, or otherwise making 52 | available, online or offline, copies of the Work or providing access to its 53 | essential functionalities at the disposal of any other natural or legal 54 | person. 55 | 56 | 2. Scope of the rights granted by the Licence 57 | 58 | The Licensor hereby grants You a worldwide, royalty-free, non-exclusive, 59 | sublicensable licence to do the following, for the duration of copyright vested 60 | in the Original Work: 61 | 62 | - use the Work in any circumstance and for all usage, 63 | - reproduce the Work, 64 | - modify the Work, and make Derivative Works based upon the Work, 65 | - communicate to the public, including the right to make available or display 66 | the Work or copies thereof to the public and perform publicly, as the case may 67 | be, the Work, 68 | - distribute the Work or copies thereof, 69 | - lend and rent the Work or copies thereof, 70 | - sublicense rights in the Work or copies thereof. 71 | 72 | Those rights can be exercised on any media, supports and formats, whether now 73 | known or later invented, as far as the applicable law permits so. 74 | 75 | In the countries where moral rights apply, the Licensor waives his right to 76 | exercise his moral right to the extent allowed by law in order to make effective 77 | the licence of the economic rights here above listed. 78 | 79 | The Licensor grants to the Licensee royalty-free, non-exclusive usage rights to 80 | any patents held by the Licensor, to the extent necessary to make use of the 81 | rights granted on the Work under this Licence. 82 | 83 | 3. Communication of the Source Code 84 | 85 | The Licensor may provide the Work either in its Source Code form, or as 86 | Executable Code. If the Work is provided as Executable Code, the Licensor 87 | provides in addition a machine-readable copy of the Source Code of the Work 88 | along with each copy of the Work that the Licensor distributes or indicates, in 89 | a notice following the copyright notice attached to the Work, a repository where 90 | the Source Code is easily and freely accessible for as long as the Licensor 91 | continues to distribute or communicate the Work. 92 | 93 | 4. Limitations on copyright 94 | 95 | Nothing in this Licence is intended to deprive the Licensee of the benefits from 96 | any exception or limitation to the exclusive rights of the rights owners in the 97 | Work, of the exhaustion of those rights or of other applicable limitations 98 | thereto. 99 | 100 | 5. Obligations of the Licensee 101 | 102 | The grant of the rights mentioned above is subject to some restrictions and 103 | obligations imposed on the Licensee. Those obligations are the following: 104 | 105 | Attribution right: The Licensee shall keep intact all copyright, patent or 106 | trademarks notices and all notices that refer to the Licence and to the 107 | disclaimer of warranties. The Licensee must include a copy of such notices and a 108 | copy of the Licence with every copy of the Work he/she distributes or 109 | communicates. The Licensee must cause any Derivative Work to carry prominent 110 | notices stating that the Work has been modified and the date of modification. 111 | 112 | Copyleft clause: If the Licensee distributes or communicates copies of the 113 | Original Works or Derivative Works, this Distribution or Communication will be 114 | done under the terms of this Licence or of a later version of this Licence 115 | unless the Original Work is expressly distributed only under this version of the 116 | Licence — for example by communicating ‘EUPL v. 1.2 only’. The Licensee 117 | (becoming Licensor) cannot offer or impose any additional terms or conditions on 118 | the Work or Derivative Work that alter or restrict the terms of the Licence. 119 | 120 | Compatibility clause: If the Licensee Distributes or Communicates Derivative 121 | Works or copies thereof based upon both the Work and another work licensed under 122 | a Compatible Licence, this Distribution or Communication can be done under the 123 | terms of this Compatible Licence. For the sake of this clause, ‘Compatible 124 | Licence’ refers to the licences listed in the appendix attached to this Licence. 125 | Should the Licensee's obligations under the Compatible Licence conflict with 126 | his/her obligations under this Licence, the obligations of the Compatible 127 | Licence shall prevail. 128 | 129 | Provision of Source Code: When distributing or communicating copies of the Work, 130 | the Licensee will provide a machine-readable copy of the Source Code or indicate 131 | a repository where this Source will be easily and freely available for as long 132 | as the Licensee continues to distribute or communicate the Work. 133 | 134 | Legal Protection: This Licence does not grant permission to use the trade names, 135 | trademarks, service marks, or names of the Licensor, except as required for 136 | reasonable and customary use in describing the origin of the Work and 137 | reproducing the content of the copyright notice. 138 | 139 | 6. Chain of Authorship 140 | 141 | The original Licensor warrants that the copyright in the Original Work granted 142 | hereunder is owned by him/her or licensed to him/her and that he/she has the 143 | power and authority to grant the Licence. 144 | 145 | Each Contributor warrants that the copyright in the modifications he/she brings 146 | to the Work are owned by him/her or licensed to him/her and that he/she has the 147 | power and authority to grant the Licence. 148 | 149 | Each time You accept the Licence, the original Licensor and subsequent 150 | Contributors grant You a licence to their contributions to the Work, under the 151 | terms of this Licence. 152 | 153 | 7. Disclaimer of Warranty 154 | 155 | The Work is a work in progress, which is continuously improved by numerous 156 | Contributors. It is not a finished work and may therefore contain defects or 157 | ‘bugs’ inherent to this type of development. 158 | 159 | For the above reason, the Work is provided under the Licence on an ‘as is’ basis 160 | and without warranties of any kind concerning the Work, including without 161 | limitation merchantability, fitness for a particular purpose, absence of defects 162 | or errors, accuracy, non-infringement of intellectual property rights other than 163 | copyright as stated in Article 6 of this Licence. 164 | 165 | This disclaimer of warranty is an essential part of the Licence and a condition 166 | for the grant of any rights to the Work. 167 | 168 | 8. Disclaimer of Liability 169 | 170 | Except in the cases of wilful misconduct or damages directly caused to natural 171 | persons, the Licensor will in no event be liable for any direct or indirect, 172 | material or moral, damages of any kind, arising out of the Licence or of the use 173 | of the Work, including without limitation, damages for loss of goodwill, work 174 | stoppage, computer failure or malfunction, loss of data or any commercial 175 | damage, even if the Licensor has been advised of the possibility of such damage. 176 | However, the Licensor will be liable under statutory product liability laws as 177 | far such laws apply to the Work. 178 | 179 | 9. Additional agreements 180 | 181 | While distributing the Work, You may choose to conclude an additional agreement, 182 | defining obligations or services consistent with this Licence. However, if 183 | accepting obligations, You may act only on your own behalf and on your sole 184 | responsibility, not on behalf of the original Licensor or any other Contributor, 185 | and only if You agree to indemnify, defend, and hold each Contributor harmless 186 | for any liability incurred by, or claims asserted against such Contributor by 187 | the fact You have accepted any warranty or additional liability. 188 | 189 | 10. Acceptance of the Licence 190 | 191 | The provisions of this Licence can be accepted by clicking on an icon ‘I agree’ 192 | placed under the bottom of a window displaying the text of this Licence or by 193 | affirming consent in any other similar way, in accordance with the rules of 194 | applicable law. Clicking on that icon indicates your clear and irrevocable 195 | acceptance of this Licence and all of its terms and conditions. 196 | 197 | Similarly, you irrevocably accept this Licence and all of its terms and 198 | conditions by exercising any rights granted to You by Article 2 of this Licence, 199 | such as the use of the Work, the creation by You of a Derivative Work or the 200 | Distribution or Communication by You of the Work or copies thereof. 201 | 202 | 11. Information to the public 203 | 204 | In case of any Distribution or Communication of the Work by means of electronic 205 | communication by You (for example, by offering to download the Work from a 206 | remote location) the distribution channel or media (for example, a website) must 207 | at least provide to the public the information requested by the applicable law 208 | regarding the Licensor, the Licence and the way it may be accessible, concluded, 209 | stored and reproduced by the Licensee. 210 | 211 | 12. Termination of the Licence 212 | 213 | The Licence and the rights granted hereunder will terminate automatically upon 214 | any breach by the Licensee of the terms of the Licence. 215 | 216 | Such a termination will not terminate the licences of any person who has 217 | received the Work from the Licensee under the Licence, provided such persons 218 | remain in full compliance with the Licence. 219 | 220 | 13. Miscellaneous 221 | 222 | Without prejudice of Article 9 above, the Licence represents the complete 223 | agreement between the Parties as to the Work. 224 | 225 | If any provision of the Licence is invalid or unenforceable under applicable 226 | law, this will not affect the validity or enforceability of the Licence as a 227 | whole. Such provision will be construed or reformed so as necessary to make it 228 | valid and enforceable. 229 | 230 | The European Commission may publish other linguistic versions or new versions of 231 | this Licence or updated versions of the Appendix, so far this is required and 232 | reasonable, without reducing the scope of the rights granted by the Licence. New 233 | versions of the Licence will be published with a unique version number. 234 | 235 | All linguistic versions of this Licence, approved by the European Commission, 236 | have identical value. Parties can take advantage of the linguistic version of 237 | their choice. 238 | 239 | 14. Jurisdiction 240 | 241 | Without prejudice to specific agreement between parties, 242 | 243 | - any litigation resulting from the interpretation of this License, arising 244 | between the European Union institutions, bodies, offices or agencies, as a 245 | Licensor, and any Licensee, will be subject to the jurisdiction of the Court 246 | of Justice of the European Union, as laid down in article 272 of the Treaty on 247 | the Functioning of the European Union, 248 | 249 | - any litigation arising between other parties and resulting from the 250 | interpretation of this License, will be subject to the exclusive jurisdiction 251 | of the competent court where the Licensor resides or conducts its primary 252 | business. 253 | 254 | 15. Applicable Law 255 | 256 | Without prejudice to specific agreement between parties, 257 | 258 | - this Licence shall be governed by the law of the European Union Member State 259 | where the Licensor has his seat, resides or has his registered office, 260 | 261 | - this licence shall be governed by Belgian law if the Licensor has no seat, 262 | residence or registered office inside a European Union Member State. 263 | 264 | Appendix 265 | 266 | ‘Compatible Licences’ according to Article 5 EUPL are: 267 | 268 | - GNU General Public License (GPL) v. 2, v. 3 269 | - GNU Affero General Public License (AGPL) v. 3 270 | - Open Software License (OSL) v. 2.1, v. 3.0 271 | - Eclipse Public License (EPL) v. 1.0 272 | - CeCILL v. 2.0, v. 2.1 273 | - Mozilla Public Licence (MPL) v. 2 274 | - GNU Lesser General Public Licence (LGPL) v. 2.1, v. 3 275 | - Creative Commons Attribution-ShareAlike v. 3.0 Unported (CC BY-SA 3.0) for 276 | works other than software 277 | - European Union Public Licence (EUPL) v. 1.1, v. 1.2 278 | - Québec Free and Open-Source Licence — Reciprocity (LiLiQ-R) or Strong 279 | Reciprocity (LiLiQ-R+). 280 | 281 | The European Commission may update this Appendix to later versions of the above 282 | licences without producing a new version of the EUPL, as long as they provide 283 | the rights granted in Article 2 of this Licence and protect the covered Source 284 | Code from exclusive appropriation. 285 | 286 | All other changes or additions to this Appendix require the production of a new 287 | EUPL version. 288 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # LibORL 2 | 3 | LibORL is a high level motion planning library for [Franka Emika](https://www.franka.de/) Panda robots. It is a wrapper around [libfranka](https://github.com/frankaemika/libfranka) which provides 4 | simple to use motion functionality in cartesian space. 5 | 6 | LibORL was created during the Open-Robotics-Lab (therefore the name ORL) at the [Institute for Robotics and Process Control (IRP)](https://www.rob.cs.tu-bs.de/) 7 | at TU Braunschweig. The name is probably not the best but "orl" makes a nice C++ namespace :) 8 | 9 | ## Requirements 10 | * [libfranka](https://frankaemika.github.io/docs/installation_linux.html) 11 | * Eigen3 12 | 13 | ## Installation 14 | 15 | ```bash 16 | mkdir build 17 | cd build 18 | cmake .. -DCMAKE_BUILD_TYPE=Release 19 | cmake --build . 20 | sudo make install 21 | ``` 22 | after that you can use it in your own CMake project like this: 23 | ```cmake 24 | find_package(orl REQUIRED) 25 | add_executable(your_target main.cpp) 26 | target_link_libraries(your_target orl::orl) 27 | ``` 28 | 29 | ## Generate Documentation 30 | ```bash 31 | cd doxygen 32 | doxygen Doxyfile 33 | firefox html/index.html 34 | ``` 35 | ## Features 36 | LibORL is a library with which you can easily generate cartesian motions 37 | and execute them on the robot. 38 | 39 | These are the key features of this library: 40 | * Easy trajectory generation in cartesian space + optional elbow control 41 | * Impedance mode which can follow an moving attractor Pose 42 | * Different kinds of speed profiles 43 | * Trajectory generation using B-Splines, Bezier-Curves or circles 44 | * Abort movements when there is too much force applied on the end-effector 45 | * Move to specific joint configurations 46 | * Usage of the Gripper 47 | 48 | The basic workflow is like this; 49 | * Define a PoseGenerator (a function which gets a progress of the movement, the initial pose of the robot and the current robot state and returns a pose) 50 | * Apply a SpeedProfile (you can use a QuinticPolynomial a Cosine function or an S-Curve Speed Profile) 51 | * (optional) create a StopCondition. For example you can say that the robot abort the motion if there is an external force pushing the robot. Be careful with this when you open the gripper afterwards! Look at the docs. 52 | * Let the robot execute the PoseGenerator by providing a duration for the motion. 53 | 54 | ## Showcase 55 | [![Bartender Robots](https://img.youtube.com/vi/POVh1aRzogc/0.jpg)](https://www.youtube.com/watch?v=POVh1aRzogc) 56 | 57 | LibORL was used in a beer pouring task to execute the motions. You can find out more about this project in this [blog post](https://pouya-moh.com/bartender-robots). 58 | ## Intro 59 | Consider the following example which moves the robot to the position (0.5,0,0.3) within 2 seconds 60 | ```cpp 61 | using namespace orl; 62 | const double execution_time = 2.0; 63 | Robot franka("franka"); // IP-Address or hostname of the robot 64 | auto pose_generator = PoseGenerators::AbsoluteMotion({0.5,0,0.3}); 65 | apply_speed_profile(pose_generator, SpeedProfiles::QuinticPolynomialProfile()); 66 | franka.move_cartesian(pose_generator, execution_time); 67 | ``` 68 | a simplified version of this is 69 | ```cpp 70 | using namespace orl; 71 | const double execution_time = 2.0; 72 | Robot franka("franka"); // IP-Address or hostname of the robot 73 | franka.absolute_cart_motion({0.5,0,0.3}, execution_time); 74 | ``` 75 | 76 | Ok but what about Orientations? For that you have to define a Pose. There are multiple ways. One way is to set a Position 77 | and an Orientation. We initialize the orientation with roll, pitch, yaw values 78 | ```cpp 79 | using namespace orl; 80 | const double execution_time = 2.0; 81 | Robot franka("franka"); // IP-Address or hostname of the robot 82 | Pose goal_pose({0,2,3}, {-M_PI,0,0}); 83 | auto pose_generator = PoseGenerators::MoveToPose(goal_pose); 84 | apply_speed_profile(pose_generator, SpeedProfiles::QuinticPolynomialProfile()); 85 | franka.move_cartesian(pose_generator, execution_time); 86 | ``` 87 | or simplified 88 | ```cpp 89 | using namespace orl; 90 | const double execution_time = 2.0; 91 | Robot franka("franka"); // IP-Address or hostname of the robot 92 | Pose goal_pose({0,2,3}, {-M_PI,0,0}); 93 | franka.move_cartesian(goal_pose, execution_time); 94 | 95 | ``` 96 | 97 | But can we calculate Position and Orientation separately? 98 | 99 | Yes. you can compose a PoseGenerator by combining a PositionGenerator and an OrientationGenerator. So lets define an OrientationGenerator which does not change the orientation at all: 100 | ```cpp 101 | OrientationGenerator constant_orientation_generator = [=](const PoseGeneratorInput &input) -> Eigen::Quaterniond { 102 | return input.initial_pose.quaternion(); 103 | }; 104 | ``` 105 | We can see that OrientationGenerators are just pure functions, just like Like Position- and PoseGenerators. 106 | So our OrientationGenerator takes the PoseGeneratorInput and returns the Quaternion of the initial pose. 107 | 108 | 109 | Let us also define a PositionGenerator which does nothing: 110 | ```cpp 111 | PositionGenerator constant_position_generator = [=](const PoseGeneratorInput &input) -> Eigen::Vector3d { 112 | return input.initial_pose.getPosition(); 113 | }; 114 | ``` 115 | When you calculate a PositionGenerator you should try to provide equidistant points. That means if you take the distance 116 | from the points that you get by taking inputs like [0,0.01,0.02,...,0.99,1] that all output points should have the same distance from their neighbors. 117 | Sadly, this is not always true in our library as the Bezier and B-Spline Motions do not have this property and that means that some parts of the movement are executed faster than others. 118 | 119 | So now that we have Position and OrientationGenerators we can generate our PoseGenerator and directly apply a SpeedProfile 120 | 121 | ```cpp 122 | PoseGenerator constant_pose_generator = generate_pose_generator(constant_position_generator,constant_orientation_generator,SpeedProfiles::QuinticPolynomialProfile()) 123 | ``` 124 | after that we can send our PoseGenerator to the robot and give him 5 seconds to execute the PoseGenerator 125 | ```cpp 126 | franka.move_cartesian(constant_pose_generator,5) 127 | ``` 128 | 129 | Of course the robot does nothing. But there are a set of Position-/OrientationGenerators which you can use. Look into the PoseGenerator.h 130 | 131 | There are also a set of PoseGenerators in orl::PoseGenerators namespace. Lets look at a simple one: 132 | 133 | ```cpp 134 | PoseGenerator RelativeMotion(const Position &translation, Frame frame = Frame::RobotBase, 135 | const boost::optional &maybe_orientation_generator = boost::none); 136 | ``` 137 | This PoseGenerator describes a Relative Movement. We already used this one in the first example. But what about the other Parameters? 138 | We have 3 different Frames. The most important one is the RobotBase Frame which describes a the Pose of the End-Effector with respect to the RobotBase. 139 | There is also the UnitFrame which describes the motion with respect to a zero translation and rotation. As the RelativeMotion only changes the position you can give it a custom 140 | OrientationGenerator. If you do not provide an OrientationGenerator the orientation will stay the same. 141 | 142 | Lets define a pose_generator which moves the from the current position of the robot 50 cm in x direction 143 | ```cpp 144 | PoseGenerator pose_generator_robot = PoseGenerators::RelativeMotion({0.5,0,0}, Frame::RobotBase); 145 | ``` 146 | And now we define one which moves from the Unit-Frame to 0.3 meter in z-direction 147 | ```cpp 148 | PoseGenerator pose_generator_unit = PoseGenerators::RelativeMotion({0,0,0.3}, Frame::UnitFrame); 149 | ``` 150 | You can think of each Pose as homogenous matrix and you can multiply them. But we can do the same for PoseGenerators: 151 | ```cpp 152 | PoseGenerator pose_generator_combined = pose_generator_unit * pose_generator_robot; 153 | ``` 154 | So we can treat PoseGenerators like Matrices. For example we can create a Screw motion by using by combining a 155 | a Relative Motion with a ScrewMotion like in the pose_generators_example.cpp: 156 | ```cpp 157 | Position circle_center = Position(0.45, -0.1, 0.39); 158 | auto no_rotation = generate_constant_orientation_orientation_generator(); 159 | auto circle = PoseGenerators::CirclePoseGenerator(circle_center, -M_PI * 2, Plane::YZ, no_rotation); 160 | auto move_horizontal = PoseGenerators::RelativeMotion(Position(0.2, 0, 0.), Frame::UnitFrame); 161 | auto pose_generator = move_horizontal * circle; 162 | apply_speed_profile(pose_generator); 163 | robot.move_cartesian(pose_generator, 5); 164 | ``` 165 | ![PoseGenerators](https://s8.gifyu.com/images/screw.gif) 166 | 167 | ## PoseGenerators 168 | 169 | The following PoseGenerators are already available: 170 | * CirclePoseGenerator() 171 | * RelativeMotion() 172 | * AbsoluteMotion() 173 | * BezierMotion() 174 | * BSplineMotion() 175 | 176 | But you can also define your own. There are also multiple Position- and OrientationGenerators predefined. 177 | 178 | ## SpeedProfiles 179 | The following SpeedProfiles are already available: 180 | * CosineProfile() 181 | * QuinticPolynomialProfile() 182 | * LinearProfile() <- does nothing 183 | * SCurveProfile() 184 | 185 | Note that the S-Curve Profile is the fastest profile. However you can get joint_{velocity,acceleration}_discontinuities 186 | when the robot is not able to achieve your desired profile in joint space. In this case you have to slow down your S-Curve Profile. 187 | But you can also define your own SpeedProfile 188 | ## StopConditions 189 | StopConditions can be used to abort a movement. It is a function which takes a PoseGenerator input and returns true if and only if 190 | the motion should be aborted. See the fore_stop_example.cpp 191 | 192 | There is only one implemented StopCondition which is triggered by Force. But feel free to define your own. 193 | You can specify a force threshold and a minimum amount of time in percent 194 | when the StopCondition can be activated. It is important to realize that if you specify a StopCondition the program will 195 | not crash due to a cartesian reflex error. It will catch this Exception and will continue. So be careful if you want open the gripper 196 | after one. Also note that the measured Force on the end-effector is not always zero and it can be hard to tune the Force 197 | value so that it will not be triggered immediately. 198 | 199 | ![StopCondition](https://s8.gifyu.com/images/stopconditiond18cb4d431ea96ce.gif) 200 | ## Impedance Mode 201 | There is also an impedance Mode where you can set the attractor point with a PoseGenerator. See the impedance_example.cpp 202 | 203 | ## Licence 204 | This library is copyrighted © 2020 Marco Boneberger, Maximilian von Unwerth, Pouya Mohammadi 205 | 206 | 207 | Licensed under the EUPL, Version 1.2 or – as soon they will be approved by the European Commission - subsequent versions of the EUPL (the "Licence"); 208 | 209 | You may not use this work except in compliance with the Licence. 210 | You may obtain a copy of the Licence at: 211 | 212 | [https://joinup.ec.europa.eu/software/page/eupl](https://joinup.ec.europa.eu/software/page/eupl) 213 | 214 | Unless required by applicable law or agreed to in writing, software distributed under the Licence is distributed on an "AS IS" basis 215 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 216 | 217 | See the Licence for the specific language governing permissions and limitations under the Licence. 218 | 219 | This software includes third party source code from [libfranka](https://github.com/frankaemika/libfranka) 220 | in the folder [3rdparty/libfranka](3rdparty/libfranka) which has its own license. Please see [3rdparty/libfranka/README.md](3rdparty/libfranka/README.md) and 221 | [3rdparty/libfranka/LICENSE-APACHE](3rdparty/libfranka/LICENSE-APACHE) 222 | -------------------------------------------------------------------------------- /cmake/orlConfig.cmake.in: -------------------------------------------------------------------------------- 1 | 2 | @PACKAGE_INIT@ 3 | include(CMakeFindDependencyMacro) 4 | find_dependency(Franka) 5 | find_dependency(Eigen3) 6 | include(${CMAKE_CURRENT_LIST_DIR}/orlTargets.cmake) 7 | 8 | 9 | -------------------------------------------------------------------------------- /examples/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | cmake_minimum_required(VERSION 3.5) 2 | project(examples) 3 | 4 | set(CMAKE_CXX_STANDARD 14) 5 | find_package(Threads) 6 | 7 | include_directories( 8 | # include 9 | SYSTEM 10 | PRIVATE 11 | . 12 | ) 13 | 14 | add_executable(cartesian_motion_example force_stop_example.cpp) 15 | 16 | target_link_libraries(cartesian_motion_example 17 | orl 18 | ) 19 | 20 | add_executable(gripper_control gripper_control.cpp) 21 | 22 | target_link_libraries(gripper_control 23 | orl 24 | ) 25 | add_executable(check_joint_states check_joint_states.cpp) 26 | 27 | target_link_libraries(check_joint_states 28 | orl 29 | ) 30 | 31 | add_executable(circle_example circle_example.cpp) 32 | 33 | target_link_libraries(circle_example 34 | orl 35 | ) 36 | add_executable(pose_generators_example pose_generators_example.cpp) 37 | 38 | target_link_libraries(pose_generators_example 39 | orl 40 | ) 41 | 42 | add_executable(bezier_example bezier_example.cpp) 43 | 44 | target_link_libraries(bezier_example 45 | orl 46 | ) 47 | 48 | add_executable(b_spline_example b_spline_example.cpp) 49 | 50 | target_link_libraries(b_spline_example 51 | orl 52 | ) 53 | add_executable(s_curve_example s_curve_example.cpp) 54 | 55 | target_link_libraries(s_curve_example 56 | orl 57 | ) 58 | add_executable(impedance_example impedance_example.cpp) 59 | 60 | target_link_libraries(impedance_example 61 | orl 62 | ) -------------------------------------------------------------------------------- /examples/b_spline_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example b_spline_example.cpp 5 | * @brief an example on showing how to use B-Splines 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | using namespace orl; 12 | 13 | int main(int argc, char **argv) { 14 | if (argc != 2) { 15 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 16 | return -1; 17 | } 18 | Robot robot(argv[1]); 19 | std::cout << "WARNING: This example will move the robot! " 20 | << "Please make sure to have the user stop button at hand!" << std::endl 21 | << "Double tap the robot to continue..." << std::endl; 22 | robot.double_tap_robot_to_continue(); 23 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 24 | robot.joint_motion(q_goal, 0.2); 25 | robot.absolute_cart_motion(-0.2, 0.4, 0.2, 2); 26 | Orientation down_orientation; 27 | down_orientation.set_RPY(-M_PI, 0, 0); 28 | robot.line_gripper_up_to_orientation(down_orientation, 1); 29 | 30 | // the first waypoint will always be the robot position, you dont have to specify this one 31 | std::vector way_points{{-0.20, 0.4, 0.5}, 32 | {0.3, 0.4, 0.5}, 33 | {0.3, 0.4, 0.2}}; 34 | const int degree_of_b_spline = 2; 35 | auto b_spline_movement = PoseGenerators::BSplineMotion(way_points, degree_of_b_spline); 36 | double trajectory_duration; 37 | const double max_jerk = 16; 38 | const double max_acceleration = 6; 39 | const double max_velocity = 0.1; 40 | apply_speed_profile(b_spline_movement, 41 | SpeedProfiles::SCurveProfile(b_spline_movement, robot.get_current_pose().getPosition(), 42 | max_jerk, max_acceleration, 43 | max_velocity, 44 | trajectory_duration)); 45 | std::cout << trajectory_duration << std::endl; 46 | robot.move_cartesian(b_spline_movement, trajectory_duration); 47 | } 48 | 49 | -------------------------------------------------------------------------------- /examples/bezier_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example bezier_example.cpp 5 | * 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | 12 | using namespace orl; 13 | 14 | int main(int argc, char **argv) { 15 | if (argc != 2) { 16 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 17 | return -1; 18 | } 19 | Robot robot(argv[1]); 20 | std::cout << "WARNING: This example will move the robot! " 21 | << "Please make sure to have the user stop button at hand!" << std::endl 22 | << "Double tap the robot to continue..." << std::endl; 23 | robot.double_tap_robot_to_continue(); 24 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 25 | robot.joint_motion(q_goal, 0.2); 26 | std::vector way_points{{-0.20, 0.2, 0.8}, 27 | {0.3, 0.2, 0.8}, 28 | {0.3, 0.2, 0.2}}; 29 | auto bezier_movement = PoseGenerators::BezierMotion(way_points); 30 | apply_speed_profile(bezier_movement, SpeedProfiles::QuinticPolynomialProfile()); 31 | const double execution_time = 10; 32 | robot.move_cartesian(bezier_movement, execution_time); 33 | } 34 | 35 | -------------------------------------------------------------------------------- /examples/check_joint_states.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Maximilian von Unwerth 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example check_joint_states.cpp 5 | * @authors Maximilian von Unwerth 6 | */ 7 | 8 | #include 9 | #include 10 | #include 11 | #include 12 | #include 13 | #include 14 | #include 15 | 16 | /** 17 | * Watch the robots joint states to identify possible problems 18 | * @param argc Not used 19 | * @param argv Not used 20 | * @return Not used 21 | */ 22 | int main(int argc, char **argv) { 23 | try { 24 | if (argc != 2) { 25 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 26 | return -1; 27 | } 28 | orl::Robot r(argv[1]); 29 | franka::Robot &robot = r.get_franka_robot(); 30 | // load the kinematics and dynamics model 31 | franka::Model model = robot.loadModel(); 32 | 33 | std::array q_min = {-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973}; 34 | std::array q_max = {2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973}; 35 | 36 | while (true) { 37 | std::cout << std::fixed << std::setprecision(4); 38 | std::array joint_states = robot.readOnce().q; //Get the robots current configuration 39 | std::stringstream min, max, real_values; 40 | min << "min: "; 41 | real_values << "val: "; 42 | max << "max: "; 43 | for (int i = 0; i < 7; i++) { // Iterate over all joints 44 | min << q_min[i] << ",\t"; 45 | real_values << joint_states[i] << ",\t"; 46 | max << q_max[i] << ",\t"; 47 | 48 | } 49 | std::cout << min.str() << std::endl; 50 | std::cout << real_values.str() << std::endl; 51 | std::cout << max.str() << std::endl; 52 | 53 | std::cout << std::endl; 54 | r.wait_milliseconds(1000); 55 | } 56 | 57 | 58 | } catch (const franka::Exception &ex) { 59 | // print exception 60 | std::cout << ex.what() << std::endl; 61 | } 62 | return 0; 63 | } -------------------------------------------------------------------------------- /examples/circle_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example circle_example.cpp 5 | * @brief shows how to use the make circle Movements 6 | */ 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | 12 | int main(int argc, char **argv) { 13 | 14 | 15 | if (argc != 2) { 16 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 17 | return -1; 18 | } 19 | orl::Robot robot(argv[1]); 20 | std::cout << "WARNING: This example will move the robot! " 21 | << "Please make sure to have the user stop button at hand!" << std::endl 22 | << "Double tap the robot to continue..." << std::endl; 23 | robot.double_tap_robot_to_continue(); 24 | 25 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 26 | robot.joint_motion(q_goal, 0.2); 27 | 28 | orl::Pose start_p = robot.get_current_pose(); 29 | start_p.set_position(0.45, -0.1, 0.5); 30 | robot.cart_motion(start_p, 3); 31 | orl::Position circle_center = orl::Position(0.45, -0.1, 0.39); 32 | orl::Orientation down_orientation; 33 | down_orientation.set_RPY(-M_PI, 0, 0); 34 | 35 | robot.line_gripper_up_to_orientation(down_orientation, 2); 36 | 37 | auto no_rotation = orl::generate_constant_orientation_orientation_generator(); 38 | robot.move_circle_yz(circle_center, -M_PI * 2, 5, no_rotation); 39 | robot.move_circle_xy(orl::Position(0.4, 0, 0.4), -M_PI * 2, 5, no_rotation); 40 | robot.move_circle_zx(circle_center, -M_PI * 2, 5, no_rotation); 41 | robot.move_circle_yz(circle_center, -M_PI_2, 5); 42 | robot.move_circle_xy(circle_center, -M_PI_2 * 0.5, 2); 43 | robot.move_circle_yz(circle_center, M_PI_2, 5); 44 | robot.move_circle_zx(circle_center, -M_PI_2 * 0.5, 10); 45 | } -------------------------------------------------------------------------------- /examples/force_stop_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example force_stop_example.cpp 5 | * @brief The robot stops the movement when there is an object in the way 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | int main(int argc, char **argv) { 12 | 13 | 14 | try { 15 | if (argc != 2) { 16 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 17 | return -1; 18 | } 19 | orl::Robot robot(argv[1]); 20 | std::cout << "WARNING: This example will move the robot! " 21 | << "Please make sure to have the user stop button at hand!" << std::endl 22 | << "Double tap the robot to continue..." << std::endl; 23 | robot.double_tap_robot_to_continue(); 24 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 25 | robot.joint_motion(q_goal, 0.2); 26 | 27 | orl::Pose pregrasp_pose(robot.get_current_pose()); 28 | pregrasp_pose.set_position(0.4, 0.15, 0.05); 29 | pregrasp_pose.set_RPY(-M_PI, 0, 0); 30 | 31 | orl::Pose move_pose(robot.get_current_pose()); 32 | move_pose.set_position(0.7, 0.15, 0.05); 33 | move_pose.set_RPY(-M_PI, 0, 0); 34 | 35 | robot.close_gripper(); 36 | robot.cart_motion(pregrasp_pose, 5); 37 | robot.cart_motion(move_pose, 5, orl::StopConditions::Force(5)); 38 | robot.cart_motion(pregrasp_pose, 5); 39 | robot.joint_motion(q_goal, 0.2); 40 | 41 | } catch (const franka::Exception &e) { 42 | std::cout << e.what() << std::endl; 43 | return -1; 44 | } 45 | return 0; 46 | } -------------------------------------------------------------------------------- /examples/gripper_control.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Maximilian von Unwerth 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example gripper_control.cpp 5 | * @authors Maximilian von Unwerth 6 | * 7 | * Controls Franka Emika Grippers - designed for a 4 arm platform 8 | * Simply enter the number of the robot and o or c for open and close 9 | */ 10 | #include 11 | #include 12 | #include 13 | 14 | using namespace std; 15 | 16 | 17 | /** 18 | * Open or close the gripper using command line instructions 19 | * @param argc Not used 20 | * @param argv Not used 21 | * @return Not used 22 | */ 23 | int main(int argc, char **argv) { 24 | 25 | 26 | if (argc != 2) { 27 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 28 | return -1; 29 | } 30 | 31 | try { 32 | orl::Robot robot(argv[1]); 33 | string input; 34 | while (input != "o" and input != "c") { // wait for valid input 35 | cout << "Enter (o)PEN or (c)LOSE to control the gripper." << endl; 36 | cin >> input; 37 | } 38 | if (input == "o") { 39 | robot.open_gripper(0.05); 40 | } 41 | if (input == "c") { 42 | robot.close_gripper(); 43 | } 44 | return 0; 45 | } catch (const franka::Exception &ex) { 46 | std::cout << ex.what() << std::endl; // print exception details 47 | } 48 | } -------------------------------------------------------------------------------- /examples/impedance_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example impedance_example.cpp 5 | * @brief shows how to use the impedance mode 6 | */ 7 | #include 8 | #include 9 | 10 | using namespace std; 11 | const double rotational_stiffness = 60; 12 | const double translational_stiffness = 200; 13 | const double execution_time = 5; 14 | 15 | int main(int argc, char **argv) { 16 | 17 | if (argc != 2) { 18 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 19 | return -1; 20 | } 21 | orl::Robot robot(argv[1]); 22 | std::cout << "WARNING: This example will move the robot! " 23 | << "Please make sure to have the user stop button at hand!" << std::endl 24 | << "Double tap the robot to continue..." << std::endl; 25 | robot.double_tap_robot_to_continue(); 26 | 27 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 28 | 29 | robot.joint_motion(q_goal, 0.2); 30 | orl::Pose start_p = robot.get_current_pose(); 31 | start_p.set_position(0.45, -0.1, 0.5); 32 | robot.cart_motion(start_p, 3); 33 | orl::Position circle_center = orl::Position(0.45, -0.1, 0.39); 34 | orl::Orientation down_orientation; 35 | down_orientation.set_RPY(-M_PI, 0, 0); 36 | robot.line_gripper_up_to_orientation(down_orientation, 2); 37 | 38 | auto no_rotation = orl::generate_constant_orientation_orientation_generator(); 39 | 40 | auto attractorPoseGenerator = orl::PoseGenerators::CirclePoseGenerator(circle_center, -M_PI * 2, orl::Plane::YZ, 41 | no_rotation); 42 | orl::apply_speed_profile(attractorPoseGenerator); 43 | while (true) { 44 | robot.impedance_mode(translational_stiffness, rotational_stiffness, execution_time, attractorPoseGenerator, 45 | execution_time); 46 | } 47 | } -------------------------------------------------------------------------------- /examples/pose_generators_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example pose_generators_example.cpp 5 | * @brief example which shows the capabilites of PoseGenerators 6 | */ 7 | 8 | #include 9 | #include 10 | 11 | using namespace orl; 12 | 13 | int main(int argc, char **argv) { 14 | 15 | 16 | if (argc != 2) { 17 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 18 | return -1; 19 | } 20 | Robot robot(argv[1]); 21 | std::cout << "WARNING: This example will move the robot! " 22 | << "Please make sure to have the user stop button at hand!" << std::endl 23 | << "Double tap the robot to continue..." << std::endl; 24 | robot.double_tap_robot_to_continue(); 25 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 26 | robot.joint_motion(q_goal, 0.2); 27 | Pose start_p = robot.get_current_pose(); 28 | start_p.set_position(0.45, -0.1, 0.5); 29 | robot.cart_motion(start_p, 3); 30 | Position circle_center = Position(0.45, -0.1, 0.39); 31 | Orientation down_orientation; 32 | down_orientation.set_RPY(-3 * M_PI / 8, 0, 0); 33 | robot.line_gripper_up_to_orientation(down_orientation, 2); 34 | 35 | 36 | // TCP Motion 37 | auto tcp_motion = PoseGenerators::RelativeMotion(Position(0, 0, 0.2), Frame::RobotTCP); 38 | apply_speed_profile(tcp_motion); 39 | robot.move_cartesian(tcp_motion, 4); 40 | 41 | tcp_motion = PoseGenerators::RelativeMotion(Position(0, 0, -0.2), Frame::RobotTCP); 42 | apply_speed_profile(tcp_motion); 43 | robot.move_cartesian(tcp_motion, 4); 44 | 45 | // line up gripper 46 | down_orientation.set_RPY(-M_PI, 0, 0); 47 | robot.line_gripper_up_to_orientation(down_orientation, 2); 48 | 49 | 50 | // screw motion 51 | auto no_rotation = generate_constant_orientation_orientation_generator(); 52 | auto circle = PoseGenerators::CirclePoseGenerator(circle_center, -M_PI * 2, Plane::YZ, no_rotation); 53 | auto move_horizontal = PoseGenerators::RelativeMotion(Position(0.2, 0, 0.), Frame::UnitFrame); 54 | auto pose_generator = move_horizontal * circle; 55 | apply_speed_profile(pose_generator); 56 | robot.move_cartesian(pose_generator, 5); 57 | 58 | } 59 | 60 | -------------------------------------------------------------------------------- /examples/s_curve_example.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @example s_curve_example.cpp 5 | * @brief shows how to use an S-Curve SpeedProfile 6 | */ 7 | #include 8 | #include 9 | 10 | using namespace orl; 11 | 12 | int main(int argc, char **argv) { 13 | 14 | if (argc != 2) { 15 | std::cerr << "Usage: " << argv[0] << " " << std::endl; 16 | return -1; 17 | } 18 | Robot robot(argv[1]); 19 | std::cout << "WARNING: This example will move the robot! " 20 | << "Please make sure to have the user stop button at hand!" << std::endl 21 | << "Double tap the robot to continue..." << std::endl; 22 | robot.double_tap_robot_to_continue(); 23 | std::array q_goal = {{0.542536, 0.258638, -0.141972, -1.99709, -0.0275616, 2.38391, 1.12856}}; 24 | robot.joint_motion(q_goal, 0.2); 25 | robot.absolute_cart_motion(0.65, 0.0, 0.2, 2); 26 | Orientation down_orientation(-M_PI, 0, 0); 27 | robot.line_gripper_up_to_orientation(down_orientation, 1); 28 | std::vector way_points{{-0.20, 0.4, 0.5}, 29 | {0.3, 0.4, 0.5}, 30 | {0.3, 0.4, 0.2}}; 31 | auto circle_movement = PoseGenerators::CirclePoseGenerator({0.55, 0, 0}, 10 * M_PI, Plane::XY, 32 | generate_constant_orientation_orientation_generator()); 33 | double trajectory_duration; 34 | const double max_jerk = 16; 35 | const double max_acceleration = 6; 36 | const double max_velocity = 0.1; 37 | apply_speed_profile(circle_movement, 38 | SpeedProfiles::SCurveProfile(circle_movement, robot.get_current_pose().getPosition(), max_jerk, 39 | max_acceleration, 40 | max_velocity, 41 | trajectory_duration)); 42 | std::cout << trajectory_duration << std::endl; 43 | robot.move_cartesian(circle_movement, trajectory_duration); 44 | } 45 | 46 | -------------------------------------------------------------------------------- /include/liborl/Frame.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file Frame.h 5 | * @brief defines the Frame struct 6 | * @authors Marco Boneberger 7 | */ 8 | #ifndef LIBORL_FRAME_H 9 | #define LIBORL_FRAME_H 10 | 11 | 12 | /// enum which is there to select a Frame of reference. When in doubt use RobotBase 13 | enum class Frame { 14 | RobotBase, ///< The the end-effector Frame wrt to the Robot Base. This is what you want in most cases 15 | RobotTCP, ///< This is the end-effector Frame of the robot 16 | UnitFrame ///< Frame of the Unit-Homogenous Matrix (Zero rotation around any axis and zero translation) 17 | }; 18 | #endif //LIBORL_FRAME_H 19 | -------------------------------------------------------------------------------- /include/liborl/Payload.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Maximilian von Unwerth 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file Payload.h 5 | * @brief Payload representation for different types of payload. 6 | * @authors Maximilian von Unwerth, Marco Boneberger 7 | */ 8 | #ifndef LIBORL_PAYLOAD_H 9 | #define LIBORL_PAYLOAD_H 10 | 11 | #include 12 | 13 | /** 14 | * Class which represents a grasped payload 15 | */ 16 | namespace orl { 17 | struct Payload { 18 | /** 19 | * Payload mass 20 | */ 21 | double mass; 22 | 23 | /** 24 | * Flange to CoM of payload 25 | */ 26 | std::array pos_wrt_flange; 27 | 28 | /// Inertia matrix 29 | std::array inertia_matrix; 30 | }; 31 | namespace Payloads { 32 | 33 | /** 34 | * Creates a Payload which is shaped like a cylinder 35 | * @param radius of the cylinder in meter 36 | * @param height of the cylinder in meter 37 | * @param mass of the cylinder in kg 38 | * @param position_wrt_flange translation vector from the robot flange to the center of mass of the cylinder. 39 | * 40 | * \note 41 | * Often you just want to have a z offset for the translation 42 | * @return a Payload with the desired properties of a cylinder 43 | */ 44 | Payload Cylinder(double radius, double height, double mass, const std::array &position_wrt_flange); 45 | 46 | /** 47 | * Creates a Payload which is shaped like a sphere 48 | * @param radius in meter 49 | * @param mass in kg 50 | * @param position_wrt_flange translation vector from the robot flange to the center of mass of the sphere. 51 | * 52 | * \note 53 | * Often you just want to have a z offset for the translation 54 | * @return a Payload with the desired properties of a spherical object 55 | */ 56 | Payload Sphere(double radius, double mass, const std::array &position_wrt_flange); 57 | } 58 | } 59 | #endif //LIBORL_PAYLOAD_H 60 | -------------------------------------------------------------------------------- /include/liborl/Pose.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file Pose.h 5 | * @brief Structs for Pose consisting of Position and Orientation 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_POSE_H 10 | #define LIBORL_POSE_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | namespace orl { 17 | 18 | /** 19 | * A Eigen::Vector3d which represents a Position 20 | */ 21 | typedef Eigen::Vector3d Position; 22 | 23 | /** 24 | * 3D Orientation represented as Quaternion 25 | */ 26 | struct Orientation { 27 | Orientation(); 28 | 29 | Eigen::Quaterniond quaternion; 30 | 31 | /** 32 | * sets orientation from quaternion 33 | * @param quaternion 34 | */ 35 | Orientation(const Eigen::Quaterniond &quaternion); 36 | 37 | /** 38 | * sets orientation with roll, pitch, yaw values 39 | * @param roll in radian 40 | * @param pitch in radian 41 | * @param yaw in radian 42 | */ 43 | Orientation(double roll, double pitch, double yaw); 44 | 45 | /** 46 | * multiplies two orientations as Quaternion 47 | * @param b right hand side 48 | * @return a * b 49 | */ 50 | Orientation operator*(const Orientation &b) const; 51 | 52 | /** 53 | * Calculates the inverse Quaternion 54 | * @return inverse Quaternion as Orientation 55 | */ 56 | Orientation inverse() const; 57 | 58 | /** 59 | * gives a String representation of the roll, pitch and yaw values 60 | * @return 61 | */ 62 | std::string toString() const; 63 | 64 | /** 65 | * calculates roll pitch yaw of the Quaternion 66 | * @param[out] rol in radian 67 | * @param[out] pitch in radian 68 | * @param[out] yaw in radian 69 | */ 70 | void get_RPY(double &roll, double &pitch, double &yaw) const; 71 | 72 | /** 73 | * sets the quaternion to a Roll-Pitch-Yaw rotation 74 | * @param r roll in radian 75 | * @param p pitch in radian 76 | * @param y yay in radian 77 | */ 78 | void set_RPY(double r, double p, double y); 79 | 80 | /** 81 | * Rotate around a Quaternion which is generated by roll-pitch-yaw values 82 | * @param r roll angle in radian 83 | * @param p pitch angle in radian 84 | * @param y yaw angle in radian 85 | */ 86 | void add_RPY(double r, double p, double y); 87 | }; 88 | 89 | /*** 90 | * Pose 6D Space (3D Position + 3D Orientation) 91 | */ 92 | struct Pose { 93 | 94 | Eigen::Affine3d pose; 95 | 96 | /** 97 | * Empty Pose Constructor. Generates a Pose 98 | */ 99 | Pose(); 100 | 101 | /** 102 | * Create a Pose with Position and Orientation 103 | * @param p Position 104 | * @param o Orientation 105 | */ 106 | Pose(const Position &p, const Orientation &o); 107 | 108 | /** 109 | * Creates Pose with Eigen::Vector3D and Quaternion 110 | * @param p Eigen::Vector3D Pose 111 | * @param o Quaternion Orientation 112 | */ 113 | Pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &o); 114 | 115 | /** 116 | * Initializes Pose from 4x4 homogenous matrix 117 | * @param mat 4x4 homogenous matrix represented as a 16 element array by concatenation of the columns of the matrix 118 | */ 119 | Pose(std::array mat); 120 | 121 | /** 122 | * Creates a Pose from an Eigen Affine 3D 123 | * @param pose the pose to be used 124 | */ 125 | Pose(const Eigen::Affine3d &pose); 126 | 127 | /** 128 | * Returns the translational part of the pose 129 | * @return Translational part of the pose 130 | */ 131 | Position getPosition() const; 132 | 133 | /** 134 | * set the translational part of a pose. this does not affect the rotational part 135 | * @param position desired position 136 | */ 137 | void setPosition(const Position &position); 138 | 139 | /** 140 | * Gets the quaternion of the pose represented as our custom orientation Struct 141 | * @return 142 | */ 143 | Orientation getOrientation() const; 144 | 145 | /** 146 | * sets the orientational part of the pose. This does not affect the translational part of the pose. so it is no rotation. 147 | * see \ref multiplication to perform a rotation 148 | * @param orientation 149 | */ 150 | void setOrientation(const Orientation &orientation); 151 | 152 | 153 | /** 154 | * returns the translational part of the transformation. It is similar to the getPosition function 155 | * @return 156 | */ 157 | Eigen::Vector3d translation() const; 158 | 159 | /** 160 | * returns the rotational part of the pose as a matrix 161 | * @return 3x3 rotation matrix 162 | */ 163 | Eigen::Affine3d::LinearMatrixType rotation() const; 164 | 165 | /** 166 | * returns the rotational part of the pose as quaternion 167 | * @return 168 | */ 169 | Eigen::Quaterniond quaternion() const; 170 | 171 | /** 172 | * Performs multiplication of Poses as homogenous matrix multiplication 173 | * @param pose_b an other 174 | * @return the result of the multiplication 175 | */ 176 | Pose operator*(const Pose &pose_b) const; 177 | 178 | /** 179 | * Performs multiplication of Poses as homogenous matrix multiplication 180 | * @param affine 181 | * @return he result of the multiplication 182 | */ 183 | Pose operator*(const Eigen::Affine3d &affine) const { 184 | return {pose * affine}; 185 | }; 186 | 187 | /** 188 | * performs multiplication of Poses as homogenous matrix multiplication 189 | * @param lhs left hand side as Affine3d 190 | * @param rhs right hand side as Affine3d 191 | * @return 192 | */ 193 | friend Pose operator*(const Eigen::Affine3d &lhs, const Pose &rhs) { 194 | return {lhs * rhs.pose}; 195 | }; 196 | 197 | /** 198 | * sets the translational part of the matrix 199 | * @param x x-coordinate in meter 200 | * @param y y-coordinate in meter 201 | * @param z z-coordinate in meter 202 | */ 203 | void set_position(double x, double y, double z); 204 | 205 | /** 206 | * adds offset to the current position 207 | * @param x x-offset in meter 208 | * @param y y-offset in meter 209 | * @param z z-offset in meter 210 | */ 211 | void add_position(double x, double y, double z); 212 | 213 | /** 214 | * Sets Pose from from 4x4 homogenous matrix 215 | * @param mat 4x4 homogenous matrix represented as a 16 element array by concatenation of the columns of the matrix 216 | */ 217 | void set(std::array mat); 218 | 219 | /** 220 | * Overrides << for print out a pose 221 | * @param os Stream 222 | * @return Output Stream 223 | */ 224 | std::ostream &operator<<(std::ostream &os) const; 225 | 226 | /** 227 | * converts pose to a double array; 228 | * @return 4x4 homogenous matrix represented as a 16 element array by concatenation of the columns of the matrix 229 | */ 230 | std::array to_matrix() const; 231 | 232 | /** 233 | * Interpolates the Pose to a destination Pose 234 | * @param dest_pose goal Pose 235 | * @param progress value of [0-1] where 0 is the start pose and 1 is the end Pose 236 | * @return interpolated Pose 237 | */ 238 | Pose interpolate(const Pose &dest_pose, double progress) const; 239 | 240 | /** 241 | * Converts a Pose to a String 242 | * @return Pose as String 243 | */ 244 | std::string toString() const; 245 | 246 | /** 247 | * gets roll, pitch and yaw angles of the pose 248 | * @param[out] roll roll angle in radian 249 | * @param[out] pitch pitch angle in radian 250 | * @param[out] yaw yaw angle in radian 251 | */ 252 | void get_RPY(double &roll, double &pitch, double &yaw) const; 253 | 254 | /** 255 | * sets roll, pitch yaw angles 256 | * @param r roll angle in radian 257 | * @param p pitch angle in radian 258 | * @param y yaw angle in radian 259 | */ 260 | void set_RPY(double r, double p, double y); 261 | 262 | /** 263 | * Rotate around a Quaternion which is generated by roll-pitch-yaw values 264 | * @param r roll angle in radian 265 | * @param p pitch angle in radian 266 | * @param y yaw angle in radian 267 | */ 268 | void add_RPY(double r, double p, double y); 269 | 270 | }; 271 | } 272 | #endif //LIBORL_POSE_H 273 | -------------------------------------------------------------------------------- /include/liborl/PoseGenerator.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file PoseGenerator.h 5 | * @brief defines PoseGenerators and contains a set of default PoseGenerators 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_POSEGENERATOR_H 10 | #define LIBORL_POSEGENERATOR_H 11 | 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | 21 | namespace orl { 22 | 23 | /** 24 | * Input for the Generator Functions 25 | */ 26 | struct PoseGeneratorInput { 27 | double progress; ///< progress of the movement between 0 and 1 28 | orl::Pose initial_pose; ///< initial pose at the start of the movement 29 | franka::RobotState state; ///< the current state of the robot 30 | }; 31 | /** 32 | * Functions which generates Positions as Eigen::Vector3d. 33 | * The function takes a PoseGeneratorInput and spits out a Eigen::Vector3d. 34 | */ 35 | typedef std::function PositionGenerator; 36 | /** 37 | * Functions which generates Orientations as Eigen::Quaterniond. 38 | * The function takes a PoseGeneratorInput and spits out a Eigen::Quaterniond. 39 | */ 40 | typedef std::function OrientationGenerator; 41 | 42 | /** 43 | * Functions which generates Poses as orl::Pose. 44 | * The function takes a PoseGeneratorInput and spits out a orl::Pose. 45 | */ 46 | typedef std::function PoseGenerator; 47 | 48 | /** 49 | * A SpeedProfile is a function which takes a double which represents the time-wise progress of 50 | * a motion as interval of [0,1] and converts it to a motion-wise progress as interval of [0,1] 51 | * 52 | * While the the time-wise progress of the motion is always linear (with time), the motion progress 53 | * can be any monotonic function of [0,1] -> [0,1] 54 | */ 55 | typedef std::function SpeedProfile; 56 | 57 | /** 58 | * Generates a PoseGenerator by combining a Position- and a OrientationGenerator 59 | * @param position_generator function which generates positions 60 | * @param orientation_generator function which generates orientations 61 | * @param speed_profile optional speed profile which should be applied. If no SpeedProfile is specified the 62 | * QuinticPolynomialProfile will be used. If you do not want that use the the LinearProfile. 63 | * @return a PoseGenerator 64 | */ 65 | PoseGenerator 66 | generate_pose_generator(const PositionGenerator &position_generator, 67 | const OrientationGenerator &orientation_generator, 68 | const boost::optional &speed_profile = boost::none); 69 | 70 | /** 71 | * creates an orientation generator where the robot does not change its orientation. 72 | * This is useful when you only want the robot to move but do not want the end-effector orientation to change 73 | * @return orientation generator which does not alter the robots orientation 74 | */ 75 | OrientationGenerator generate_constant_orientation_orientation_generator( 76 | const boost::optional &orientation = boost::none); 77 | 78 | /** 79 | * creates an orientation generator where the robot orients itself towards a given point. 80 | * @param angle_axis point where the end-effector should point to 81 | * @return 82 | */ 83 | OrientationGenerator generate_angle_axis_orientation_generator(const Eigen::AngleAxisd &angle_axis); 84 | 85 | /** 86 | * creates a position generator where the robot does not change its position. 87 | * Useful when you only want to control the orientation of the robot 88 | * @return PositionGenerator which does not alter the robots position 89 | */ 90 | PositionGenerator 91 | generate_constant_position_position_generator(const boost::optional &position = boost::none); 92 | 93 | /** 94 | * creates a PoseGenerator which interpolates from the start position of the robot to the desired goal pose 95 | * @param end_pose desired goal pose 96 | * @return PoseGenerator function to interpolate to the goal pose 97 | */ 98 | PoseGenerator generate_pose_interpolation_pose_generator(const orl::Pose &end_pose); 99 | 100 | /** 101 | * creates a orientation generator which interpolates between orientations via slerp. 102 | * @param end_orientation desired goal orientation 103 | * @return orientation generator function to interpolate to the goal orientation 104 | */ 105 | OrientationGenerator 106 | generate_orientation_interpolation_orientation_generator(const Eigen::Quaterniond &end_orientation); 107 | 108 | 109 | PoseGenerator operator*(const PoseGenerator &lhs, const PoseGenerator &rhs); 110 | 111 | namespace PoseGenerators { 112 | /** 113 | * Generates a PoseGenerator which describes a Circle 114 | * @param center center of the circle 115 | * @param rotation_angle the length of the circle in radians. This can be higher than 2 * pi for more multiple circles 116 | * @param plane on which plane the circle should be performed 117 | * @param maybe_orientation_generator an orientation generator for the movement. If there is no orientation generator. this function 118 | * will generate one which rotates the end-effector around the normal of the plane 119 | * @return a PoseGenerator which describes a circle 120 | */ 121 | PoseGenerator CirclePoseGenerator(const Position ¢er, double rotation_angle, const Plane &plane, 122 | const boost::optional &maybe_orientation_generator = boost::none); 123 | 124 | /** 125 | * Generates a PoseGenerator which describes a translation with respect to the given Frame. Per Default it is 126 | * relative to the Position of the robot base. 127 | * @param translation offset of the current position to the goal position 128 | * @param frame a Frame of Reference 129 | * @param maybe_orientation_generator an orientation generator for the movement. If there is no orientation generator this function 130 | * will generate one which does not change the orientation at all 131 | * @return a PoseGenerator which describes a relative offset 132 | */ 133 | PoseGenerator RelativeMotion(const Position &translation, Frame frame = Frame::RobotBase, 134 | const boost::optional &maybe_orientation_generator = boost::none); 135 | 136 | /** 137 | * Generates a PoseGenerator which describes a movement from the given frame to the desired position. 138 | * @param translation the goal position 139 | * @param frame a Frame of Reference 140 | * @param maybe_orientation_generator an orientation generator for the movement. If there is no orientation generator this function 141 | * will generate one which does not change the orientation at all 142 | * @return a PoseGenerator which describes a movement to the desired position 143 | */ 144 | PoseGenerator AbsoluteMotion(const Position &translation, Frame frame = Frame::RobotBase, 145 | const boost::optional &maybe_orientation_generator = boost::none); 146 | 147 | /** 148 | * Generates a PoseGenerator which describes a movement from the given frame to the desired pose 149 | * @param dest goal pose 150 | * @param frame a Frame of Reference 151 | * @return a PoseGenerator the the desired Pose 152 | */ 153 | PoseGenerator MoveToPose(const Pose &dest, Frame frame = Frame::RobotBase); 154 | 155 | /** 156 | * Generates a Bezier PoseGenerator. It only cares about position so you can add an extra OrientationGenerator. 157 | * @note This PoseGenerator does not produce equidistant points, so the derivative wrt to time is not constant 158 | * this means that it is not possible to properly plan the velocity as some part of the movements are faster than others. sry 159 | * @param points vector of control points 160 | * @param frame Reference Frame 161 | * @param maybe_orientation_generator an optional orientation generator otherwise a constant orientation OrientationGenerator will be used 162 | * @return Bezier PoseGenerator 163 | */ 164 | PoseGenerator BezierMotion(const std::vector &points, Frame frame = Frame::RobotBase, 165 | const boost::optional &maybe_orientation_generator = boost::none); 166 | 167 | /** 168 | * Generates a B-Spline PoseGenerator. The B-Spline only cares about position so you can add an extra OrientationGenerator 169 | * @note This PoseGenerator does not produce equidistant points, so the derivative wrt to time is not constant 170 | * this means that it is not possible to properly plan the velocity as some part of the movements are faster than others. sry 171 | * @param points vector of control points 172 | * @param degree degree of the B-Spline polynomial 173 | * @param frame Reference Frame 174 | * @param maybe_orientation_generator an optional orientation generator otherwise a constant orientation OrientationGenerator will be used 175 | * @param maybe_knot_vector vector of knot positions. will assign a default one if you dont want to set your own 176 | * @return B-Spline PoseGenerator 177 | */ 178 | PoseGenerator BSplineMotion(const std::vector &points, int degree, Frame frame = Frame::RobotBase, 179 | const boost::optional &maybe_orientation_generator = boost::none, 180 | const boost::optional> &maybe_knot_vector = boost::none); 181 | } 182 | 183 | /** 184 | * calculates the length of a PoseGenerator given a guess of the initial position 185 | * @param pose_generator a PoseGenerator whose length should be determined 186 | * @param start_pose a guess of where the pose start 187 | * @param steps how many samples of the trajectory should be taken (you should use an even number) 188 | * @return estimated length of the PoseGenerator 189 | */ 190 | double calculate_length(const PoseGenerator &pose_generator, const Pose &start_pose, int steps = 10000); 191 | } 192 | 193 | #endif //LIBORL_POSEGENERATOR_H 194 | -------------------------------------------------------------------------------- /include/liborl/Robot.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file Robot.h 5 | * @brief High-Level functions for the franka::Robot to allow fast and simple work with the robot 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_ROBOT_H 10 | #define LIBORL_ROBOT_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | 22 | namespace orl { 23 | /** 24 | * Wrapper around franka::Robot with more high_level functionality. 25 | * 26 | * Use orl::Robot::get_franka_robot() to access the low-level functionality of franka::Robot 27 | */ 28 | class Robot { 29 | public: 30 | /** 31 | * Establishes a connection to the Franka robot and sets collision and force thresholds 32 | * @param robot_name name or IP of the Franka robot 33 | */ 34 | explicit Robot(const std::string &robot_name); 35 | 36 | /** 37 | * gets the current End-Effector Pose 38 | * @return Current End-Effector Pose 39 | */ 40 | Pose get_current_pose(); 41 | 42 | /** 43 | * Opens the gripper 44 | * @param speed opening speed in m/s 45 | * @param width opening width in m 46 | */ 47 | void open_gripper(double speed = 0.05, double width = 0.1); 48 | 49 | /** 50 | * Closes the gripper 51 | * @param width target width 52 | * @param speed closing speed 53 | * @param force grasping force in Newton 54 | * @param epsilon_width Maximum tolerated deviation from the target width 55 | */ 56 | void close_gripper(double width = 0, double speed = 0.05, double force = 100, double epsilon_width = 0.1); 57 | 58 | /** 59 | * Today I don't feel like doing anything 60 | * @param milliseconds wait time 61 | */ 62 | static void wait_milliseconds(int milliseconds); 63 | 64 | /** 65 | * gets the current joint configuration 66 | * @return 67 | */ 68 | std::array get_current_Joints(); 69 | 70 | /** 71 | * Relative Cartesian motion in x, y, z direction. 72 | * The orientation stays the same 73 | * \warning This command will move the robot! 74 | * @param x in meter 75 | * @param y in meter 76 | * @param z in meter 77 | * @param max_time duration of the trajectory in seconds 78 | * @param stop_condition a Stop condition which stops the movement 79 | */ 80 | void relative_cart_motion(double x, double y, double z, double max_time = 10, 81 | const boost::optional &stop_condition = boost::none); 82 | 83 | /** 84 | * Absolute Cartesian motion to x, y, z position, 85 | * The orientation stays the same 86 | * \warning This command will move the robot! 87 | * @param x in meter 88 | * @param y in meter 89 | * @param z in meter 90 | * @param max_time duration of the trajectory in seconds 91 | * @param stop_condition a Stop condition which stops the movement 92 | */ 93 | void absolute_cart_motion(double x, double y, double z, double max_time = 10, 94 | const boost::optional &stop_condition = boost::none); 95 | 96 | /** 97 | * Cartesian motion to a destination pose 98 | * \warning This command will move the robot! 99 | * @param dest destination Pose 100 | * @param max_time duration of the trajectory in seconds 101 | * @param stop_condition a Stop condition which stops the movement 102 | */ 103 | void cart_motion(const Pose &dest, double max_time = 10, 104 | const boost::optional &stop_condition = boost::none); 105 | 106 | /** 107 | * Get Franka Robot to access low_level functionality 108 | * @return Reference to franka::Robot 109 | */ 110 | franka::Robot &get_franka_robot(); 111 | 112 | /** 113 | * Get Franka Model 114 | * @return Reference to franka::Model 115 | */ 116 | franka::Model &get_franka_model(); 117 | 118 | /** 119 | * Get Franka Gripper 120 | * @return Reference to franka::Gripper 121 | */ 122 | franka::Gripper &get_franka_gripper(); 123 | 124 | /** 125 | * Go into cartesian impedance mode. 126 | * The attractor point will move from the current pose the desired attractor pose 127 | * in the specified amount of time. 128 | * 129 | * The impedance mode will stop after max_time has passed 130 | * \warning This will move the robot! 131 | * @param translational_stiffness 132 | * @param rotational_stiffness 133 | * @param desired_attractor_pose 134 | * @param max_time duration of the impedance mode 135 | * @param move_to_attractor_duration time where the attractor point will be shifted to the goal pose 136 | */ 137 | void 138 | impedance_mode(double translational_stiffness, double rotational_stiffness, const Pose &desired_attractor_pose, 139 | double max_time = 1000000, double move_to_attractor_duration = 2); 140 | 141 | /** 142 | * Go into cartesian impedance mode 143 | * @param translational_stiffness 144 | * @param rotational_stiffness 145 | * @param max_time time after which the impedance mode gets aborted 146 | */ 147 | void impedance_mode(double translational_stiffness, double rotational_stiffness, double max_time = 1000000); 148 | 149 | /** 150 | * * Go into cartesian impedance mode 151 | * The attractor point will be set by the pose Generator 152 | * @param translational_stiffness 153 | * @param rotational_stiffness 154 | * @param attractor_pose_generator PoseGenerator to execute 155 | * @param max_time duration of the impedance mode 156 | * @param move_to_attractor_duration execution time for the PoseGenerator 157 | */ 158 | void 159 | impedance_mode(double translational_stiffness, double rotational_stiffness, 160 | double max_time, const PoseGenerator &attractor_pose_generator, 161 | double move_to_attractor_duration); 162 | 163 | /** 164 | * Moves the robot with the help of a PoseGenerator. 165 | * This is the most important function here and most other functions 166 | * are calling this function by the end of the day 167 | * 168 | * 169 | * @param cartesian_pose_generator a function to generate the poses 170 | * @param max_time time the robot has to execute the movement 171 | * @param stop_condition a Stop condition which stops the movement 172 | * @param elbow desired elbow configuration 173 | */ 174 | void move_cartesian(PoseGenerator cartesian_pose_generator, double max_time, 175 | const boost::optional &stop_condition = boost::none, 176 | boost::optional elbow = boost::none); 177 | 178 | /** 179 | * lets the robot move in a circle on the x-y-plane. 180 | * 181 | * @param center center of the circle 182 | * @param rotation_angle how many radians to rotate 183 | * @param max_time how long the movement should take 184 | * @param maybe_orientation_generator provide an alternative orientationGenerator 185 | * default is a orientation generator which does not change the orientation. 186 | * @param stop_condition a Stop condition which stops the movement 187 | */ 188 | void move_circle_xy(const Position ¢er, double rotation_angle, double max_time, 189 | const boost::optional &maybe_orientation_generator = boost::none, 190 | const boost::optional &stop_condition = boost::none); 191 | 192 | /** 193 | * lets the robot move in a circle on the y-z-plane. 194 | * 195 | * @param center center of the circle 196 | * @param rotation_angle how many radians to rotate 197 | * @param max_time how long the movement should take 198 | * @param maybe_orientation_generator provide an alternative orientationGenerator 199 | * default is a orientation generator which does not change the orientation. 200 | * @param stop_condition a Stop condition which stops the movement 201 | */ 202 | void move_circle_yz(const Position ¢er, double rotation_angle, double max_time, 203 | const boost::optional &maybe_orientation_generator = boost::none, 204 | const boost::optional &stop_condition = boost::none); 205 | 206 | /** 207 | * lets the robot move in a circle on the z-x-plane. 208 | * 209 | * @param center center of the circle 210 | * @param rotation_angle how many radians to rotate 211 | * @param max_time how long the movement should take 212 | * @param maybe_orientation_generator provide an alternative orientationGenerator 213 | * default is a orientation generator which does not change the orientation. 214 | * @param stop_condition a Stop condition which stops the movement 215 | */ 216 | void move_circle_zx(const Position ¢er, double rotation_angle, double max_time, 217 | const boost::optional &maybe_orientation_generator = boost::none, 218 | const boost::optional &stop_condition = boost::none); 219 | 220 | /** 221 | * moves the robot to a certain orientation 222 | * @param desired_orientation goal orientation 223 | * @param max_time time to execute the movement 224 | */ 225 | void line_gripper_up_to_orientation(const Orientation &desired_orientation, double max_time); 226 | 227 | /** 228 | * waits for a human to put two times a small force on the end-effector (or a joint close to the end-effector) 229 | * in a short amount of time 230 | */ 231 | void double_tap_robot_to_continue(); 232 | 233 | /** 234 | * Set the load of the robot 235 | * @param load Payload object 236 | */ 237 | void setLoad(const Payload &load); 238 | 239 | /** 240 | * Generates a Joint Motion to the desired goal 241 | * @param q_goal joint states 242 | * @param speed_factor value between 0 and 1 as factor of the maximum speed 243 | */ 244 | void joint_motion(std::array q_goal, double speed_factor); 245 | 246 | /** 247 | * This function blocks until a certain amount of force is applied in a given direction 248 | * @param force minimum force the robot has to experience before he continues 249 | * @param axis an axis of the robot from which we compare the force 250 | */ 251 | void force_to_continue(double force, Axis axis); 252 | 253 | /** 254 | * This function blocks until a certain amount of force is applied in a given direction or a certain time has passed 255 | * @param force minimum force the robot has to experience before he continues 256 | * @param axis an axis of the robot from which we compare the force 257 | * @param time_out maximum time in seconds after which the function returns 258 | * @return true if the forces were reached; false if the time limit has been reached 259 | */ 260 | bool force_to_continue(double force, Axis axis, double time_out); 261 | 262 | /** 263 | * This function blocks until a certain amount of torque is applied on a given axis 264 | * @param torque minimum torque the robot has to experience before he continues 265 | * @param axis an axis of the robot from which we compare the torque 266 | */ 267 | void torque_to_continue(double torque, Axis axis); 268 | 269 | /** 270 | * This function blocks until a certain amount of torque is applied on a given axis or a certain time has passed 271 | * @param torque minimum torque the robot has to experience before he continues 272 | * @param axis an axis of the robot from which we compare the torque 273 | * @param time_out maximum time in seconds after which the function returns 274 | * @return true if the torques were reached; false if the time limit has been reached 275 | */ 276 | bool torque_to_continue(double torque, Axis axis, double time_out); 277 | 278 | private: 279 | /** 280 | * franka::Robot instance 281 | * Use get_franka_robot() to get it 282 | */ 283 | franka::Robot robot; 284 | 285 | /** 286 | * franka::Gripper instance 287 | * Use get_franka_gripper() to get it 288 | */ 289 | franka::Gripper gripper; 290 | 291 | /** 292 | * franka::Model instance 293 | * Use get_franka_model() to get it 294 | */ 295 | franka::Model model; 296 | 297 | /** 298 | * The robots payload 299 | */ 300 | Payload payload; 301 | 302 | /** 303 | * sets collision and impedance values 304 | */ 305 | void setDefaultBehavior(); 306 | 307 | 308 | /** 309 | * moves the robot circle on a circle given a center and a given plane of the moving 310 | * direction and the direction to the center. 311 | * 312 | * 313 | * @param center center of the circle 314 | * @param rotation_angle how many radians to rotate 315 | * @param max_time time to execute the circle movement 316 | * @param plane 317 | * @param maybe_orientation_generator 318 | * @param stop_condition a Stop condition which stops the movement 319 | */ 320 | void move_circle(const Position ¢er, double rotation_angle, double max_time, Plane plane, 321 | const boost::optional &maybe_orientation_generator, 322 | const boost::optional &stop_condition = boost::none); 323 | 324 | 325 | }; 326 | } 327 | #endif //LIBORL_ROBOT_H 328 | -------------------------------------------------------------------------------- /include/liborl/SCurve.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file SCurve.h 5 | * @brief contains necessary structs and methods to calculate S-Curves 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_SCURVE_H 10 | #define LIBORL_SCURVE_H 11 | 12 | #include 13 | #include 14 | #include 15 | 16 | 17 | namespace SCurve { 18 | 19 | /** 20 | * Struct which contains the desired limits for jerk, acceleration and velocity in SI units 21 | */ 22 | struct SCurveConstraints { 23 | double max_jerk = 0; 24 | double max_acceleration = 0; 25 | double max_velocity = 0; 26 | }; 27 | 28 | /// Enum which is used to select whether you want to calculate 29 | /// Position, Velocity, Acceleration or Jerk of your S-Curve 30 | enum Derivative { 31 | Position = 0, Velocity = 1, Acceleration = 2, Jerk = 3 32 | }; 33 | 34 | /** 35 | * Struct which contains the time intervals of the S-Curve 36 | */ 37 | struct SCurveTimings { 38 | double T_j1 = 0;///< time-interval in which the jerk is constant (j max or j min ) during the acceleration phase 39 | double T_j2 = 0; ///< time-interval in which the jerk is constant (j max or j min ) during the deceleration phase 40 | double T_a = 0;///< Acceleration period 41 | double T_v = 0;///< constant velocity period 42 | double T_d = 0;///< deceleration period 43 | 44 | /** 45 | * total duration of the trajectory ( = T a + T v + T d ) 46 | * @return duration in seconds 47 | */ 48 | double T() const; 49 | 50 | /** 51 | * determines if the maximum acceleration is reaches 52 | * @return 53 | */ 54 | bool is_max_acceleration_not_reached() const; 55 | }; 56 | 57 | /// struct in which the start and end conditions are stored 58 | struct SCurveStartConditions { 59 | double q0 = 0; ///< start position 60 | double q1 = 1; ///< end position 61 | double v0 = 0; ///< start velocity 62 | double v1 = 0; ///< end velocity 63 | 64 | /** 65 | * displacement form start q0 to end q1 66 | * @return displacement in meter 67 | */ 68 | double h() const; 69 | }; 70 | 71 | /** 72 | * Parameters of the SCurve. This is the set of variable which completely defines the trajectory 73 | */ 74 | struct SCurveTrajectoryParameters { 75 | SCurveTimings times;///< S-Curve time intervals 76 | double j_max = 0; ///< max jerk 77 | double j_min = 0;///< min jerk 78 | double a_lim_a = 0;///< maximum acceleration in the acceleration phase 79 | double a_lim_d = 0;///< maximum acceleration in the deceleration phase 80 | 81 | double v_lim = 0;///< start position 82 | SCurveStartConditions s;///< start position 83 | 84 | }; 85 | 86 | /** 87 | * Input parameters which are necessary to generate a S-Curve. This is not the same as the SCurveTrajectoryParameters but you can 88 | * calculate them with the initialization parameters 89 | */ 90 | struct SCurveInitializationParameters { 91 | SCurveConstraints constraints; ///< constraints for the S-Curve 92 | SCurveStartConditions startConditions; ///< start conditions 93 | public: 94 | /** 95 | * calculates the time intervals for the parameters 96 | * @return 97 | */ 98 | SCurveTimings calc_times() const; 99 | 100 | /** 101 | * checks if a trajectory is feasible 102 | * @return 103 | */ 104 | bool is_trajectory_feasible() const; 105 | 106 | private: 107 | /** 108 | * checks if the maximum acceleration can not be reached 109 | * @return 110 | */ 111 | bool is_a_max_not_reached() const; 112 | 113 | /** 114 | * checks if the minimum acceleration can not be reached 115 | * @return 116 | */ 117 | bool is_a_min_not_reached() const; 118 | 119 | 120 | /** 121 | * calculates the times if the maximum velocity can not be reached 122 | * @return 123 | */ 124 | SCurveTimings calc_times_case_2() const; 125 | 126 | /** 127 | * calculates the times if the maximum velocity can be reached 128 | * @return 129 | */ 130 | SCurveTimings calc_times_case_1() const; 131 | 132 | /** 133 | * This function checks if the acceleration or deceleration phase are negative and fixes them 134 | * @param times current time intervals 135 | * @param new_initialization set of initialization parameters to try it again 136 | * @return 137 | */ 138 | SCurveTimings &handle_negative_acceleration_time(SCurveTimings ×, 139 | const SCurveInitializationParameters &new_initialization) const; 140 | }; 141 | 142 | /** 143 | * generates the trajectory parameters 144 | * @param times S-Curve intervals 145 | * @param initialization trajectory initialization parameters 146 | * @return a fully defined set of S-Curve Parameters 147 | */ 148 | SCurveTrajectoryParameters 149 | generate_trajectory_parameters(const SCurveTimings ×, const SCurveInitializationParameters &initialization); 150 | 151 | /** 152 | * Calculates values of the S-Curve during the Acceleration Phase 153 | * @param parameters trajectory parameters 154 | * @param t time in seconds. this is the function input 155 | * @param derivative What derivative you are interested in. Default is position 156 | * @return value of the S-Curve at time t 157 | */ 158 | double acceleration_phase(const SCurveTrajectoryParameters ¶meters, double t, 159 | Derivative derivative = Derivative::Position); 160 | 161 | /** 162 | * Calculates values of the S-Curve during the Constant Velocity Phase 163 | * @param parameters trajectory parameters 164 | * @param t time in seconds. this is the function input 165 | * @param derivative What derivative you are interested in. Default is position 166 | * @return value of the S-Curve at time t 167 | */ 168 | double 169 | constant_velocity_phase(const SCurveTrajectoryParameters ¶meters, double t, 170 | Derivative derivative = Derivative::Position); 171 | 172 | /** 173 | * Calculates values of the S-Curve during the Deceleration Phase 174 | * @param parameters trajectory parameters 175 | * @param t time in seconds. this is the function input 176 | * @param derivative What derivative you are interested in. Default is position 177 | * @return value of the S-Curve at time t 178 | */ 179 | double deceleration_phase(const SCurveTrajectoryParameters ¶meters, double t, 180 | Derivative derivative = Derivative::Position); 181 | 182 | /** 183 | * Evaluates the S-Curve at a given time 184 | * @param parameters trajectory parameters 185 | * @param t time in seconds. this is the function input 186 | * @param derivative What derivative you are interested in. Default is position 187 | * @return value of the S-Curve at time t 188 | */ 189 | double 190 | eval_s_curve(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative = Derivative::Position); 191 | 192 | /** 193 | * This function generates a function which takes time as input and returns the value of the S-Curve at the given time 194 | * It also returns the Parameters of the S-Curve 195 | * @param init_params initialization parameters for the S-Curve 196 | * @param derivative What derivative you are interested in. Default is position 197 | * @return SCurveTrajectoryParameters and SCurve function 198 | */ 199 | std::pair> 200 | s_curve_generator(const SCurveInitializationParameters &init_params, Derivative derivative = Derivative::Position); 201 | } 202 | 203 | #endif //LIBORL_SCURVE_H 204 | -------------------------------------------------------------------------------- /include/liborl/SpeedProfile.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file SpeedProfile.h 5 | * @brief defines SpeedProfiles and offers a set of SpeedProfiles that can be used 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_SPEEDPROFILE_H 10 | #define LIBORL_SPEEDPROFILE_H 11 | 12 | #include 13 | #include 14 | 15 | namespace orl { 16 | 17 | namespace SpeedProfiles { 18 | 19 | /** 20 | * Generates a SpeedProfile which uses a polynomial of degree 5. This is the Default SpeedProfile 21 | * @return a QuinticPolynomial SpeedProfile 22 | */ 23 | SpeedProfile QuinticPolynomialProfile(); 24 | 25 | /** 26 | * Generates a SpeedProfile which does not alter the PoseGenerator. Use it when you do not want your PoseGenerator to 27 | * be implicitly converted to a Quintic Polynomial 28 | * @return a Linear SpeedProfile 29 | */ 30 | SpeedProfile LinearProfile(); 31 | 32 | /** 33 | * Generates a SpeedProfile which uses 1 - cos(x) 34 | * @return a Cosine SpeedProfile 35 | */ 36 | SpeedProfile CosineProfile(); 37 | 38 | /** 39 | * Generates an S-Curve Profile if you already know the trajectory parameters. 40 | * @param params Parameters of the S-Curve 41 | * @return S-Curve Speed Profile 42 | */ 43 | SpeedProfile SCurveProfile(const SCurve::SCurveTrajectoryParameters ¶ms); 44 | 45 | /** 46 | * Generates an S-Curve Speed Profile 47 | * 48 | * @warning If it is not possible to fulfill your trajectory the acceleration and velocity will be decreased 49 | * @param[in] pg a the Pose Generator for which the profile should be calculated 50 | * @param[in] initial_position an initial guess of the start position 51 | * @param[in] jerk maximum allowed jerk for your profile 52 | * @param[in] acceleration maximum allowed acceleration for your profile 53 | * @param[in] velocity maximum allowed velocity for your profile 54 | * @param[out] trajectory_duration duration of the trajectory 55 | * @return S-Curve Speed Profile 56 | */ 57 | SpeedProfile 58 | SCurveProfile(const PoseGenerator &pg, const Position &initial_position, double jerk, double acceleration, 59 | double velocity, double &trajectory_duration); 60 | 61 | /** 62 | * Generates an S-Curve Speed Profile 63 | * @warning If it is not possible to fulfill your trajectory the acceleration and velocity will be decreased. 64 | * They are writen in your params variable 65 | * @param[in] pg a the Pose Generator for which the profile should be calculated 66 | * @param[in] initial_position an initial guess of the start position 67 | * @param[in] jerk maximum allowed jerk for your profile 68 | * @param[in] acceleration maximum allowed acceleration for your profile 69 | * @param[in] velocity maximum allowed velocity for your profile 70 | * @param[out] params the calculated trajectory parameters 71 | * @return 72 | */ 73 | SpeedProfile 74 | SCurveProfile(const PoseGenerator &pg, const Position &initial_position, double jerk, double acceleration, 75 | double velocity, SCurve::SCurveTrajectoryParameters ¶ms); 76 | } 77 | 78 | /** 79 | * applies a SpeedProfile to a PoseGenerator 80 | * @param pose_generator 81 | * @param speed_profile 82 | */ 83 | void apply_speed_profile(PoseGenerator &pose_generator, 84 | const SpeedProfile &speed_profile = SpeedProfiles::QuinticPolynomialProfile()); 85 | 86 | } 87 | 88 | #endif //LIBORL_SPEEDPROFILE_H 89 | -------------------------------------------------------------------------------- /include/liborl/StopCondition.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file StopCondition.h 5 | * @brief defines StopCondtions and offers a method to stop the robot by Force 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_STOPCONDITION_H 10 | #define LIBORL_STOPCONDITION_H 11 | 12 | #include 13 | 14 | namespace orl { 15 | /** 16 | * Function which returns a bool whether to stop the robot at a specific condition 17 | */ 18 | typedef std::function StopCondition; 19 | namespace StopConditions { 20 | 21 | /** 22 | * generates a function which stops the robot when there is a force on the endeffector which exceeds max_force 23 | * and the movement within the last part of the movement 24 | * \warning if you dont set a minimum progress the robot can abort the motion at any time due to the condition. 25 | * if you open the gripper afterwards the object could fall at anytime! 26 | * @param max_force 27 | * @param minimum_progress minimum progress [0,1] after which the stop condition can be fired 28 | * @return a StopCondition 29 | */ 30 | StopCondition Force(double max_force, double minimum_progress = 0); 31 | 32 | } 33 | } 34 | #endif //LIBORL_STOPCONDITION_H 35 | -------------------------------------------------------------------------------- /include/liborl/enums.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file enums.h 5 | * @brief File which contains some Enums needed for selecting Axis and Planes 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_ENUMS_H 10 | #define LIBORL_ENUMS_H 11 | namespace orl { 12 | 13 | 14 | /** 15 | * Axis enum for selecting an axis of the robot 16 | */ 17 | enum Axis { 18 | X = 0, Y = 1, Z = 2 19 | }; 20 | /** 21 | * Plane enum for selecting XY, YZ, ZX plane 22 | */ 23 | enum class Plane { 24 | XY, YZ, ZX 25 | }; 26 | } 27 | #endif //LIBORL_ENUMS_H 28 | -------------------------------------------------------------------------------- /include/liborl/geometry_math.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file geometry_math.h 5 | * @brief set of useful functions to do geometry 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | #ifndef LIBORL_GEOMETRY_MATH_H 10 | #define LIBORL_GEOMETRY_MATH_H 11 | 12 | #include 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | 19 | /** 20 | * calculates the angle between a direction "a" and a direction "b" around a given normal 21 | * @param a from direction from origin 22 | * @param b to direction from origin 23 | * @param normal normal around which to calculate the angle 24 | * @return angle in radian 25 | */ 26 | double angle_between(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &normal); 27 | 28 | /** 29 | * Function to calculate a circle generator around a center 30 | * 31 | * This function returns a function where you can pass in a value [0,1] and it spits out a point on the circle 32 | * where 0 is the start and 1 is at the end. 33 | * 34 | * @param center center of the circle 35 | * @param normal normal between the moving direction and the direction to the center 36 | * @param start start position of the circle 37 | * @param rotation_angle how many radian you want on the circle 38 | * @return parameterized function which generates the circle 39 | */ 40 | std::function 41 | generate_circle_movement(const Eigen::Vector3d ¢er, const Eigen::Vector3d &normal, const Eigen::Vector3d &start, 42 | double rotation_angle); 43 | 44 | /** 45 | * generates a circle function on the x-y-plane. 46 | * the center gets automatically projected to the z position of the center 47 | * \see generate_circle_movement for more details 48 | * @param center center of the circle 49 | * @param start start position of the circle 50 | * @param rotation_angle how many radian you want on the circle 51 | * @return circle generating function 52 | */ 53 | std::function 54 | generate_circle_movement_xy_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle); 55 | 56 | /** 57 | * generates a circle function on the y-z-plane. 58 | * the center gets automatically projected to the x position of the center 59 | * \see generate_circle_movement for more details 60 | * @param center center of the circle 61 | * @param start start position of the circle 62 | * @param rotation_angle how many radian you want on the circle 63 | * @return circle generating function 64 | */ 65 | std::function 66 | generate_circle_movement_yz_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle); 67 | 68 | /** 69 | * generates a circle function on the z-x-plane. 70 | * the center gets automatically projected to the y position of the center 71 | * \see generate_circle_movement for more details 72 | * @param center center of the circle 73 | * @param start start position of the circle 74 | * @param rotation_angle how many radian you want on the circle 75 | * @return circle generating function 76 | */ 77 | std::function 78 | generate_circle_movement_zx_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle); 79 | 80 | 81 | /** 82 | * generates a Bezier-Curve-function which maps [0,1] -> Positions 83 | * @param points vector of control points 84 | * @return Bezier Function 85 | */ 86 | std::function 87 | generate_bezier_curve(const std::vector &points); 88 | 89 | 90 | /** 91 | * calculates the value of a B-spline with known in index k. 92 | * This is an mostly internal function. You will probably better of with the generate_spline function 93 | * This is the optimized variant of De Boors algorithm according to Wikipedia 94 | * and its Python implementation 95 | * @param k Index of knot interval that contains x 96 | * @param p Degree of B-spline 97 | * @param x Position 98 | * @param t Vector of knot positions, needs to be padded 99 | * @param c Vector of control points 100 | * @return Value of the b-Spline 101 | */ 102 | Eigen::Vector3d 103 | b_spline_de_boor(int k, int p, double x, const std::vector &t, const std::vector &c); 104 | 105 | /** 106 | * generates a B-Spline function which maps [0,1] -> Positions using De Boors Algorithm 107 | * @param points vector of control points 108 | * @param degree degree of the B-Spline 109 | * @param knot_vector Knot vector see Wikipedia:NURBS N for an explanation of knot vectors 110 | * @return B-Spline function 111 | */ 112 | std::function 113 | generate_b_spline(const std::vector &points, int degree, const std::vector &knot_vector); 114 | 115 | /** 116 | * generates a B-Spline function which maps [0,1] -> Positions using De Boors Algorithm 117 | * with a default knot vector 118 | * 119 | * @param points vector of control points 120 | * @param degree degree of the B-Spline 121 | * @return B-Spline function 122 | */ 123 | std::function 124 | generate_b_spline(const std::vector &points, int degree); 125 | 126 | /** 127 | * So you wanna use B-Splines? But you don't know anything 'bout knot vectors, don't ya? Dont you worry. We got you covered. 128 | * This function generates sane knot vectors for any amount of control points and degrees. 129 | * 130 | * For degree 1 it creates a natural sequence [0,1,2,..,m] 131 | * For degree 2 it creates something like [0,0,0,1,2,3,..,m-2,m-1,m,m,m] 132 | * and for Further degrees it creates an additional 0 at the start and m at the end 133 | * @param num_control_points amount of control points 134 | * @param degree degree of the B-Spline 135 | * @return knot vector 136 | */ 137 | std::vector create_default_knot_vector(int num_control_points, int degree); 138 | 139 | #endif //LIBORL_GEOMETRY_MATH_H 140 | -------------------------------------------------------------------------------- /include/liborl/liborl.h: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /** 4 | * @file liborl.h 5 | * @brief This file includes everything else you need in libORL 6 | * @authors Marco Boneberger 7 | */ 8 | 9 | 10 | #ifndef LIBORL_LIBORL_H 11 | #define LIBORL_LIBORL_H 12 | 13 | #include 14 | #include 15 | #include 16 | #include 17 | #include 18 | #include 19 | #include 20 | #include 21 | #include 22 | #include 23 | 24 | #endif //LIBORL_LIBORL_H 25 | -------------------------------------------------------------------------------- /src/Payload.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Maximilian von Unwerth 2 | // Licensed under the EUPL-1.2-or-later 3 | /* 4 | * Payload representation for different types of payload. 5 | * The inertia matrix of the payloads is calculated by its type 6 | */ 7 | #include 8 | using namespace orl; 9 | Payload Payloads::Sphere(double radius, double mass, const std::array &position_wrt_flange) { 10 | double i = (2.0 / 5.0) * mass * radius * radius; 11 | std::array inertia_matrix = {i, 0, 0, 0, i, 0, 0, 0, i}; 12 | return {mass, position_wrt_flange, inertia_matrix}; 13 | } 14 | 15 | Payload Payloads::Cylinder(double radius, double height, double mass, 16 | const std::array &position_wrt_flange) { 17 | std::array inertia_matrix = {(mass / 12) * (3 * radius * radius + height * height), 0, 0, 0, 18 | (mass / 12) * (3 * radius * radius + height * height), 0, 0, 0, 19 | (mass / 2) * radius * radius}; 20 | 21 | return {mass, position_wrt_flange, inertia_matrix}; 22 | } 23 | -------------------------------------------------------------------------------- /src/Pose.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #include 4 | using namespace orl; 5 | using namespace Eigen; 6 | 7 | 8 | 9 | void Orientation::set_RPY(double r, double p, double y) { 10 | Eigen::Matrix3d a; 11 | a = AngleAxisd(y, Vector3d::UnitZ()) 12 | * AngleAxisd(p, Vector3d::UnitY()) 13 | * AngleAxisd(r, Vector3d::UnitX()); 14 | quaternion = a; 15 | quaternion.normalize(); 16 | 17 | } 18 | 19 | void Orientation::get_RPY(double &roll, double &pitch, double &yaw) const { 20 | //ZYX, yaw, pitch, roll 21 | Eigen::Vector3d rpy_vec = quaternion.toRotationMatrix().eulerAngles(2, 1, 0); 22 | 23 | roll = rpy_vec.z(); 24 | pitch = rpy_vec.y(); 25 | yaw = rpy_vec.x(); 26 | } 27 | 28 | std::string Orientation::toString() const { 29 | std::stringstream ss; 30 | double roll, pitch, yaw; 31 | get_RPY(roll, pitch, yaw); 32 | ss << "roll: " << roll << "\tpitch: " << pitch << "\tyaw: " << yaw; 33 | return ss.str(); 34 | } 35 | 36 | Orientation::Orientation(const Quaterniond &quaternion) : quaternion(quaternion) {} 37 | 38 | Orientation::Orientation() : quaternion() {} 39 | 40 | Orientation::Orientation(double roll, double pitch, double yaw) : quaternion() { 41 | set_RPY(roll, pitch, yaw); 42 | } 43 | 44 | void Orientation::add_RPY(double r, double p, double y) { 45 | quaternion = Orientation(r, p, y).quaternion * quaternion; 46 | } 47 | 48 | Orientation Orientation::operator*(const Orientation &b) const { 49 | return Orientation(quaternion * b.quaternion); 50 | } 51 | 52 | Orientation Orientation::inverse() const { 53 | return Orientation(quaternion.inverse()); 54 | } 55 | 56 | Pose::Pose() { 57 | // pose 58 | // orientation.set_RPY(0, 0, 0); 59 | } 60 | 61 | Pose::Pose(std::array mat) { 62 | set(mat); 63 | } 64 | 65 | std::ostream &Pose::operator<<(std::ostream &os) const { 66 | os << "position" << std::endl; 67 | return os; 68 | } 69 | 70 | std::array Pose::to_matrix() const { 71 | 72 | Matrix3d rot_mat = rotation(); 73 | std::array mat = {rot_mat(0, 0), rot_mat(1, 0), rot_mat(2, 0), 0, rot_mat(0, 1), rot_mat(1, 1), 74 | rot_mat(2, 1), 0, rot_mat(0, 2), rot_mat(1, 2), rot_mat(2, 2), 0, translation().x(), 75 | translation().y(), translation().z(), 1}; 76 | return mat; 77 | } 78 | 79 | Pose Pose::interpolate(const Pose &dest_pose, double progress) const { 80 | if (progress > 1) { 81 | progress = 1; 82 | } 83 | Eigen::Vector3d pos_result = translation() + (dest_pose.translation() - translation()) * progress; 84 | Eigen::Quaterniond quat_start, quat_end, quat_result; 85 | quat_start = rotation(); 86 | quat_end = dest_pose.rotation(); 87 | quat_result = quat_start.slerp(progress, quat_end); 88 | Pose result_pose(pos_result, quat_result); 89 | return result_pose; 90 | } 91 | 92 | void Pose::set(std::array mat) { 93 | Eigen::Vector3d position(mat[12], mat[13], mat[14]); 94 | Eigen::Affine3d::LinearMatrixType rot_mat; 95 | rot_mat << mat[0], mat[4], mat[8], mat[1], mat[5], mat[9], mat[2], mat[6], mat[10]; 96 | pose = Eigen::Translation3d(position) * rot_mat; 97 | } 98 | 99 | std::string Pose::toString() const { 100 | std::stringstream ss; 101 | double roll, pitch, yaw; 102 | get_RPY(roll, pitch, yaw); 103 | 104 | ss << pose.matrix() << std::endl; 105 | ss << "roll: " << roll << "\tpitch: " << pitch << "\tyaw: " << yaw; 106 | return ss.str(); 107 | } 108 | 109 | Pose::Pose(const Position &p, const Orientation &o) { 110 | pose = Eigen::Translation3d(p) * o.quaternion; 111 | } 112 | 113 | Pose::Pose(const Eigen::Vector3d &p, const Eigen::Quaterniond &o) { 114 | pose = Eigen::Translation3d(p) * o; 115 | } 116 | 117 | Position Pose::getPosition() const { 118 | return translation(); 119 | } 120 | 121 | void Pose::setPosition(const Position &position) { 122 | pose = Eigen::Translation3d(position) * rotation(); 123 | } 124 | 125 | Orientation Pose::getOrientation() const { 126 | return {Eigen::Quaterniond(rotation())}; 127 | } 128 | 129 | void Pose::setOrientation(const Orientation &orientation) { 130 | pose = Eigen::Translation3d(translation()) * orientation.quaternion; 131 | } 132 | 133 | Pose::Pose(const Affine3d &pose) : pose(pose) {} 134 | 135 | void Pose::set_RPY(double r, double p, double y) { 136 | Eigen::Affine3d a; 137 | a = AngleAxisd(y, Vector3d::UnitZ()) 138 | * AngleAxisd(p, Vector3d::UnitY()) 139 | * AngleAxisd(r, Vector3d::UnitX()); 140 | pose = Eigen::Translation3d(translation()) * a; 141 | 142 | } 143 | 144 | void Pose::get_RPY(double &roll, double &pitch, double &yaw) const { 145 | //ZYX, yaw, pitch, roll 146 | Eigen::Vector3d rpy_vec = rotation().eulerAngles(2, 1, 0); 147 | 148 | roll = rpy_vec.z(); 149 | pitch = rpy_vec.y(); 150 | yaw = rpy_vec.x(); 151 | } 152 | 153 | void Pose::set_position(double x, double y, double z) { 154 | setPosition(Position(x, y, z)); 155 | } 156 | 157 | void Pose::add_position(double x, double y, double z) { 158 | pose = Eigen::Translation3d(x, y, z) * pose; 159 | } 160 | 161 | 162 | Pose Pose::operator*(const Pose &pose_b) const { 163 | return {pose * pose_b.pose}; 164 | } 165 | 166 | Eigen::Quaterniond Pose::quaternion() const { 167 | Eigen::Quaterniond quat; 168 | quat = rotation(); 169 | return quat; 170 | } 171 | 172 | Eigen::Vector3d Pose::translation() const { 173 | return pose.translation(); 174 | } 175 | 176 | Eigen::Affine3d::LinearMatrixType Pose::rotation() const { 177 | return pose.rotation(); 178 | } 179 | 180 | void Pose::add_RPY(double r, double p, double y) { 181 | setOrientation(Orientation(r, p, y) * quaternion()); 182 | } -------------------------------------------------------------------------------- /src/PoseGenerator.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /* 4 | * Implementation of Generator functions 5 | */ 6 | 7 | #include 8 | #include 9 | #include 10 | 11 | using namespace orl; 12 | 13 | PoseGenerator 14 | orl::generate_pose_generator(const PositionGenerator &position_generator, 15 | const OrientationGenerator &orientation_generator, 16 | const boost::optional &speed_profile) { 17 | PoseGenerator pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 18 | orl::Pose out(position_generator(input), orientation_generator(input).normalized()); 19 | return out; 20 | }; 21 | SpeedProfile concrete_speed_profile = speed_profile.value_or(SpeedProfiles::QuinticPolynomialProfile()); 22 | apply_speed_profile(pose_generator, concrete_speed_profile); 23 | return pose_generator; 24 | } 25 | 26 | OrientationGenerator 27 | orl::generate_constant_orientation_orientation_generator(const boost::optional &orientation) { 28 | if (orientation.is_initialized()) { 29 | return [=](const PoseGeneratorInput &input) -> Eigen::Quaterniond { 30 | return orientation.value(); 31 | }; 32 | } 33 | OrientationGenerator constant_orientation_generator = [=](const PoseGeneratorInput &input) -> Eigen::Quaterniond { 34 | return input.initial_pose.quaternion(); 35 | }; 36 | return constant_orientation_generator; 37 | } 38 | 39 | 40 | OrientationGenerator orl::generate_angle_axis_orientation_generator(const Eigen::AngleAxisd &angle_axis) { 41 | OrientationGenerator constant_orientation_generator = [=](const PoseGeneratorInput &input) -> Eigen::Quaterniond { 42 | Eigen::Quaterniond desired_orientation(angle_axis); 43 | desired_orientation *= input.initial_pose.quaternion(); 44 | return generate_orientation_interpolation_orientation_generator(desired_orientation)(input); 45 | }; 46 | return constant_orientation_generator; 47 | } 48 | 49 | PositionGenerator orl::generate_constant_position_position_generator(const boost::optional &position) { 50 | if (position.is_initialized()) { 51 | return [=](const PoseGeneratorInput &input) -> Eigen::Vector3d { 52 | return position.value(); 53 | }; 54 | } 55 | PositionGenerator constant_position_generator = [=](const PoseGeneratorInput &input) -> Eigen::Vector3d { 56 | return input.initial_pose.translation(); 57 | }; 58 | return constant_position_generator; 59 | } 60 | 61 | 62 | PoseGenerator orl::generate_pose_interpolation_pose_generator(const orl::Pose &end_pose) { 63 | orl::Pose d = end_pose; 64 | PoseGenerator pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 65 | return input.initial_pose.interpolate(d, input.progress); 66 | 67 | 68 | }; 69 | return pose_generator; 70 | } 71 | 72 | OrientationGenerator 73 | orl::generate_orientation_interpolation_orientation_generator(const Eigen::Quaterniond &end_orientation) { 74 | return [=](const PoseGeneratorInput &input) -> Eigen::Quaterniond { 75 | return input.initial_pose.quaternion().slerp(input.progress, end_orientation); 76 | }; 77 | } 78 | 79 | PoseGenerator orl::operator*(const PoseGenerator &lhs, const PoseGenerator &rhs) { 80 | PoseGenerator pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 81 | return lhs(input) * rhs(input); 82 | }; 83 | return pose_generator; 84 | } 85 | 86 | double orl::calculate_length(const PoseGenerator &pose_generator, const Pose &start_pose, int steps) { 87 | std::vector positions; 88 | positions.reserve(steps + 1); 89 | for (int i = 0; i <= steps; i++) { 90 | double progress = i / (double) steps; 91 | PoseGeneratorInput input{progress, start_pose, franka::RobotState()}; 92 | positions.push_back(pose_generator(input).getPosition()); 93 | } 94 | double sum = 0.0; 95 | for (int i = 0; i < int(positions.size()) - 1; ++i) { 96 | sum += (positions[i] - positions[i + 1]).norm(); 97 | } 98 | return sum; 99 | } 100 | 101 | 102 | PoseGenerator PoseGenerators::CirclePoseGenerator(const Position ¢er, double rotation_angle, const Plane &plane, 103 | const boost::optional &maybe_orientation_generator) { 104 | Eigen::Vector3d normal; 105 | switch (plane) { 106 | case Plane::XY: 107 | normal << 0, 0, 1; 108 | break; 109 | case Plane::YZ: 110 | normal << 1, 0, 0; 111 | break; 112 | case Plane::ZX: 113 | default: 114 | normal << 0, 1, 0; 115 | } 116 | PositionGenerator position_generator = [=](const PoseGeneratorInput &input) { 117 | auto circle_generator_initializer = [=](Plane plane) { 118 | 119 | switch (plane) { 120 | case Plane::XY: 121 | return generate_circle_movement_xy_plane(center, input.initial_pose.translation(), 122 | rotation_angle); 123 | case Plane::YZ: 124 | return generate_circle_movement_yz_plane(center, input.initial_pose.translation(), 125 | rotation_angle); 126 | case Plane::ZX: 127 | default: 128 | return generate_circle_movement_zx_plane(center, input.initial_pose.translation(), 129 | rotation_angle); 130 | } 131 | }; 132 | 133 | /* 134 | * \todo make this more efficient by not recalculating this every time. 135 | * Make sure that it still works if two threads use this function at the same time 136 | */ 137 | auto circle_generator = circle_generator_initializer(plane); 138 | return circle_generator(input.progress); 139 | }; 140 | 141 | Eigen::AngleAxisd orient_to_center(rotation_angle, normal); 142 | auto orientation_generator = maybe_orientation_generator.value_or( 143 | generate_angle_axis_orientation_generator(orient_to_center)); 144 | auto pose_generator = generate_pose_generator(position_generator, orientation_generator, 145 | SpeedProfiles::LinearProfile()); 146 | return pose_generator; 147 | } 148 | 149 | PoseGenerator PoseGenerators::RelativeMotion(const Position &translation, Frame frame, 150 | const boost::optional &maybe_orientation_generator) { 151 | 152 | PositionGenerator position_generator = [=](const PoseGeneratorInput &input) -> orl::Position { 153 | orl::Pose goal; 154 | orl::Pose start; 155 | switch (frame) { 156 | case Frame::RobotBase: 157 | goal = start = input.initial_pose; 158 | goal.add_position(translation.x(), translation.y(), translation.z()); 159 | break; 160 | case Frame::UnitFrame: 161 | goal = start = {Position(0, 0, 0), Orientation(0, 0, 0).quaternion}; 162 | goal.add_position(translation.x(), translation.y(), translation.z()); 163 | break; 164 | case Frame::RobotTCP: 165 | start = input.initial_pose; 166 | goal = start * Pose(translation, Eigen::Quaterniond()); 167 | } 168 | 169 | return start.getPosition() + (-start.getPosition() + goal.getPosition()) * input.progress; 170 | }; 171 | auto orientation_generator = maybe_orientation_generator.value_or( 172 | generate_constant_orientation_orientation_generator()); 173 | if (frame == Frame::UnitFrame) { 174 | orientation_generator = maybe_orientation_generator.value_or( 175 | generate_constant_orientation_orientation_generator(Orientation(0, 0, 0).quaternion)); 176 | } 177 | auto pose_generator = generate_pose_generator(position_generator, orientation_generator, 178 | SpeedProfiles::LinearProfile()); 179 | return pose_generator; 180 | } 181 | 182 | PoseGenerator PoseGenerators::AbsoluteMotion(const Position &translation, Frame frame, 183 | const boost::optional &maybe_orientation_generator) { 184 | PositionGenerator position_generator = [=](const PoseGeneratorInput &input) -> orl::Position { 185 | orl::Position goal; 186 | orl::Position start; 187 | switch (frame) { 188 | case Frame::RobotTCP: 189 | throw std::invalid_argument("RobotTCP in absolute motion does not make any sense"); 190 | case Frame::RobotBase: 191 | goal = start = input.initial_pose.getPosition(); 192 | goal = translation; 193 | break; 194 | case Frame::UnitFrame: 195 | std::cerr 196 | << "Absolute motion in Unit Frame equals Relative Motion in Unit Frame. Use Relative Motion the next time" 197 | << std::endl; 198 | goal = start = {0, 0, 0}; 199 | goal = (translation); 200 | break; 201 | 202 | } 203 | return start + (-start + goal) * input.progress; 204 | }; 205 | auto orientation_generator = maybe_orientation_generator.value_or( 206 | generate_constant_orientation_orientation_generator()); 207 | auto pose_generator = generate_pose_generator(position_generator, orientation_generator, 208 | SpeedProfiles::LinearProfile()); 209 | return pose_generator; 210 | } 211 | 212 | PoseGenerator PoseGenerators::BezierMotion(const std::vector &points, Frame frame, 213 | const boost::optional &maybe_orientation_generator) { 214 | PoseGenerator pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 215 | 216 | orl::Pose start; 217 | if (frame == Frame::UnitFrame) { 218 | start = Pose(Eigen::Vector3d(0, 0, 0), Eigen::Quaterniond()); 219 | } else { 220 | start = input.initial_pose; 221 | } 222 | std::vector points_with_start{start.getPosition()}; 223 | orl::Pose goal = start; 224 | goal.setPosition(points.at(points.size() - 1)); 225 | for (const auto &point : points) { 226 | points_with_start.push_back(point); 227 | } 228 | auto position_generator = generate_bezier_curve(points_with_start); 229 | auto orientation_generator = maybe_orientation_generator.value_or( 230 | generate_constant_orientation_orientation_generator()); 231 | return Pose(position_generator(input.progress), orientation_generator(input)); 232 | }; 233 | return pose_generator; 234 | } 235 | 236 | PoseGenerator PoseGenerators::MoveToPose(const Pose &dest, Frame frame) { 237 | PoseGenerator pose_generator; 238 | switch (frame) { 239 | case Frame::RobotTCP: 240 | throw std::invalid_argument("RobotTCP in MoveToPose does not make any sense"); 241 | case Frame::RobotBase: 242 | pose_generator = generate_pose_interpolation_pose_generator(dest); 243 | break; 244 | case Frame::UnitFrame: 245 | pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 246 | return Pose(Position(0, 0, 0), Eigen::Quaterniond()).interpolate(dest, input.progress); 247 | }; 248 | break; 249 | 250 | } 251 | return pose_generator; 252 | } 253 | 254 | PoseGenerator PoseGenerators::BSplineMotion(const std::vector &points, int degree, Frame frame, 255 | const boost::optional &maybe_orientation_generator, 256 | const boost::optional> &maybe_knot_vector) { 257 | if (points.size() < 2) { 258 | throw std::invalid_argument("To few points for B-Spline. Need at least 2"); 259 | } 260 | // add one to the points size, to make up for the initial position of the robot which is not known yet 261 | std::vector knot_vector = maybe_knot_vector.value_or(create_default_knot_vector(points.size() + 1, degree)); 262 | PoseGenerator pose_generator = [=](const PoseGeneratorInput &input) -> orl::Pose { 263 | 264 | orl::Pose start; 265 | if (frame == Frame::UnitFrame) { 266 | start = Pose(Eigen::Vector3d(0, 0, 0), Eigen::Quaterniond()); 267 | } else { 268 | start = input.initial_pose; 269 | } 270 | std::vector points_with_start{start.getPosition()}; 271 | orl::Pose goal = start; 272 | goal.setPosition(points.at(points.size() - 1)); 273 | for (const auto &point : points) { 274 | points_with_start.push_back(point); 275 | } 276 | auto position_generator = generate_b_spline(points_with_start, degree, knot_vector); 277 | auto orientation_generator = maybe_orientation_generator.value_or( 278 | generate_constant_orientation_orientation_generator()); 279 | return Pose(position_generator(input.progress), orientation_generator(input)); 280 | }; 281 | return pose_generator; 282 | } 283 | -------------------------------------------------------------------------------- /src/QuinticPolynomial.hpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Pouya Mohammadi 2 | // Licensed under the EUPL-1.2-or-later 3 | /* 4 | * File: QuinticPolynomial.hpp 5 | * Author: Pouya Mohammadi 6 | * 7 | * Created on Friday June 3, 2015, 11:57 8 | * This is based on quintic polynomial presented in 9 | * Prof. Allesandro de Luca's slides of robotics 1. 10 | */ 11 | 12 | #ifndef QUINTICPOLYNOMIAL_HPP 13 | #define QUINTICPOLYNOMIAL_HPP 14 | 15 | 16 | #include 17 | 18 | template 19 | class QuinticPolynomial { 20 | public: 21 | typedef Eigen::Matrix Vector; 22 | 23 | QuinticPolynomial() {} 24 | 25 | QuinticPolynomial(double start_time, double end_time, Vector init_conf, Vector final_conf) { 26 | this->start_time = start_time; 27 | this->end_time = end_time; 28 | this->deltaT = end_time - start_time; 29 | this->q_i = init_conf; 30 | this->q_f = final_conf; 31 | this->delta_q = q_f - q_i; 32 | this->dof = q_i.size(); 33 | } 34 | 35 | void setParams(double start_time, double end_time, Vector init_conf, Vector final_conf) { 36 | this->start_time = start_time; 37 | this->end_time = end_time; 38 | this->deltaT = end_time - start_time; 39 | this->q_i = init_conf; 40 | this->q_f = final_conf; 41 | this->delta_q = q_f - q_i; 42 | this->dof = q_i.size(); 43 | } 44 | 45 | Vector getQ(double time) const { 46 | Vector ret(dof); 47 | if (time >= end_time) 48 | time = end_time; 49 | 50 | double tau = (time - start_time) / (deltaT); 51 | for (int i = 0; i < dof; ++i) { 52 | ret(i) = q_i(i) + 53 | delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); 54 | } 55 | return ret; 56 | } 57 | 58 | void getQ(double time, Eigen::Matrix _ret) { 59 | // Vector ret(dof); 60 | if (time >= end_time) 61 | time = end_time; 62 | 63 | double tau = (time - start_time) / (deltaT); 64 | for (int i = 0; i < dof; ++i) { 65 | _ret(i) = q_i(i) + 66 | delta_q(i) * (6.0 * std::pow(tau, 5.0) - 15.0 * std::pow(tau, 4.0) + 10.0 * std::pow(tau, 3.0)); 67 | } 68 | } 69 | 70 | Vector getQd(double time) { 71 | Vector ret(dof); 72 | if (time >= end_time) 73 | time = end_time; 74 | 75 | double tau = (time - start_time) / (deltaT); 76 | for (int i = 0; i < dof; ++i) { 77 | ret(i) = delta_q(i) * (30.0 * std::pow(tau, 4.0) - 60.0 * std::pow(tau, 3.0) + 30.0 * std::pow(tau, 2.0)); 78 | } 79 | 80 | return ret; 81 | } 82 | 83 | void getQd(double time, Vector &_ret) { 84 | // Vector ret(dof); 85 | if (time >= end_time) 86 | time = end_time; 87 | 88 | double tau = (time - start_time) / (deltaT); 89 | for (int i = 0; i < dof; ++i) { 90 | _ret(i) = delta_q(i) * (30.0 * std::pow(tau, 4.0) - 60.0 * std::pow(tau, 3.0) + 30.0 * std::pow(tau, 2.0)); 91 | } 92 | } 93 | 94 | Vector getQdd(double time) { 95 | Vector ret(dof); 96 | if (time >= end_time) 97 | time = end_time; 98 | 99 | double tau = (time - start_time) / (deltaT); 100 | for (int i = 0; i < dof; ++i) { 101 | ret(i) = delta_q(i) * (120.0 * std::pow(tau, 3.0) - 180.0 * std::pow(tau, 2.0) + 60 * tau); 102 | } 103 | 104 | return ret; 105 | } 106 | 107 | void getQdd(double time, Vector &_ret) { 108 | // Vector ret(dof); 109 | if (time >= end_time) 110 | time = end_time; 111 | 112 | double tau = (time - start_time) / (deltaT); 113 | for (int i = 0; i < dof; ++i) { 114 | _ret(i) = delta_q(i) * (120.0 * std::pow(tau, 3.0) - 180.0 * std::pow(tau, 2.0) + 60 * tau); 115 | } 116 | } 117 | 118 | int getDofSize() { 119 | return this->dof; 120 | } 121 | 122 | // void setInitialConf(Vector init); 123 | private: 124 | double start_time; 125 | double end_time; 126 | double deltaT; 127 | 128 | int dof; 129 | 130 | Vector q_i; 131 | Vector q_f; 132 | Vector delta_q; 133 | }; 134 | 135 | #endif // QUINTICPOLYNOMIAL_HPP 136 | -------------------------------------------------------------------------------- /src/Robot.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #include 4 | #include 5 | #include 6 | #include "QuinticPolynomial.hpp" 7 | #include 8 | #include 9 | #include 10 | #include 11 | #include "examples_common.h" 12 | 13 | using namespace orl; 14 | 15 | void 16 | Robot::relative_cart_motion(double x, double y, double z, const double max_time, 17 | const boost::optional &stop_condition) { 18 | PoseGenerator pose_generator = PoseGenerators::RelativeMotion(Position(x, y, z), Frame::RobotBase); 19 | apply_speed_profile(pose_generator); 20 | move_cartesian(pose_generator, max_time, stop_condition); 21 | 22 | } 23 | 24 | 25 | void Robot::cart_motion(const Pose &dest, const double max_time, const boost::optional &stop_condition) { 26 | PoseGenerator pose_generator = PoseGenerators::MoveToPose(dest); 27 | apply_speed_profile(pose_generator); 28 | move_cartesian(pose_generator, max_time, stop_condition); 29 | } 30 | 31 | 32 | void Robot::close_gripper(double width, double speed, double force, double epsilon_width) { 33 | /* 34 | * Maximum tolerated deviation when the actual grasped width is smaller than the commanded grasp width. 35 | [in] 36 | epsilon_outer 37 | Maximum tolerated deviation when the actual grasped width is larger than the commanded grasp width. 38 | */ 39 | gripper.grasp(width, speed, force, epsilon_width, epsilon_width); 40 | } 41 | 42 | void Robot::open_gripper(double speed, double width) { 43 | gripper.move(width, speed); 44 | } 45 | 46 | void Robot::wait_milliseconds(int milliseconds) { 47 | std::this_thread::sleep_for(std::chrono::milliseconds(milliseconds)); 48 | } 49 | 50 | Pose Robot::get_current_pose() { 51 | return Pose(robot.readOnce().O_T_EE_c); 52 | } 53 | 54 | std::array Robot::get_current_Joints() { 55 | return robot.readOnce().q; 56 | } 57 | 58 | Robot::Robot(const std::string &robot_name) : robot(robot_name), gripper(robot_name), model(robot.loadModel()) { 59 | robot.setGuidingMode({true, true, true, true, true, true}, false); 60 | // robot.automaticErrorRecovery(); 61 | 62 | 63 | // Set additional parameters always before the control loop, NEVER in the control loop! 64 | // Set collision behavior. 65 | setDefaultBehavior(); 66 | Payload zero_payload = Payloads::Sphere(0, 0, {0, 0, 0}); 67 | setLoad(zero_payload); 68 | } 69 | 70 | void Robot::setDefaultBehavior() { 71 | robot.setCollisionBehavior( 72 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 73 | {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, 74 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, 75 | {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); 76 | robot.setJointImpedance({{3000, 3000, 3000, 2500, 2500, 2000, 2000}}); 77 | robot.setCartesianImpedance({{3000, 3000, 3000, 300, 300, 300}}); 78 | } 79 | 80 | franka::Model &Robot::get_franka_model() { 81 | return model; 82 | } 83 | 84 | franka::Robot &Robot::get_franka_robot() { 85 | return robot; 86 | } 87 | 88 | void 89 | Robot::impedance_mode(double translational_stiffness, double rotational_stiffness, const Pose &desired_attractor_pose, 90 | double max_time, double move_to_attractor_duration) { 91 | auto attractor_pose_generator = PoseGenerators::MoveToPose(desired_attractor_pose); 92 | apply_speed_profile(attractor_pose_generator); 93 | impedance_mode(translational_stiffness, rotational_stiffness, max_time, attractor_pose_generator, 94 | move_to_attractor_duration); 95 | } 96 | 97 | void 98 | Robot::impedance_mode(double translational_stiffness, double rotational_stiffness, 99 | double max_time, const PoseGenerator &attractor_pose_generator, 100 | double move_to_attractor_duration) { 101 | 102 | Eigen::MatrixXd stiffness(6, 6), damping(6, 6); 103 | stiffness.setZero(); 104 | stiffness.topLeftCorner(3, 3) << translational_stiffness * Eigen::MatrixXd::Identity(3, 3); 105 | stiffness.bottomRightCorner(3, 3) << rotational_stiffness * Eigen::MatrixXd::Identity(3, 3); 106 | damping.setZero(); 107 | damping.topLeftCorner(3, 3) << 2.0 * sqrt(translational_stiffness) * 108 | Eigen::MatrixXd::Identity(3, 3); 109 | damping.bottomRightCorner(3, 3) << 2.0 * sqrt(rotational_stiffness) * 110 | Eigen::MatrixXd::Identity(3, 3); 111 | try { 112 | // connect to robot 113 | franka::RobotState initial_state = robot.readOnce(); 114 | orl::Pose initial_pose(initial_state.O_T_EE); 115 | // equilibrium point is the initial position 116 | // Eigen::Affine3d initial_transform(Eigen::Matrix4d::Map(desired_attractor_pose.to_matrix().data())); 117 | // Eigen::Vector3d position_d(initial_transform.translation()); 118 | // Eigen::Quaterniond orientation_d(initial_transform.linear()); 119 | // set collision behavior 120 | robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 121 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 122 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, 123 | {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); 124 | // define callback for the torque control loop 125 | double time = 0; 126 | std::function 127 | impedance_control_callback = [&](const franka::RobotState &robot_state, 128 | franka::Duration duration) -> franka::Torques { 129 | // get state variables 130 | time += duration.toSec(); 131 | if (time == 0) { 132 | initial_pose.set(robot_state.O_T_EE_c); 133 | // Create 1D interpolation progress variable 134 | Eigen::Vector3d init1, fin1; 135 | init1 << 0, 0, 0; 136 | fin1 << 1, 1, 1; 137 | 138 | } 139 | const double progress = time / move_to_attractor_duration; 140 | PoseGeneratorInput generatorInput{progress, initial_pose, robot_state}; 141 | 142 | orl::Pose new_pose(attractor_pose_generator(generatorInput)); 143 | Eigen::Affine3d progress_transform(Eigen::Matrix4d::Map(new_pose.to_matrix().data())); 144 | Eigen::Vector3d position_d(progress_transform.translation()); 145 | Eigen::Quaterniond orientation_d(progress_transform.linear()); 146 | std::array coriolis_array = model.coriolis(robot_state); 147 | std::array jacobian_array = 148 | model.zeroJacobian(franka::Frame::kEndEffector, robot_state); 149 | // convert to Eigen 150 | Eigen::Map> coriolis(coriolis_array.data()); 151 | Eigen::Map> jacobian(jacobian_array.data()); 152 | Eigen::Map> q(robot_state.q.data()); 153 | Eigen::Map> dq(robot_state.dq.data()); 154 | 155 | Eigen::Affine3d transform(Eigen::Matrix4d::Map(robot_state.O_T_EE.data())); 156 | Eigen::Vector3d position(transform.translation()); 157 | Eigen::Quaterniond orientation(transform.linear()); 158 | // compute error to desired equilibrium pose 159 | // position error 160 | Eigen::Matrix error; 161 | error.head(3) << position - position_d; 162 | // orientation error 163 | // "difference" quaternion 164 | if (orientation_d.coeffs().dot(orientation.coeffs()) < 0.0) { 165 | orientation.coeffs() << -orientation.coeffs(); 166 | } 167 | // "difference" quaternion 168 | Eigen::Quaterniond error_quaternion(orientation.inverse() * orientation_d); 169 | error.tail(3) << error_quaternion.x(), error_quaternion.y(), error_quaternion.z(); 170 | // Transform to base frame 171 | error.tail(3) << -transform.linear() * error.tail(3); 172 | // compute control 173 | Eigen::VectorXd tau_task(7), tau_d(7); 174 | // Spring damper system with damping ratio=1 175 | tau_task << jacobian.transpose() * (-stiffness * error - damping * (jacobian * dq)); 176 | tau_d << tau_task + coriolis; 177 | std::array tau_d_array{}; 178 | Eigen::VectorXd::Map(&tau_d_array[0], 7) = tau_d; 179 | if (time > max_time) { 180 | return franka::MotionFinished(franka::Torques(tau_d_array)); 181 | } 182 | return tau_d_array; 183 | }; 184 | robot.control(impedance_control_callback); 185 | } catch (const franka::Exception &ex) { 186 | // print exception 187 | std::cout << ex.what() << std::endl; 188 | } 189 | setDefaultBehavior(); 190 | } 191 | 192 | void Robot::impedance_mode(double translational_stiffness, double rotational_stiffness, double max_time) { 193 | impedance_mode(translational_stiffness, rotational_stiffness, get_current_pose(), max_time, 1); 194 | } 195 | 196 | void Robot::move_cartesian(PoseGenerator cartesian_pose_generator, double max_time, 197 | const boost::optional &stop_condition, boost::optional elbow) { 198 | Pose initial_pose(robot.readOnce().O_T_EE_c); 199 | std::array initial_elbow; 200 | 201 | double time = 0.0; 202 | 203 | // std::cout << "Start motion..." << std::endl; 204 | bool should_stop = false; 205 | try { 206 | robot.control( 207 | [=, &initial_elbow, &time, &max_time, &initial_pose, &cartesian_pose_generator, &should_stop]( 208 | const franka::RobotState &state, 209 | franka::Duration time_step) -> franka::CartesianPose { 210 | time += time_step.toSec(); 211 | if (time == 0) { 212 | initial_pose.set(state.O_T_EE_c); 213 | initial_elbow = state.elbow_c; 214 | } 215 | auto new_elbow = initial_elbow; 216 | const double progress = time / max_time; 217 | PoseGeneratorInput generatorInput{progress, initial_pose, state}; 218 | Pose new_pose_pose = cartesian_pose_generator(generatorInput); 219 | if (time == 0) { 220 | // std::cout << "desired pose: " << new_pose_pose.toString() << std::endl; 221 | // std::cout << "start: " << initial_pose.toString() << std::endl; 222 | } 223 | std::array new_pose = new_pose_pose.to_matrix(); 224 | // std::cout << new_pose_pose.toString() << std::endl << std::endl; 225 | // std::cout << Pose(state.O_T_EE).toString() << std::endl << std::endl; 226 | // for (int i = 0; i < 4; i++) { 227 | // for (int j = 0; j < 4; j++) { 228 | // std::cout << new_pose[i + 4 * j] << "\t"; 229 | // } 230 | // std::cout << std::endl; 231 | // } 232 | // Eigen::Vector3d a; 233 | // a << new_pose[0], new_pose[1], new_pose[2]; 234 | // Eigen::Vector3d b; 235 | // b << new_pose[4], new_pose[5], new_pose[6]; 236 | // Eigen::Vector3d c; 237 | // c << new_pose[8], new_pose[9], new_pose[10]; 238 | // std::cout << a.norm() << " " << b.norm() << " " << c.norm() << std::endl; 239 | // std::cout << std::endl; 240 | 241 | // Print the xyz-forces at the EE in every step of control loop: 242 | // double force_norm = sqrt(state.O_F_ext_hat_K[0] * state.O_F_ext_hat_K[0] + 243 | // state.O_F_ext_hat_K[1] * state.O_F_ext_hat_K[1] + 244 | // state.O_F_ext_hat_K[2] * state.O_F_ext_hat_K[2]); 245 | // std::cout << "Force: " << force_norm << std::endl; 246 | if (elbow.is_initialized()) { 247 | new_elbow[0] += (elbow.value() - initial_elbow[0]) * (1 - std::cos(M_PI * progress)) / 2.0; 248 | } 249 | if (stop_condition.is_initialized()) { 250 | should_stop = stop_condition.value()(generatorInput); 251 | 252 | 253 | } 254 | if (time >= max_time or should_stop) { 255 | if (should_stop) { 256 | // std::cout << "Stopped motion by stop condition" << std::endl; 257 | } 258 | // std::cout << std::endl << "Finished motion, shutting down example" << std::endl; 259 | if (elbow.is_initialized()) { 260 | return franka::MotionFinished({new_pose, new_elbow}); 261 | } 262 | return franka::MotionFinished(new_pose); 263 | } 264 | if (elbow.is_initialized()) { 265 | return {new_pose, new_elbow}; 266 | } 267 | return new_pose; 268 | }, franka::ControllerMode::kCartesianImpedance); 269 | } catch (franka::Exception exception) { 270 | std::cout << exception.what() << std::endl; 271 | if (should_stop) { 272 | std::cout << "robot has stopped due to StopCondition" << std::endl; 273 | robot.automaticErrorRecovery(); 274 | } else { 275 | throw std::move(exception); 276 | } 277 | } 278 | 279 | } 280 | 281 | void Robot::move_circle_xy(const Position ¢er, double rotation_angle, double max_time, 282 | const boost::optional &maybe_orientation_generator, 283 | const boost::optional &stop_condition) { 284 | move_circle(center, rotation_angle, max_time, Plane::XY, maybe_orientation_generator, stop_condition); 285 | } 286 | 287 | void Robot::move_circle_yz(const Position ¢er, double rotation_angle, double max_time, 288 | const boost::optional &maybe_orientation_generator, 289 | const boost::optional &stop_condition) { 290 | move_circle(center, rotation_angle, max_time, Plane::YZ, maybe_orientation_generator, stop_condition); 291 | } 292 | 293 | void Robot::move_circle_zx(const Position ¢er, double rotation_angle, double max_time, 294 | const boost::optional &maybe_orientation_generator, 295 | const boost::optional &stop_condition) { 296 | move_circle(center, rotation_angle, max_time, Plane::ZX, maybe_orientation_generator, stop_condition); 297 | } 298 | 299 | void Robot::line_gripper_up_to_orientation(const Orientation &desired_orientation, double max_time) { 300 | auto orientation_gen = generate_orientation_interpolation_orientation_generator(desired_orientation.quaternion); 301 | auto a = generate_pose_generator(generate_constant_position_position_generator(), orientation_gen); 302 | move_cartesian(a, max_time); 303 | 304 | } 305 | 306 | void Robot::move_circle(const Position ¢er, double rotation_angle, double max_time, Plane plane, 307 | const boost::optional &maybe_orientation_generator, 308 | const boost::optional &stop_condition) { 309 | 310 | 311 | PoseGenerator pose_generator = PoseGenerators::CirclePoseGenerator(center, rotation_angle, plane, 312 | maybe_orientation_generator); 313 | apply_speed_profile(pose_generator); 314 | move_cartesian(pose_generator, max_time, stop_condition); 315 | } 316 | 317 | franka::Gripper &Robot::get_franka_gripper() { 318 | return gripper; 319 | } 320 | 321 | void Robot::double_tap_robot_to_continue() { 322 | auto s = robot.readOnce(); 323 | int touch_counter = false; 324 | bool can_be_touched_again = true; 325 | Eigen::Vector3d start_force; 326 | auto last_time_something_happened = std::chrono::system_clock::now(); 327 | auto last_time_something_touched = std::chrono::system_clock::now(); 328 | start_force << s.O_F_ext_hat_K[0], s.O_F_ext_hat_K[1], s.O_F_ext_hat_K[2]; 329 | robot.read( 330 | [&]( 331 | const franka::RobotState &robot_state) { 332 | // std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< 2 and can_be_touched_again) { 340 | touch_counter++; 341 | can_be_touched_again = false; 342 | last_time_something_happened = std::chrono::system_clock::now(); 343 | last_time_something_touched = std::chrono::system_clock::now(); 344 | } 345 | if (touch_counter > 0 and (start_force - force).norm() < 1) { 346 | can_be_touched_again = true; 347 | last_time_something_happened = std::chrono::system_clock::now(); 348 | } 349 | std::chrono::duration elapse_time_touched = 350 | std::chrono::system_clock::now() - last_time_something_touched; 351 | if (elapse_time_touched.count() > 0.5) { 352 | touch_counter = 0; 353 | } 354 | std::chrono::duration elapse_time_happend = 355 | std::chrono::system_clock::now() - last_time_something_happened; 356 | if (elapse_time_happend.count() > 2) { 357 | start_force = force; 358 | last_time_something_happened = std::chrono::system_clock::now(); 359 | 360 | } 361 | return touch_counter < 2; 362 | }); 363 | wait_milliseconds(100); 364 | } 365 | 366 | void Robot::force_to_continue(double force, Axis axis) { 367 | 368 | auto s = robot.readOnce(); 369 | 370 | Eigen::Vector3d start_force; 371 | start_force << s.O_F_ext_hat_K[0], s.O_F_ext_hat_K[1], s.O_F_ext_hat_K[2]; 372 | robot.read( 373 | [&]( 374 | const franka::RobotState &robot_state) { 375 | // std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< time_out * 1000) { 405 | return false; 406 | } 407 | return std::abs(start_force[axis] - force_v[axis]) < force; 408 | }); 409 | return counter < time_out * 1000; 410 | } 411 | 412 | void Robot::torque_to_continue(double torque, Axis axis) { 413 | 414 | auto s = robot.readOnce(); 415 | 416 | Eigen::Vector3d start_force; 417 | start_force << s.O_F_ext_hat_K[3], s.O_F_ext_hat_K[4], s.O_F_ext_hat_K[5]; 418 | robot.read( 419 | [&]( 420 | const franka::RobotState &robot_state) { 421 | // std::cout << Pose(robot_state.O_T_EE).orientation.quaternion.toRotationMatrix() << std::endl< time_out * 1000) { 452 | return false; 453 | } 454 | return std::abs(start_force[axis] - force_v[axis]) < torque; 455 | }); 456 | return counter < time_out * 1000; 457 | } 458 | 459 | void Robot::setLoad(const Payload &load) { 460 | this->payload = load; 461 | this->robot.setLoad(payload.mass, payload.pos_wrt_flange, payload.inertia_matrix); 462 | } 463 | 464 | void Robot::absolute_cart_motion(double x, double y, double z, double max_time, 465 | const boost::optional &stop_condition) { 466 | PoseGenerator pose_generator = PoseGenerators::AbsoluteMotion(Position(x, y, z), Frame::RobotBase); 467 | apply_speed_profile(pose_generator); 468 | move_cartesian(pose_generator, max_time, stop_condition); 469 | } 470 | 471 | void Robot::joint_motion(std::array q_goal, double speed_factor) { 472 | MotionGenerator motion_generator(speed_factor, q_goal); 473 | robot.control(motion_generator); 474 | } -------------------------------------------------------------------------------- /src/SCurve.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #include 4 | 5 | using namespace std; 6 | using namespace SCurve; 7 | 8 | double SCurve::deceleration_phase(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { 9 | const auto × = parameters.times; 10 | 11 | if (t <= times.T() - times.T_d + times.T_j2) { 12 | 13 | if (derivative == Derivative::Position) { 14 | return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + 15 | parameters.v_lim * (t - times.T() + times.T_d) - parameters.j_max * pow( 16 | t - times.T() + times.T_d, 3) / 6; 17 | } 18 | if (derivative == Derivative::Velocity) { 19 | return parameters.v_lim - parameters.j_max * pow(t - times.T() + times.T_d, 2) / 2; 20 | } 21 | if (derivative == Derivative::Acceleration) { 22 | return -parameters.j_max * (t - times.T() + times.T_d); 23 | } 24 | if (derivative == Derivative::Jerk) { 25 | return parameters.j_min; 26 | } 27 | } 28 | if (t <= times.T() - times.T_j2) { 29 | 30 | if (derivative == Derivative::Position) { 31 | return parameters.s.q1 - (parameters.v_lim + parameters.s.v1) * times.T_d / 2 + 32 | parameters.v_lim * (t - times.T() + times.T_d) + 33 | parameters.a_lim_d / 6 * ( 34 | 3 * pow(t - times.T() + times.T_d, 2) - 3 * times.T_j2 * ( 35 | t - times.T() + times.T_d) + pow(times.T_j2, 2)); 36 | 37 | } 38 | if (derivative == Derivative::Velocity) { 39 | return parameters.v_lim + parameters.a_lim_d * (t - times.T() + times.T_d - times.T_j2 / 2); 40 | } 41 | if (derivative == Derivative::Acceleration) { 42 | return parameters.a_lim_d; 43 | } 44 | if (derivative == Derivative::Jerk) { 45 | return 0; 46 | } 47 | 48 | } 49 | if (t <= times.T()) { 50 | 51 | if (derivative == Derivative::Position) { 52 | return parameters.s.q1 - parameters.s.v1 * (times.T() - t) - parameters.j_max * pow(times.T() - t, 3) / 6; 53 | } 54 | if (derivative == Derivative::Velocity) { 55 | return parameters.s.v1 + parameters.j_max * pow(times.T() - t, 2) / 2; 56 | } 57 | if (derivative == Derivative::Acceleration) { 58 | return -parameters.j_max * (times.T() - t); 59 | } 60 | if (derivative == Derivative::Jerk) { 61 | return parameters.j_max; 62 | } 63 | } else { 64 | std::stringstream ss; 65 | ss << "t should be between " << times.T_a + times.T_v << " and " << times.T() << " but it is " << t; 66 | 67 | throw std::invalid_argument(ss.str()); 68 | } 69 | } 70 | 71 | double SCurve::eval_s_curve(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { 72 | const auto × = parameters.times; 73 | if (t > times.T()) { 74 | t = times.T(); 75 | } 76 | if (t <= times.T_a) 77 | return acceleration_phase(parameters, t, derivative); 78 | if (t <= times.T_a + times.T_v) 79 | return constant_velocity_phase(parameters, t, derivative); 80 | if (t <= times.T()) 81 | return deceleration_phase(parameters, t, derivative); 82 | else { 83 | std::stringstream ss; 84 | ss << "t should be between 0 and " << times.T() << " but it is " << t; 85 | throw std::invalid_argument(ss.str()); 86 | } 87 | } 88 | 89 | double SCurve::constant_velocity_phase(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { 90 | const auto × = parameters.times; 91 | if (derivative == Derivative::Position) { 92 | return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 + 93 | parameters.v_lim * (t - times.T_a); 94 | } 95 | if (derivative == Derivative::Velocity) { 96 | return parameters.v_lim; 97 | } 98 | if (derivative == Derivative::Acceleration) { 99 | return 0; 100 | } 101 | if (derivative == Derivative::Jerk) { 102 | return 0; 103 | } 104 | } 105 | 106 | double SCurve::acceleration_phase(const SCurveTrajectoryParameters ¶meters, double t, Derivative derivative) { 107 | const auto × = parameters.times; 108 | if (t <= times.T_j1) { 109 | if (derivative == Derivative::Position) { 110 | return parameters.s.q0 + parameters.s.v0 * t + parameters.j_max * pow(t, 3) / 6; 111 | } 112 | if (derivative == Derivative::Velocity) { 113 | return parameters.s.v0 + parameters.j_max * pow(t, 2) / 2; 114 | } 115 | if (derivative == Derivative::Acceleration) { 116 | return parameters.j_max * t; 117 | } 118 | if (derivative == Derivative::Jerk) { 119 | return parameters.j_max; 120 | } 121 | } 122 | if (t <= times.T_a - times.T_j1) { 123 | 124 | if (derivative == Derivative::Position) { 125 | return parameters.s.q0 + parameters.s.v0 * t + 126 | parameters.a_lim_a / 6 * (3 * pow(t, 2) - 3 * times.T_j1 * t + pow(times.T_j1, 2)); 127 | } 128 | if (derivative == Derivative::Velocity) { 129 | return parameters.s.v0 + parameters.a_lim_a * (t - times.T_j1 / 2); 130 | } 131 | if (derivative == Derivative::Acceleration) { 132 | return parameters.a_lim_a; 133 | } 134 | if (derivative == Derivative::Jerk) { 135 | return 0; 136 | } 137 | } 138 | if (t <= times.T_a) { 139 | 140 | if (derivative == Derivative::Position) { 141 | return parameters.s.q0 + (parameters.v_lim + parameters.s.v0) * times.T_a / 2 - 142 | parameters.v_lim * (times.T_a - t) - parameters.j_min * pow( 143 | times.T_a - t, 3) / 6; 144 | } 145 | if (derivative == Derivative::Velocity) { 146 | return parameters.v_lim + parameters.j_min * pow(times.T_a - t, 2) / 2; 147 | } 148 | if (derivative == Derivative::Acceleration) { 149 | return -parameters.j_min * (times.T_a - t); 150 | } 151 | if (derivative == Derivative::Jerk) { 152 | return parameters.j_min; 153 | } 154 | } else { 155 | std::stringstream ss; 156 | ss << "t should be between " << 0 << " and " << times.T_a << " but it is " << t; 157 | throw std::invalid_argument(ss.str()); 158 | } 159 | } 160 | 161 | SCurveTrajectoryParameters SCurve::generate_trajectory_parameters(const SCurveTimings ×, 162 | const SCurveInitializationParameters &initialization) { 163 | double a_lim_a = initialization.constraints.max_jerk * times.T_j1; 164 | double a_lim_d = -initialization.constraints.max_jerk * times.T_j2; 165 | double v_lim = initialization.startConditions.v0 + (times.T_a - times.T_j1) * a_lim_a; 166 | 167 | return SCurveTrajectoryParameters{times, initialization.constraints.max_jerk, -initialization.constraints.max_jerk, 168 | a_lim_a, a_lim_d, v_lim, 169 | {initialization.startConditions.q0, initialization.startConditions.q1, 170 | initialization.startConditions.v0, 171 | initialization.startConditions.v1}}; 172 | } 173 | 174 | std::pair> 175 | SCurve::s_curve_generator(const SCurveInitializationParameters &init_params, Derivative derivative) { 176 | auto times = init_params.calc_times(); 177 | auto params = generate_trajectory_parameters(times, init_params); 178 | auto s_curve = [=](double t) { 179 | return eval_s_curve(params, t, derivative); 180 | }; 181 | return {params, s_curve}; 182 | 183 | } 184 | 185 | SCurveTimings & 186 | SCurveInitializationParameters::handle_negative_acceleration_time(SCurveTimings ×, 187 | const SCurveInitializationParameters &new_initialization) const { 188 | if (times.T_a < 0) { 189 | times.T_j1 = 0; 190 | times.T_a = 0; 191 | times.T_d = 2 * startConditions.h() / (startConditions.v0 + startConditions.v1); 192 | times.T_j2 = (new_initialization.constraints.max_jerk * startConditions.h() - 193 | sqrt(new_initialization.constraints.max_jerk * 194 | (new_initialization.constraints.max_jerk * pow(startConditions.h(), 2) + 195 | pow(startConditions.v0 + startConditions.v1, 2) * ( 196 | startConditions.v1 - startConditions.v0)))) / ( 197 | new_initialization.constraints.max_jerk * (startConditions.v1 + startConditions.v0)); 198 | } 199 | if (times.T_d < 0) { 200 | times.T_j2 = 0; 201 | times.T_d = 0; 202 | times.T_a = 2 * startConditions.h() / (startConditions.v0 + startConditions.v1); 203 | times.T_j2 = (new_initialization.constraints.max_jerk * startConditions.h() - 204 | sqrt(new_initialization.constraints.max_jerk * 205 | (new_initialization.constraints.max_jerk * pow(startConditions.h(), 2) - 206 | pow(startConditions.v0 + startConditions.v1, 2) * ( 207 | startConditions.v1 - startConditions.v0)))) / ( 208 | new_initialization.constraints.max_jerk * (startConditions.v1 + startConditions.v0)); 209 | } 210 | return times; 211 | } 212 | 213 | SCurveTimings SCurveInitializationParameters::calc_times_case_1() const { 214 | SCurveTimings times; 215 | SCurveInitializationParameters new_input = *this; 216 | if (is_a_max_not_reached()) { 217 | times.T_j1 = sqrt((new_input.constraints.max_velocity - startConditions.v0) / new_input.constraints.max_jerk); 218 | times.T_a = 2 * times.T_j1; 219 | } else { 220 | times.T_j1 = new_input.constraints.max_acceleration / new_input.constraints.max_jerk; 221 | times.T_a = times.T_j1 + 222 | (new_input.constraints.max_velocity - startConditions.v0) / new_input.constraints.max_acceleration; 223 | } 224 | 225 | if (is_a_min_not_reached()) { 226 | times.T_j2 = sqrt((new_input.constraints.max_velocity - startConditions.v1) / new_input.constraints.max_jerk); 227 | times.T_d = 2 * times.T_j2; 228 | } else { 229 | times.T_j2 = new_input.constraints.max_acceleration / new_input.constraints.max_jerk; 230 | times.T_d = times.T_j2 + 231 | (new_input.constraints.max_velocity - startConditions.v1) / new_input.constraints.max_acceleration; 232 | } 233 | 234 | times.T_v = startConditions.h() / new_input.constraints.max_velocity - 235 | times.T_a / 2 * (1 + startConditions.v0 / new_input.constraints.max_velocity) - 236 | times.T_d / 2 * (1 + startConditions.v1 / new_input.constraints.max_velocity); 237 | if (times.T_v <= 0) { 238 | return calc_times_case_2(); 239 | } 240 | if (times.is_max_acceleration_not_reached()) { 241 | new_input.constraints.max_acceleration *= 0.99; 242 | if (new_input.constraints.max_acceleration > 0.1) { 243 | return new_input.calc_times_case_2(); 244 | } 245 | new_input.constraints.max_acceleration = 0; 246 | } 247 | times = handle_negative_acceleration_time(times, new_input); 248 | 249 | return times; 250 | } 251 | 252 | SCurveTimings SCurveInitializationParameters::calc_times_case_2() const { 253 | SCurveTimings times; 254 | SCurveInitializationParameters new_input = *this; 255 | times.T_j1 = new_input.constraints.max_acceleration / new_input.constraints.max_jerk; 256 | times.T_j2 = new_input.constraints.max_acceleration / new_input.constraints.max_jerk; 257 | double delta = pow(new_input.constraints.max_acceleration, 4) / pow(new_input.constraints.max_jerk, 2) + 258 | 2 * (pow(startConditions.v0, 2) + pow(startConditions.v1, 2)) + 259 | new_input.constraints.max_acceleration * (4 * startConditions.h() - 260 | 2 * new_input.constraints.max_acceleration / 261 | new_input.constraints.max_jerk * 262 | (startConditions.v0 + startConditions.v1)); 263 | times.T_a = 264 | (pow(new_input.constraints.max_acceleration, 2) / new_input.constraints.max_jerk - 2 * startConditions.v0 + 265 | sqrt(delta)) / (2 * new_input.constraints.max_acceleration); 266 | times.T_d = 267 | (pow(new_input.constraints.max_acceleration, 2) / new_input.constraints.max_jerk - 2 * startConditions.v1 + 268 | sqrt(delta)) / (2 * new_input.constraints.max_acceleration); 269 | times.T_v = 0; 270 | if (times.is_max_acceleration_not_reached()) { 271 | new_input.constraints.max_acceleration *= 0.99; 272 | if (new_input.constraints.max_acceleration > 0.1) { 273 | return new_input.calc_times_case_2(); 274 | } 275 | new_input.constraints.max_acceleration = 0; 276 | } 277 | times = handle_negative_acceleration_time(times, new_input); 278 | 279 | return times; 280 | } 281 | 282 | bool SCurveInitializationParameters::is_a_max_not_reached() const { 283 | return (constraints.max_velocity - startConditions.v0) * constraints.max_jerk < 284 | pow(constraints.max_acceleration, 2); 285 | } 286 | 287 | bool SCurveInitializationParameters::is_a_min_not_reached() const { 288 | return (constraints.max_velocity - startConditions.v1) * constraints.max_jerk < 289 | pow(constraints.max_acceleration, 2); 290 | } 291 | 292 | bool SCurveInitializationParameters::is_trajectory_feasible() const { 293 | double T_j_star = fmin(sqrt(abs(startConditions.v1 - startConditions.v0) / constraints.max_jerk), 294 | constraints.max_acceleration / constraints.max_jerk); 295 | if (T_j_star == constraints.max_acceleration / constraints.max_jerk) { 296 | return startConditions.h() > 0.5 * (startConditions.v1 + startConditions.v0) * (T_j_star + 297 | abs(startConditions.v1 - 298 | startConditions.v0) / 299 | constraints.max_acceleration); 300 | } 301 | if (T_j_star < constraints.max_acceleration / constraints.max_jerk) { 302 | return startConditions.h() > T_j_star * (startConditions.v0 + startConditions.v1); 303 | } 304 | return false; 305 | } 306 | 307 | SCurveTimings SCurveInitializationParameters::calc_times() const { 308 | return calc_times_case_1(); 309 | } 310 | 311 | double SCurveStartConditions::h() const { 312 | return q1 - q0; 313 | } 314 | 315 | bool SCurveTimings::is_max_acceleration_not_reached() const { 316 | return T_a < 2 * T_j1 or T_d < 2 * T_j2; 317 | } 318 | 319 | double SCurveTimings::T() const { 320 | return T_a + T_v + T_d; 321 | } 322 | -------------------------------------------------------------------------------- /src/SpeedProfile.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #include 4 | #include "QuinticPolynomial.hpp" 5 | #include 6 | using namespace orl; 7 | 8 | void orl::apply_speed_profile(PoseGenerator &pose_generator, const SpeedProfile &speed_profile) { 9 | pose_generator = [pose_generator, speed_profile](const PoseGeneratorInput &input) { 10 | PoseGeneratorInput new_input = input; 11 | new_input.progress = speed_profile(input.progress); 12 | return pose_generator(new_input); 13 | }; 14 | } 15 | 16 | SpeedProfile SpeedProfiles::QuinticPolynomialProfile() { 17 | Eigen::Vector3d init; 18 | init << 0, 0, 0; 19 | Eigen::Vector3d fin; 20 | fin << 1, 1, 1; 21 | QuinticPolynomial qp(0, 1, init, fin); 22 | qp.setParams(0, 1, init, fin); 23 | return [qp](double progress) { 24 | return qp.getQ(progress).x(); 25 | }; 26 | } 27 | 28 | SpeedProfile SpeedProfiles::LinearProfile() { 29 | return [](double progress) { 30 | return progress; 31 | }; 32 | } 33 | 34 | SpeedProfile SpeedProfiles::CosineProfile() { 35 | return [](double progress) { 36 | return (1 - std::cos(M_PI * progress)) / 2.0; 37 | }; 38 | } 39 | 40 | SpeedProfile SpeedProfiles::SCurveProfile(const SCurve::SCurveTrajectoryParameters ¶ms) { 41 | const double trajectory_duration = params.times.T(); 42 | return [=](double progress) { 43 | return SCurve::eval_s_curve(params, progress * trajectory_duration, SCurve::Derivative::Position); 44 | }; 45 | 46 | } 47 | 48 | SpeedProfile SpeedProfiles::SCurveProfile(const PoseGenerator &pg, const Position &initial_position, double jerk, 49 | double acceleration, double velocity, double &trajectory_duration) { 50 | SCurve::SCurveTrajectoryParameters params; 51 | double trajectory_length = calculate_length(pg, Pose(initial_position, Eigen::Quaterniond())); 52 | SCurve::SCurveInitializationParameters input{{jerk, acceleration, velocity}, 53 | {0, trajectory_length, 0, 0}}; 54 | auto times = input.calc_times(); 55 | params = SCurve::generate_trajectory_parameters(times, input); 56 | trajectory_duration = params.times.T(); 57 | return [=](double progress) { 58 | return SCurve::eval_s_curve(params, progress * trajectory_duration, SCurve::Derivative::Position) / 59 | trajectory_length; 60 | }; 61 | 62 | } 63 | 64 | SpeedProfile SpeedProfiles::SCurveProfile(const PoseGenerator &pg, const Position &initial_position, double jerk, 65 | double acceleration, double velocity, 66 | SCurve::SCurveTrajectoryParameters ¶ms) { 67 | double trajectory_length = calculate_length(pg, Pose(initial_position, Eigen::Quaterniond())); 68 | SCurve::SCurveInitializationParameters input{{jerk, acceleration, velocity}, 69 | {0, trajectory_length, 0, 0}}; 70 | auto times = input.calc_times(); 71 | params = SCurve::generate_trajectory_parameters(times, input); 72 | double trajectory_duration = params.times.T(); 73 | return [=](double progress) { 74 | return SCurve::eval_s_curve(params, progress * trajectory_duration, SCurve::Derivative::Position) / 75 | trajectory_length; 76 | }; 77 | } 78 | -------------------------------------------------------------------------------- /src/StopCondition.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | #include 4 | using namespace orl; 5 | 6 | StopCondition StopConditions::Force(double max_force, double minimum_progress) { 7 | StopCondition stop_condition = [=](const PoseGeneratorInput &input) { 8 | Eigen::Vector3d force; 9 | force << input.state.O_F_ext_hat_K[0], input.state.O_F_ext_hat_K[1], input.state.O_F_ext_hat_K[2]; 10 | // std::cout << "force: " << force.norm() << std::endl; 11 | return force.norm() > max_force and input.progress > minimum_progress; 12 | }; 13 | return stop_condition; 14 | } 15 | -------------------------------------------------------------------------------- /src/geometry_math.cpp: -------------------------------------------------------------------------------- 1 | // Copyright (c) 2020 Marco Boneberger 2 | // Licensed under the EUPL-1.2-or-later 3 | /* 4 | * Implementation of Useful functions to calculate basic geometric values 5 | */ 6 | #include 7 | #include 8 | #include 9 | 10 | using boost::math::factorial; 11 | 12 | double angle_between(const Eigen::Vector3d &a, const Eigen::Vector3d &b, const Eigen::Vector3d &normal) { 13 | return std::atan2(b.cross(a).dot(normal.normalized()), a.dot(b)); 14 | } 15 | 16 | std::function 17 | generate_circle_movement(const Eigen::Vector3d ¢er, const Eigen::Vector3d &normal, const Eigen::Vector3d &start, 18 | double rotation_angle) { 19 | 20 | 21 | //project center: 22 | const double radius = std::abs((center - start).norm()); 23 | // std::cout << "normal: " << normal << std::endl; 24 | Eigen::Vector3d center_direction = center - start; 25 | // v1 = [0,1,0] x normal aka the new y direction 26 | auto v1 = normal.cross(center_direction); 27 | // v1 << normal[2], 0, -normal[0]; 28 | v1.normalize(); 29 | // std::cout << "v1: " << v1 << std::endl; 30 | // std::cout << "radius: " << radius << std::endl; 31 | Eigen::Vector3d v2 = normal.cross(v1); // the new x direction 32 | // std::cout << "v2: " << v2 << std::endl; 33 | auto tmp_start = radius * v1; 34 | double start_angle = angle_between(start - center, tmp_start, normal); 35 | // std::cout << "start angle: " << start_angle << std::endl; 36 | std::function circle_generator = [=](double progress) -> Eigen::Vector3d { 37 | double desired_angle = start_angle + progress * rotation_angle; 38 | Eigen::Vector3d point_on_circle = 39 | center + radius * (v1 * std::cos(desired_angle) + v2 * std::sin(desired_angle)); 40 | return point_on_circle; 41 | }; 42 | return circle_generator; 43 | } 44 | 45 | std::function 46 | generate_circle_movement_xy_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle) { 47 | Eigen::Vector3d normal; 48 | normal << 0, 0, 1; 49 | Eigen::Vector3d projected_center = center; 50 | projected_center[2] = start[2]; 51 | 52 | return generate_circle_movement(projected_center, normal, start, rotation_angle); 53 | } 54 | 55 | std::function 56 | generate_circle_movement_yz_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle) { 57 | Eigen::Vector3d normal; 58 | normal << 1, 0, 0; 59 | Eigen::Vector3d projected_center = center; 60 | projected_center[0] = start[0]; 61 | return generate_circle_movement(projected_center, normal, start, rotation_angle); 62 | } 63 | 64 | std::function 65 | generate_circle_movement_zx_plane(const Eigen::Vector3d ¢er, const Eigen::Vector3d &start, double rotation_angle) { 66 | Eigen::Vector3d normal; 67 | normal << 0, 1, 0; 68 | Eigen::Vector3d projected_center = center; 69 | projected_center[1] = start[1]; 70 | return generate_circle_movement(projected_center, normal, start, rotation_angle); 71 | } 72 | 73 | 74 | std::function generate_bezier_curve(const std::vector &points) { 75 | auto C = [](int n, int i) -> double { 76 | return factorial(n) / (factorial(i) * factorial(n - i)); 77 | }; 78 | auto J = [C](int n, int i, double t) -> double { 79 | return C(n, i) * std::pow(t, i) * std::pow(1 - t, n - i); 80 | }; 81 | int n = points.size() - 1; 82 | std::function bezier_generator = [=](double progress) -> Eigen::Vector3d { 83 | // Eigen::Vector3d sum(points.at(0)); 84 | Eigen::Vector3d sum(0, 0, 0); 85 | for (int i = 0; i <= n; ++i) { 86 | sum += points.at(i) * J(n, i, progress); 87 | } 88 | return sum; 89 | }; 90 | return bezier_generator; 91 | 92 | } 93 | 94 | Eigen::Vector3d 95 | b_spline_de_boor(int k, int p, double x, const std::vector &t, const std::vector &c) { 96 | std::vector d; 97 | for (int j = 0; j < p + 1; ++j) { 98 | d.push_back(c[j + k - p]); 99 | } 100 | for (int r = 1; r < p + 1; ++r) { 101 | for (int j = p; j > r - 1; --j) { 102 | double alpha = (x - t[j + k - p]) / (t[j + 1 + k - r] - t[j + k - p]); 103 | d[j] = (1.0 - alpha) * d[j - 1] + alpha * d[j]; 104 | } 105 | } 106 | return d[p]; 107 | } 108 | 109 | 110 | std::vector create_default_knot_vector(int num_control_points, int degree) { 111 | int knot_vector_size = num_control_points + degree + 1; 112 | std::vector knot_vector(knot_vector_size, 0); 113 | 114 | // special case to create [0,1,2,..] vector for degree 1 115 | if (degree == 1) { 116 | for (int i = 0; i < knot_vector_size; ++i) { 117 | knot_vector[i] = i; 118 | } 119 | return knot_vector; 120 | } 121 | int current_knot_element = 0; 122 | for (int i = degree; i < knot_vector_size - degree - 1; ++i) { 123 | knot_vector[i] = current_knot_element++; 124 | } 125 | for (int i = knot_vector_size - degree - 1; i < knot_vector_size; ++i) { 126 | knot_vector[i] = current_knot_element; 127 | } 128 | return knot_vector; 129 | } 130 | 131 | std::function 132 | generate_b_spline(const std::vector &points, int degree) { 133 | return generate_b_spline(points, degree, create_default_knot_vector(points.size(), degree)); 134 | } 135 | 136 | 137 | std::function 138 | generate_b_spline(const std::vector &points, int degree, const std::vector &knot_vector) { 139 | if (knot_vector.size() != points.size() + degree + 1) { 140 | std::stringstream ss; 141 | ss << "The knot vector has the wrong length. It should have #points + degree of polynomial + 1 = " 142 | << points.size() + degree + 1 << " components." << std::endl << "But it has " << knot_vector.size() 143 | << " components"; 144 | throw std::invalid_argument(ss.str()); 145 | } 146 | auto b_spline_generator = [=](double progress) { 147 | auto p = degree; 148 | int k = 0; 149 | progress = progress * (knot_vector[knot_vector.size() - p - 1] - knot_vector[p]) + knot_vector[p]; 150 | for (int j = p; j < (int) knot_vector.size() - 1 - p; ++j) { 151 | if (knot_vector[j] <= progress and progress <= knot_vector[j + 1]) { 152 | k = j; 153 | break; 154 | } 155 | } 156 | return b_spline_de_boor(k, p, progress, knot_vector, points); 157 | }; 158 | return b_spline_generator; 159 | } -------------------------------------------------------------------------------- /test/CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include_directories(../include) 2 | find_package(GTest REQUIRED) 3 | add_executable(liborl_test test.cpp) 4 | target_link_libraries(liborl_test ${GTEST_LIBRARIES} ${GTEST_MAIN_LIBRARIES} orl pthread) 5 | add_test(NAME ORLTests COMMAND liborl_test) --------------------------------------------------------------------------------