├── .gitignore ├── CMakeLists.txt ├── LICENSE ├── README.md ├── gravity-comp1.cc └── gravity-comp2.cc /.gitignore: -------------------------------------------------------------------------------- 1 | build/ 2 | *~ 3 | -------------------------------------------------------------------------------- /CMakeLists.txt: -------------------------------------------------------------------------------- 1 | include(FetchContent) 2 | FetchContent_Declare( 3 | moteus 4 | GIT_REPOSITORY https://github.com/mjbots/moteus.git 5 | GIT_TAG ee4f6ac9c6391523876836fcf6f276411b7f3670 6 | ) 7 | 8 | FetchContent_MakeAvailable(moteus) 9 | 10 | set(CMAKE_PREFIX_PATH "${CMAKE_PREFIX_PATH};/opt/openrobots") 11 | 12 | find_package(pinocchio) 13 | 14 | add_executable(gravity-comp1 gravity-comp1.cc) 15 | target_link_libraries(gravity-comp1 moteus::cpp) 16 | 17 | add_executable(gravity-comp2 gravity-comp2.cc) 18 | target_link_libraries(gravity-comp2 moteus::cpp) 19 | target_link_libraries(gravity-comp2 pinocchio::pinocchio) 20 | -------------------------------------------------------------------------------- /LICENSE: -------------------------------------------------------------------------------- 1 | Apache License 2 | Version 2.0, January 2004 3 | http://www.apache.org/licenses/ 4 | 5 | TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION 6 | 7 | 1. Definitions. 8 | 9 | "License" shall mean the terms and conditions for use, reproduction, 10 | and distribution as defined by Sections 1 through 9 of this document. 11 | 12 | "Licensor" shall mean the copyright owner or entity authorized by 13 | the copyright owner that is granting the License. 14 | 15 | "Legal Entity" shall mean the union of the acting entity and all 16 | other entities that control, are controlled by, or are under common 17 | control with that entity. For the purposes of this definition, 18 | "control" means (i) the power, direct or indirect, to cause the 19 | direction or management of such entity, whether by contract or 20 | otherwise, or (ii) ownership of fifty percent (50%) or more of the 21 | outstanding shares, or (iii) beneficial ownership of such entity. 22 | 23 | "You" (or "Your") shall mean an individual or Legal Entity 24 | exercising permissions granted by this License. 25 | 26 | "Source" form shall mean the preferred form for making modifications, 27 | including but not limited to software source code, documentation 28 | source, and configuration files. 29 | 30 | "Object" form shall mean any form resulting from mechanical 31 | transformation or translation of a Source form, including but 32 | not limited to compiled object code, generated documentation, 33 | and conversions to other media types. 34 | 35 | "Work" shall mean the work of authorship, whether in Source or 36 | Object form, made available under the License, as indicated by a 37 | copyright notice that is included in or attached to the work 38 | (an example is provided in the Appendix below). 39 | 40 | "Derivative Works" shall mean any work, whether in Source or Object 41 | form, that is based on (or derived from) the Work and for which the 42 | editorial revisions, annotations, elaborations, or other modifications 43 | represent, as a whole, an original work of authorship. For the purposes 44 | of this License, Derivative Works shall not include works that remain 45 | separable from, or merely link (or bind by name) to the interfaces of, 46 | the Work and Derivative Works thereof. 47 | 48 | "Contribution" shall mean any work of authorship, including 49 | the original version of the Work and any modifications or additions 50 | to that Work or Derivative Works thereof, that is intentionally 51 | submitted to Licensor for inclusion in the Work by the copyright owner 52 | or by an individual or Legal Entity authorized to submit on behalf of 53 | the copyright owner. For the purposes of this definition, "submitted" 54 | means any form of electronic, verbal, or written communication sent 55 | to the Licensor or its representatives, including but not limited to 56 | communication on electronic mailing lists, source code control systems, 57 | and issue tracking systems that are managed by, or on behalf of, the 58 | Licensor for the purpose of discussing and improving the Work, but 59 | excluding communication that is conspicuously marked or otherwise 60 | designated in writing by the copyright owner as "Not a Contribution." 61 | 62 | "Contributor" shall mean Licensor and any individual or Legal Entity 63 | on behalf of whom a Contribution has been received by Licensor and 64 | subsequently incorporated within the Work. 65 | 66 | 2. Grant of Copyright License. Subject to the terms and conditions of 67 | this License, each Contributor hereby grants to You a perpetual, 68 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 69 | copyright license to reproduce, prepare Derivative Works of, 70 | publicly display, publicly perform, sublicense, and distribute the 71 | Work and such Derivative Works in Source or Object form. 72 | 73 | 3. Grant of Patent License. Subject to the terms and conditions of 74 | this License, each Contributor hereby grants to You a perpetual, 75 | worldwide, non-exclusive, no-charge, royalty-free, irrevocable 76 | (except as stated in this section) patent license to make, have made, 77 | use, offer to sell, sell, import, and otherwise transfer the Work, 78 | where such license applies only to those patent claims licensable 79 | by such Contributor that are necessarily infringed by their 80 | Contribution(s) alone or by combination of their Contribution(s) 81 | with the Work to which such Contribution(s) was submitted. If You 82 | institute patent litigation against any entity (including a 83 | cross-claim or counterclaim in a lawsuit) alleging that the Work 84 | or a Contribution incorporated within the Work constitutes direct 85 | or contributory patent infringement, then any patent licenses 86 | granted to You under this License for that Work shall terminate 87 | as of the date such litigation is filed. 88 | 89 | 4. Redistribution. You may reproduce and distribute copies of the 90 | Work or Derivative Works thereof in any medium, with or without 91 | modifications, and in Source or Object form, provided that You 92 | meet the following conditions: 93 | 94 | (a) You must give any other recipients of the Work or 95 | Derivative Works a copy of this License; and 96 | 97 | (b) You must cause any modified files to carry prominent notices 98 | stating that You changed the files; and 99 | 100 | (c) You must retain, in the Source form of any Derivative Works 101 | that You distribute, all copyright, patent, trademark, and 102 | attribution notices from the Source form of the Work, 103 | excluding those notices that do not pertain to any part of 104 | the Derivative Works; and 105 | 106 | (d) If the Work includes a "NOTICE" text file as part of its 107 | distribution, then any Derivative Works that You distribute must 108 | include a readable copy of the attribution notices contained 109 | within such NOTICE file, excluding those notices that do not 110 | pertain to any part of the Derivative Works, in at least one 111 | of the following places: within a NOTICE text file distributed 112 | as part of the Derivative Works; within the Source form or 113 | documentation, if provided along with the Derivative Works; or, 114 | within a display generated by the Derivative Works, if and 115 | wherever such third-party notices normally appear. The contents 116 | of the NOTICE file are for informational purposes only and 117 | do not modify the License. You may add Your own attribution 118 | notices within Derivative Works that You distribute, alongside 119 | or as an addendum to the NOTICE text from the Work, provided 120 | that such additional attribution notices cannot be construed 121 | as modifying the License. 122 | 123 | You may add Your own copyright statement to Your modifications and 124 | may provide additional or different license terms and conditions 125 | for use, reproduction, or distribution of Your modifications, or 126 | for any such Derivative Works as a whole, provided Your use, 127 | reproduction, and distribution of the Work otherwise complies with 128 | the conditions stated in this License. 129 | 130 | 5. Submission of Contributions. Unless You explicitly state otherwise, 131 | any Contribution intentionally submitted for inclusion in the Work 132 | by You to the Licensor shall be under the terms and conditions of 133 | this License, without any additional terms or conditions. 134 | Notwithstanding the above, nothing herein shall supersede or modify 135 | the terms of any separate license agreement you may have executed 136 | with Licensor regarding such Contributions. 137 | 138 | 6. Trademarks. This License does not grant permission to use the trade 139 | names, trademarks, service marks, or product names of the Licensor, 140 | except as required for reasonable and customary use in describing the 141 | origin of the Work and reproducing the content of the NOTICE file. 142 | 143 | 7. Disclaimer of Warranty. Unless required by applicable law or 144 | agreed to in writing, Licensor provides the Work (and each 145 | Contributor provides its Contributions) on an "AS IS" BASIS, 146 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or 147 | implied, including, without limitation, any warranties or conditions 148 | of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A 149 | PARTICULAR PURPOSE. You are solely responsible for determining the 150 | appropriateness of using or redistributing the Work and assume any 151 | risks associated with Your exercise of permissions under this License. 152 | 153 | 8. Limitation of Liability. In no event and under no legal theory, 154 | whether in tort (including negligence), contract, or otherwise, 155 | unless required by applicable law (such as deliberate and grossly 156 | negligent acts) or agreed to in writing, shall any Contributor be 157 | liable to You for damages, including any direct, indirect, special, 158 | incidental, or consequential damages of any character arising as a 159 | result of this License or out of the use or inability to use the 160 | Work (including but not limited to damages for loss of goodwill, 161 | work stoppage, computer failure or malfunction, or any and all 162 | other commercial damages or losses), even if such Contributor 163 | has been advised of the possibility of such damages. 164 | 165 | 9. Accepting Warranty or Additional Liability. While redistributing 166 | the Work or Derivative Works thereof, You may choose to offer, 167 | and charge a fee for, acceptance of support, warranty, indemnity, 168 | or other liability obligations and/or rights consistent with this 169 | License. However, in accepting such obligations, You may act only 170 | on Your own behalf and on Your sole responsibility, not on behalf 171 | of any other Contributor, and only if You agree to indemnify, 172 | defend, and hold each Contributor harmless for any liability 173 | incurred by, or claims asserted against, such Contributor by reason 174 | of your accepting any such warranty or additional liability. 175 | 176 | END OF TERMS AND CONDITIONS 177 | 178 | APPENDIX: How to apply the Apache License to your work. 179 | 180 | To apply the Apache License to your work, attach the following 181 | boilerplate notice, with the fields enclosed by brackets "[]" 182 | replaced with your own identifying information. (Don't include 183 | the brackets!) The text should be enclosed in the appropriate 184 | comment syntax for the file format. We also recommend that a 185 | file or class name and description of purpose be included on the 186 | same "printed page" as the copyright notice for easier 187 | identification within third-party archives. 188 | 189 | Copyright [yyyy] [name of copyright owner] 190 | 191 | Licensed under the Apache License, Version 2.0 (the "License"); 192 | you may not use this file except in compliance with the License. 193 | You may obtain a copy of the License at 194 | 195 | http://www.apache.org/licenses/LICENSE-2.0 196 | 197 | Unless required by applicable law or agreed to in writing, software 198 | distributed under the License is distributed on an "AS IS" BASIS, 199 | WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 200 | See the License for the specific language governing permissions and 201 | limitations under the License. 202 | -------------------------------------------------------------------------------- /README.md: -------------------------------------------------------------------------------- 1 | # moteus C++ gravity compensation examples # 2 | 3 | The source code in this repository corresponds to the following videos: 4 | 5 | * [Part 1 - 1 DoF](https://youtu.be/VTyuRj0TDcM) 6 | * [Part 2 - 2 DoF](https://youtu.be/jCM1n39dDWk) 7 | 8 | # License # 9 | 10 | All files contained in this repository, unless otherwise noted, are 11 | available under an Apache 2.0 License: 12 | https://www.apache.org/licenses/LICENSE-2.0 13 | -------------------------------------------------------------------------------- /gravity-comp1.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | 20 | #include "moteus.h" 21 | 22 | int main(int argc, char** argv) { 23 | using namespace mjbots; 24 | 25 | moteus::Controller::DefaultArgProcess(argc, argv); 26 | 27 | moteus::Controller::Options options; 28 | 29 | auto& pf = options.position_format; 30 | pf.position = moteus::kIgnore; 31 | pf.velocity = moteus::kIgnore; 32 | pf.feedforward_torque = moteus::kFloat; 33 | pf.kp_scale = moteus::kInt8; 34 | pf.kd_scale = moteus::kInt8; 35 | 36 | moteus::Controller c(options); 37 | 38 | c.SetStop(); 39 | 40 | moteus::PositionMode::Command cmd; 41 | cmd.kp_scale = 0.0; 42 | cmd.kd_scale = 0.0; 43 | cmd.feedforward_torque = 0.0; 44 | 45 | constexpr double kMassKg = 0.26; // (this is the actual value 0.3;) 46 | constexpr double kLinkLength = 0.15; 47 | 48 | int missed_replies = 0; 49 | int status_count = 0; 50 | constexpr int kStatusPeriod = 100; 51 | 52 | while (true) { 53 | ::usleep(10); 54 | 55 | const auto maybe_result = c.SetPosition(cmd); 56 | if (!maybe_result) { 57 | missed_replies++; 58 | if (missed_replies > 3) { 59 | printf("\n\nmotor timeout!\n"); 60 | break; 61 | } 62 | 63 | continue; 64 | } else { 65 | missed_replies = 0; 66 | } 67 | 68 | const auto& result = *maybe_result; 69 | const auto& v = result.values; 70 | 71 | const double command_torque = 72 | std::sin(v.position * 2 * M_PI) * kMassKg * kLinkLength * 9.8; 73 | cmd.feedforward_torque = command_torque; 74 | 75 | status_count++; 76 | if (status_count > kStatusPeriod) { 77 | status_count = 0; 78 | 79 | printf("Mode: %2d position: %6.3f cmd_torque: %6.3f temp: %4.1f \r", 80 | static_cast(v.mode), 81 | v.position, 82 | command_torque, 83 | v.temperature); 84 | fflush(stdout); 85 | } 86 | } 87 | 88 | printf("Entering fault mode!\n"); 89 | 90 | while (true) { 91 | ::usleep(50000); 92 | c.SetBrake(); 93 | } 94 | 95 | return 0; 96 | } 97 | -------------------------------------------------------------------------------- /gravity-comp2.cc: -------------------------------------------------------------------------------- 1 | // Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com 2 | // 3 | // Licensed under the Apache License, Version 2.0 (the "License"); 4 | // you may not use this file except in compliance with the License. 5 | // You may obtain a copy of the License at 6 | // 7 | // http://www.apache.org/licenses/LICENSE-2.0 8 | // 9 | // Unless required by applicable law or agreed to in writing, software 10 | // distributed under the License is distributed on an "AS IS" BASIS, 11 | // WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. 12 | // See the License for the specific language governing permissions and 13 | // limitations under the License. 14 | 15 | #include 16 | #include 17 | 18 | #include 19 | #include 20 | 21 | #include "moteus.h" 22 | 23 | #include "pinocchio/algorithm/joint-configuration.hpp" 24 | #include "pinocchio/algorithm/rnea.hpp" 25 | #include "pinocchio/parsers/sample-models.hpp" 26 | 27 | namespace { 28 | template class JointCollectionTpl> 30 | void BuildModel(pinocchio::ModelTpl* model) { 31 | using namespace pinocchio; 32 | 33 | using M = Model; 34 | using JC = JointCollectionTpl; 35 | using CV = typename JC::JointModelRX::ConfigVector_t; 36 | using TV = typename JC::JointModelRX::TangentVector_t; 37 | 38 | M::JointIndex idx = 0; 39 | 40 | constexpr double kFudge = 0.95; 41 | 42 | SE3 Tlink (SE3::Matrix3::Identity(), SE3::Vector3(0, 0, 0.15)); 43 | Inertia Ilink1(kFudge * 0.29, Tlink.translation(), 44 | Inertia::Matrix3::Identity() * 0.001); 45 | Inertia Ilink2(kFudge * 0.28, Tlink.translation(), 46 | Inertia::Matrix3::Identity() * 0.001); 47 | 48 | CV qmin = CV::Constant(-4); 49 | CV qmax = CV::Constant(4); 50 | TV vmax = CV::Constant(10); 51 | TV taumax = CV::Constant(10); 52 | 53 | idx = model->addJoint(idx, typename JC::JointModelRY(), Tlink, 54 | "link1_joint", taumax, vmax, qmin, qmax); 55 | model->appendBodyToJoint(idx, Ilink1); 56 | model->addJointFrame(idx); 57 | model->addBodyFrame("link1_body", idx); 58 | 59 | idx = model->addJoint(idx, typename JC::JointModelRY(), Tlink, 60 | "link2_joint", taumax, vmax, qmin, qmax); 61 | model->appendBodyToJoint(idx, Ilink2); 62 | model->addJointFrame(idx); 63 | model->addBodyFrame("link2_body", idx); 64 | } 65 | 66 | std::optional FindServo( 67 | const std::vector& frames, 68 | int id) { 69 | for (auto it = frames.rbegin(); it != frames.rend(); ++it) { 70 | if (it->source ==id) { 71 | return mjbots::moteus::Query::Parse(it->data, it->size); 72 | } 73 | } 74 | return {}; 75 | } 76 | 77 | double WrapAround0(double v) { 78 | const auto v1 = std::fmod(v, 1.0); 79 | const auto v2 = (v1 < 0.0) ? (v1 + 1.0) : v1; 80 | return v2 > 0.5 ? (v2 - 1.0) : v2; 81 | } 82 | } 83 | 84 | int main(int argc, char** argv) { 85 | using namespace mjbots; 86 | moteus::Controller::DefaultArgProcess(argc, argv); 87 | auto transport = moteus::Controller::MakeSingletonTransport({}); 88 | 89 | pinocchio::Model model; 90 | BuildModel(&model); 91 | pinocchio::Data data(model); 92 | 93 | Eigen::VectorXd q = Eigen::VectorXd::Zero(model.nv); 94 | Eigen::VectorXd v = Eigen::VectorXd::Zero(model.nv); 95 | Eigen::VectorXd a = Eigen::VectorXd::Zero(model.nv); 96 | 97 | moteus::Controller::Options options_common; 98 | 99 | auto& pf = options_common.position_format; 100 | pf.position = moteus::kIgnore; 101 | pf.velocity = moteus::kIgnore; 102 | pf.feedforward_torque = moteus::kFloat; 103 | pf.kp_scale = moteus::kInt8; 104 | pf.kd_scale = moteus::kInt8; 105 | 106 | std::vector> controllers = { 107 | std::make_shared([&]() { 108 | auto options = options_common; 109 | options.id = 1; 110 | return options; 111 | }()), 112 | std::make_shared([&]() { 113 | auto options = options_common; 114 | options.id = 2; 115 | return options; 116 | }()), 117 | }; 118 | 119 | for (auto& c : controllers) { c->SetStop(); } 120 | 121 | moteus::PositionMode::Command cmd; 122 | cmd.kp_scale = 0.0; 123 | cmd.kd_scale = 0.0; 124 | cmd.feedforward_torque = 0.0; 125 | 126 | 127 | double torque_command[2] = {}; 128 | std::vector send_frames; 129 | std::vector receive_frames; 130 | 131 | int missed_replies = 0; 132 | int status_count = 0; 133 | constexpr int kStatusPeriod = 100; 134 | 135 | while (true) { 136 | ::usleep(10); 137 | 138 | send_frames.clear(); 139 | receive_frames.clear(); 140 | 141 | for (size_t i = 0; i < controllers.size(); i++) { 142 | cmd.feedforward_torque = torque_command[i]; 143 | send_frames.push_back(controllers[i]->MakePosition(cmd)); 144 | } 145 | 146 | transport->BlockingCycle( 147 | &send_frames[0], send_frames.size(), 148 | &receive_frames); 149 | 150 | auto maybe_servo1 = FindServo(receive_frames, 1); 151 | auto maybe_servo2 = FindServo(receive_frames, 2); 152 | 153 | if (!maybe_servo1 || !maybe_servo2) { 154 | missed_replies++; 155 | if (missed_replies > 3) { 156 | printf("\n\nServo not responding 1=%d 2=%d\n", 157 | maybe_servo1 ? 1 : 0, 158 | maybe_servo2 ? 1 : 0); 159 | break; 160 | } 161 | continue; 162 | } else { 163 | missed_replies = 0; 164 | } 165 | 166 | const auto& v1 = *maybe_servo1; 167 | const auto& v2 = *maybe_servo2; 168 | 169 | q(0) = WrapAround0(v1.position + 0.5) * 2 * M_PI; 170 | q(1) = WrapAround0(v2.position) * 2 * M_PI; 171 | 172 | const Eigen::VectorXd& tau = pinocchio::rnea(model, data, q, v, a); 173 | 174 | torque_command[0] = tau(0); 175 | torque_command[1] = tau(1); 176 | 177 | status_count++; 178 | if (status_count > kStatusPeriod) { 179 | 180 | printf("Mode: %2d/%2d position: %6.3f/%6.3f torque: %6.3f/%6.3f temp: %4.1f/%4.1f \r", 181 | static_cast(v1.mode), static_cast(v2.mode), 182 | v1.position, v2.position, 183 | torque_command[0], torque_command[1], 184 | v1.temperature, v2.temperature); 185 | fflush(stdout); 186 | 187 | status_count = 0; 188 | } 189 | 190 | } 191 | 192 | printf("Entering fault mode!\n"); 193 | 194 | while (true) { 195 | ::usleep(50000); 196 | 197 | for (auto& c : controllers) { c->SetBrake(); } 198 | } 199 | 200 | return 0; 201 | } 202 | --------------------------------------------------------------------------------