From 80647d6debc61cec27f37acc9bc2ac8433a07b6c Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 5 Jul 2024 14:36:34 +0200 Subject: [PATCH 001/137] Created pi3hat package and copied moteus and pi3hat srource code --- .../pi3hat_hardware_interface/CMakeLists.txt | 26 + .../pi3hat_hardware_interface/LICENSE | 202 ++ .../include/moteus/BUILD | 51 + .../include/moteus/moteus.h | 1034 ++++++++++ .../include/moteus/moteus_multiplex.h | 674 +++++++ .../include/moteus/moteus_optional.h | 53 + .../include/moteus/moteus_protocol.h | 1289 +++++++++++++ .../include/moteus/moteus_tokenizer.h | 62 + .../include/moteus/moteus_transport.h | 1128 +++++++++++ .../include/pi3hat/BUILD | 75 + .../include/pi3hat/pi3hat.cc | 1703 +++++++++++++++++ .../include/pi3hat/pi3hat.h | 311 +++ .../include/pi3hat/pi3hat_moteus_transport.h | 397 ++++ .../include/pi3hat/pi3hat_tool.cc | 785 ++++++++ .../include/pi3hat/realtime.h | 46 + .../pi3hat_hardware_interface/package.xml | 18 + 16 files changed, 7854 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/LICENSE create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/package.xml diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt new file mode 100644 index 00000000..83f20c1c --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -0,0 +1,26 @@ +cmake_minimum_required(VERSION 3.8) +project(pi3hat_hardware_interface) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +ament_package() diff --git a/src/meldog_hardware/pi3hat_hardware_interface/LICENSE b/src/meldog_hardware/pi3hat_hardware_interface/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD new file mode 100644 index 00000000..2685f2db --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD @@ -0,0 +1,51 @@ +# -*- python -*- + +# Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "src", + srcs = [ + "moteus_multiplex.h", + "moteus_optional.h", + "moteus_protocol.h", + "moteus_tokenizer.h", + "moteus_transport.h", + "moteus.h", + ], +) + +cc_library( + name = "moteus", + hdrs = [":src"], +) + +cc_test( + name = "test", + srcs = [ + "test/moteus_test.cc", + "test/moteus_protocol_test.cc", + "test/test_main.cc", + ], + copts = [ + "-Ilib/cpp", + ], + deps = [ + ":moteus", + "@boost//:test", + ], + size = "small", +) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h new file mode 100644 index 00000000..058a9274 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h @@ -0,0 +1,1034 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "moteus_optional.h" +#include "moteus_protocol.h" +#include "moteus_tokenizer.h" +#include "moteus_transport.h" + +namespace mjbots { +namespace moteus { + + +/// This is the primary interface to a moteus controller. One +/// instance of this class should be created per controller that is +/// commanded or monitored. +/// +/// The primary control functions each have 3 possible forms: +/// +/// 1. A "Make" variant which constructs a CanFdFrame to be used in a +/// later call to Transport::Cycle. +/// +/// 2. A "Set" variant which sends a command to the controller and +/// waits for a response in a blocking manner. +/// +/// 3. An "Async" variant which starts the process of sending a +/// command and potentially waiting for a response. When this +/// operation is finished, a user-provided callback is invoked. +/// This callback may be called either: +/// a) from an arbitrary thread +/// b) recursively from the calling thread before returning +/// +/// While any async operation is outstanding, it is undefined behavior +/// to start another async operation or execute a blocking operation. +class Controller { + public: + struct Options { + // The ID of the servo to communicate with. + int id = 1; + + // The source ID to use for the commanding node (i.e. the host or + // master). + int source = 0; + + // Which CAN bus to send commands on and look for responses on. + // This may not be used on all transports. + int bus = 0; + + // For each possible primary command, the resolution for all + // command fields has a default set at construction time. + Query::Format query_format; + PositionMode::Format position_format; + VFOCMode::Format vfoc_format; + CurrentMode::Format current_format; + StayWithinMode::Format stay_within_format; + + // Use the given prefix for all CAN IDs. + uint32_t can_prefix = 0x0000; + + // Request the configured set of registers as a query with every + // command. + bool default_query = true; + + int64_t diagnostic_retry_sleep_ns = 200000; + + // Specify a transport to be used. If left unset, a global common + // transport will be constructed to be shared with all Controller + // instances in this process. That will attempt to auto-detect a + // reasonable transport on the system. + std::shared_ptr transport; + + Options() {} + }; + + Controller(const Options& options = {}) : options_(options) { + transport_ = options.transport; + + WriteCanData query_write(&query_frame_); + query_reply_size_ = Query::Make(&query_write, options_.query_format); + } + + const Options& options() const { return options_; } + + Transport* transport() { + if (!transport_) { + transport_ = MakeSingletonTransport({}); + } + + return transport_.get(); + } + + /// Make a transport given the cmdline arguments. + static std::shared_ptr MakeSingletonTransport( + const std::vector& args) { + auto result = ArgumentProcessHelper(args); + return result.first; + } + + /// Require that a default global transport have already been + /// created and return it. + static std::shared_ptr RequireSingletonTransport() { + auto& g_transport = *GlobalTransport(); + if (!g_transport) { + throw std::logic_error("Unexpectedly cannot find global transport"); + } + return g_transport; + } + + struct Result { + CanFdFrame frame; + Query::Result values; + }; + + + ///////////////////////////////////////// + // Query + + CanFdFrame MakeQuery(const Query::Format* format_override = nullptr) { + // We force there to always be an override for the query format, + // because if we're directly asking for a query, we should get it + // no matter the Options::default_query state. + return MakeFrame(EmptyMode(), {}, {}, + format_override == nullptr ? + &options_.query_format : format_override); + } + + // This is prefixed "Set" despite the fact that it sets nothing + // because (a) it is consistent with the other methods with + // "Make/Set/Async" prefixes, and (b) it would otherwise shadow the + // mjbots::moteus::Query structure. + Optional SetQuery(const Query::Format* format_override = nullptr) { + return ExecuteSingleCommand(MakeQuery(format_override)); + } + + void AsyncQuery(Result* result, CompletionCallback callback, + const Query::Format* format_override = nullptr) { + AsyncStartSingleCommand(MakeQuery(format_override), result, callback); + } + + + ///////////////////////////////////////// + // StopMode + + CanFdFrame MakeStop(const Query::Format* query_override = nullptr) { + return MakeFrame(StopMode(), {}, {}, query_override); + } + + Optional SetStop(const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeStop(query_override)); + } + + void AsyncStop(Result* result, CompletionCallback callback, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeStop(query_override), result, callback); + } + + + ///////////////////////////////////////// + // BrakeMode + + CanFdFrame MakeBrake(const Query::Format* query_override = nullptr) { + return MakeFrame(BrakeMode(), {}, {}, query_override); + } + + Optional SetBrake(const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeBrake(query_override)); + } + + void AsyncBrake(Result* result, CompletionCallback callback, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeBrake(query_override), result, callback); + } + + + ///////////////////////////////////////// + // PositionMode + + CanFdFrame MakePosition(const PositionMode::Command& cmd, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame( + PositionMode(), cmd, + (command_override == nullptr ? + options_.position_format : *command_override), + query_override); + } + + Optional SetPosition( + const PositionMode::Command& cmd, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakePosition(cmd, command_override, query_override)); + } + + void AsyncPosition(const PositionMode::Command& cmd, + Result* result, CompletionCallback callback, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakePosition(cmd, command_override, query_override), + result, callback); + } + + /// Repeatedly send a position command until the reported + /// trajectory_complete flag is true. This will always enable a + /// query and will return the result of the final such response. + Optional SetPositionWaitComplete( + const PositionMode::Command& cmd, + double period_s, + const PositionMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + Query::Format query_format = + query_override == nullptr ? options_.query_format : *query_override; + query_format.trajectory_complete = kInt8; + + int count = 2; + while (true) { + auto maybe_result = SetPosition(cmd, command_override, &query_format); + if (!!maybe_result) { count = std::max(count - 1, 0); } + + if (count == 0 && + !!maybe_result && + maybe_result->values.trajectory_complete) { + return *maybe_result; + } + + ::usleep(static_cast(period_s * 1e6)); + } + + return {}; + } + + + ///////////////////////////////////////// + // VFOCMode + + CanFdFrame MakeVFOC(const VFOCMode::Command& cmd, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame( + VFOCMode(), cmd, + command_override == nullptr ? options_.vfoc_format : *command_override, + query_override); + } + + Optional SetVFOC(const VFOCMode::Command& cmd, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand(MakeVFOC(cmd, command_override, query_override)); + } + + void AsyncVFOC(const VFOCMode::Command& cmd, + Result* result, CompletionCallback callback, + const VFOCMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeVFOC(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // CurrentMode + + CanFdFrame MakeCurrent(const CurrentMode::Command& cmd, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(CurrentMode(), cmd, + (command_override == nullptr ? + options_.current_format : *command_override), + query_override); + } + + Optional SetCurrent( + const CurrentMode::Command& cmd, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeCurrent(cmd, command_override, query_override)); + } + + void AsyncCurrent(const CurrentMode::Command& cmd, + Result* result, CompletionCallback callback, + const CurrentMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand(MakeCurrent(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // StayWithinMode + + CanFdFrame MakeStayWithin( + const StayWithinMode::Command& cmd, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(StayWithinMode(), cmd, + (command_override == nullptr ? + options_.stay_within_format : *command_override), + query_override); + } + + Optional SetStayWithin( + const StayWithinMode::Command& cmd, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeStayWithin(cmd, command_override, query_override)); + } + + void AsyncStayWithin(const StayWithinMode::Command& cmd, + Result* result, CompletionCallback callback, + const StayWithinMode::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeStayWithin(cmd, command_override, query_override), result, callback); + } + + + ///////////////////////////////////////// + // OutputNearest + + CanFdFrame MakeOutputNearest(const OutputNearest::Command& cmd, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(OutputNearest(), cmd, + (command_override == nullptr ? + OutputNearest::Format() : *command_override), + query_override); + } + + Optional SetOutputNearest(const OutputNearest::Command& cmd, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeOutputNearest(cmd, command_override, query_override)); + } + + void AsyncOutputNearest(const OutputNearest::Command& cmd, + Result* result, CompletionCallback callback, + const OutputNearest::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeOutputNearest(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // OutputExact + + CanFdFrame MakeOutputExact(const OutputExact::Command& cmd, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(OutputExact(), cmd, + (command_override == nullptr ? + OutputExact::Format() : *command_override), + query_override); + } + + Optional SetOutputExact(const OutputExact::Command& cmd, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeOutputExact(cmd, command_override, query_override)); + } + + void AsyncOutputExact(const OutputExact::Command& cmd, + Result* result, CompletionCallback callback, + const OutputExact::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeOutputExact(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // RequireReindex + + CanFdFrame MakeRequireReindex(const RequireReindex::Command& cmd = {}, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(RequireReindex(), cmd, + (command_override == nullptr ? + RequireReindex::Format() : *command_override), + query_override); + } + + Optional SetRequireReindex(const RequireReindex::Command& cmd = {}, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeRequireReindex(cmd, command_override, query_override)); + } + + void AsyncRequireReindex(const RequireReindex::Command& cmd, + Result* result, CompletionCallback callback, + const RequireReindex::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeRequireReindex(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // RecapturePositionVelocity + + CanFdFrame MakeRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd = {}, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(RecapturePositionVelocity(), cmd, + (command_override == nullptr ? + RecapturePositionVelocity::Format() : *command_override), + query_override); + } + + Optional SetRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd = {}, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override)); + } + + void AsyncRecapturePositionVelocity( + const RecapturePositionVelocity::Command& cmd, + Result* result, CompletionCallback callback, + const RecapturePositionVelocity::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeRecapturePositionVelocity(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // ClockTrim + + CanFdFrame MakeClockTrim(const ClockTrim::Command& cmd, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return MakeFrame(ClockTrim(), cmd, + (command_override == nullptr ? + ClockTrim::Format() : *command_override), + query_override); + } + + Optional SetClockTrim(const ClockTrim::Command& cmd, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + return ExecuteSingleCommand( + MakeClockTrim(cmd, command_override, query_override)); + } + + void AsyncClockTrim(const ClockTrim::Command& cmd, + Result* result, CompletionCallback callback, + const ClockTrim::Format* command_override = nullptr, + const Query::Format* query_override = nullptr) { + AsyncStartSingleCommand( + MakeClockTrim(cmd, command_override, query_override), + result, callback); + } + + + ///////////////////////////////////////// + // Diagnostic channel operations + + enum DiagnosticReplyMode { + kExpectOK, + kExpectSingleLine, + }; + + std::string DiagnosticCommand(const std::string& message, + DiagnosticReplyMode reply_mode = kExpectOK) { + BlockingCallback cbk; + std::string response; + AsyncDiagnosticCommand(message, &response, cbk.callback(), reply_mode); + cbk.Wait(); + return response; + } + + void AsyncDiagnosticCommand(const std::string& message, + std::string* result, + CompletionCallback callback, + DiagnosticReplyMode reply_mode = kExpectOK) { + auto context = std::make_shared(); + context->result = result; + context->remaining_command = message + "\n"; + context->controller = this; + context->transport = transport(); + context->callback = callback; + context->reply_mode = reply_mode; + + context->Start(); + } + + void DiagnosticWrite(const std::string& message, int channel = 1) { + BlockingCallback cbk; + AsyncDiagnosticWrite(message, channel, cbk.callback()); + cbk.Wait(); + } + + void AsyncDiagnosticWrite(const std::string& message, + int channel, + CompletionCallback callback) { + auto context = std::make_shared(); + context->message = message; + context->channel = channel; + context->controller = this; + context->transport = transport(); + context->callback = callback; + + context->Start(); + } + + std::string DiagnosticRead(int channel) { + std::string response; + BlockingCallback cbk; + AsyncDiagnosticRead(&response, channel, cbk.callback()); + cbk.Wait(); + return response; + } + + void AsyncDiagnosticRead(std::string* response, + int channel, + CompletionCallback callback) { + DiagnosticRead::Command read; + read.channel = channel; + read.max_length = 48; + + output_frame_ = DefaultFrame(kReplyRequired); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticRead::Make(&write_frame, read, {}); + + struct Context { + std::string* response = nullptr; + std::vector replies; + CompletionCallback callback; + Transport* transport = nullptr; + }; + auto context = std::make_shared(); + context->response = response; + context->callback = callback; + context->transport = transport(); + + context->transport->Cycle( + &output_frame_, 1, &context->replies, + [context, this](int) { + std::string response; + bool any_response = false; + + for (const auto& frame : context->replies) { + if (frame.destination != options_.source || + frame.source != options_.id || + frame.can_prefix != options_.can_prefix) { + continue; + } + auto maybe_data = DiagnosticResponse::Parse(frame.data, frame.size); + response += std::string( + reinterpret_cast(maybe_data.data), maybe_data.size); + any_response = true; + } + + if (any_response) { + *context->response = response; + context->transport->Post(std::bind(context->callback, 0)); + return; + } + + context->transport->Post(std::bind(context->callback, ETIMEDOUT)); + return; + }); + } + + void DiagnosticFlush(int channel = 1, double timeout_s = 0.2) { + // Read until nothing is left or the timeout hits. + const auto timeout = timeout_s * 1000000000ll; + const auto start = Fdcanusb::GetNow(); + auto end_time = start + timeout; + + while (true) { + const auto response = DiagnosticRead(channel); + const auto now = Fdcanusb::GetNow(); + if (!response.empty()) { + // Every time we get something, bump out timeout further into + // the future. + end_time = now + timeout; + continue; + } + if (now > end_time) { + break; + } + ::usleep(options_.diagnostic_retry_sleep_ns / 1000); + } + } + + + ///////////////////////////////////////// + // Schema version checking + + CanFdFrame MakeSchemaVersionQuery() { + GenericQuery::Format query; + query.values[0].register_number = Register::kRegisterMapVersion; + query.values[0].resolution = kInt32; + + return MakeFrame(GenericQuery(), {}, query); + } + + void VerifySchemaVersion() { + const auto result = ExecuteSingleCommand(MakeSchemaVersionQuery()); + if (!result) { + throw std::runtime_error("No response to schema version query"); + } + CheckRegisterMapVersion(*result); + } + + void AsyncVerifySchemaVersion(CompletionCallback callback) { + auto result = std::make_shared(); + auto t = transport(); + + AsyncStartSingleCommand( + MakeSchemaVersionQuery(), + result.get(), + [result, callback, t](int value) { + CheckRegisterMapVersion(*result); + t->Post(std::bind(callback, value)); + }); + } + + ////////////////////////////////////////////////// + + Optional ExecuteSingleCommand(const CanFdFrame& cmd) { + single_command_replies_.resize(0); + + transport()->BlockingCycle(&cmd, 1, &single_command_replies_); + + return FindResult(single_command_replies_); + } + + void AsyncStartSingleCommand(const CanFdFrame& cmd, + Result* result, + CompletionCallback callback) { + auto t = transport(); + output_frame_ = cmd; + t->Cycle( + &output_frame_, + 1, + &single_command_replies_, + [callback, result, this, t](int) { + auto maybe_result = this->FindResult(single_command_replies_); + if (maybe_result) { *result = *maybe_result; } + + t->Post( + std::bind( + callback, !options_.default_query ? 0 : + !!maybe_result ? 0 : ETIMEDOUT)); + }); + } + + static std::string FinalName(const std::string& name) { + const size_t pos = name.find_last_of("/"); + if (pos != std::string::npos) { return name.substr(pos + 1); } + return name; + } + + /// This may be called to allow users to configure a default + /// transport. It handles "-h" and "--help" by printing and + /// exiting, so is not suitable for cases where any other command + /// line arguments need to be handled. + static void DefaultArgProcess(int argc, char** argv) { + std::vector args; + for (int i = 0; i < argc; i++) { args.push_back(argv[i]); } + + DefaultArgProcess(args); + } + + /// The same as the above function, but accepts its arguments in a + /// std::vector form. + static void DefaultArgProcess(const std::vector& args) { + if (std::find(args.begin() + 1, args.end(), "--help") != args.end() || + std::find(args.begin() + 1, args.end(), "-h") != args.end()) { + std::cout << "Usage: " << FinalName(args[0]) << "\n"; + auto help_strs = + moteus::TransportRegistry::singleton().cmdline_arguments(); + help_strs.insert(help_strs.begin(), + TransportFactory::Argument( + "--help", 0, "Display this usage message")); + + int max_item = 0; + for (const auto& item : help_strs) { + max_item = std::max(max_item, item.name.size()); + } + max_item = std::min(30, max_item); + + for (const auto& help_item : help_strs) { + std::cout + << " " << help_item.name << " " + << std::string(std::max(0, max_item - help_item.name.size()), ' ') + << help_item.help << "\n"; + } + + std::exit(0); + } + + ArgumentProcessHelper(args); + } + + /// Configure the default transport according to the given command + /// line arguments. Return the command line arguments with all + /// processed commands removed. + /// + /// This is an optional call, and is only required if you want to + /// give a user the ability to configure the default transport from + /// the command line. + /// + /// "-h" and "--help" are not handled here in any way. Thus this + /// method can be used in applications that want to perform + /// additional processing on the command line arguments after. + static std::vector + ProcessTransportArgs(const std::vector& args) { + return ArgumentProcessHelper(args).second; + } + + /// If your application wants to support configuring the default + /// transport from the cmdline and also have application level + /// options, this list can be used to populate --help content. + static std::vector cmdline_arguments() { + return TransportRegistry::singleton().cmdline_arguments(); + } + + private: + static TransportFactory::TransportArgPair + ArgumentProcessHelper(const std::vector& args) { + auto& g_transport = *GlobalTransport(); + if (g_transport) { return std::make_pair(g_transport, args); } + + auto result = TransportRegistry::singleton().make(args); + g_transport = result.first; + return result; + } + + static std::shared_ptr* GlobalTransport() { + static std::shared_ptr g_transport; + return &g_transport; + }; + + // A helper context to maintain asynchronous state while performing + // diagnostic channel commands. + struct AsyncDiagnosticCommandContext + : public std::enable_shared_from_this { + std::string* result = nullptr; + CompletionCallback callback; + DiagnosticReplyMode reply_mode = {}; + + Controller* controller = nullptr; + Transport* transport = nullptr; + + CanFdFrame output_frame_; + std::vector replies; + + int empty_replies = 0; + std::string remaining_command; + std::string current_line; + std::ostringstream output; + + void Start() { + DoWrite(); + } + + void Callback(int error) { + if (error != 0) { + transport->Post(std::bind(callback, error)); + return; + } + + if (ProcessReplies()) { + transport->Post(std::bind(callback, 0)); + return; + } + + if (remaining_command.size()) { + DoWrite(); + } else { + DoRead(); + } + } + + void DoWrite() { + DiagnosticWrite::Command write; + write.data = remaining_command.data(); + const auto to_write = std::min(48, remaining_command.size()); + write.size = to_write; + + output_frame_ = controller->DefaultFrame(kNoReply); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticWrite::Make(&write_frame, write, {}); + + auto s = shared_from_this(); + controller->transport()->Cycle( + &output_frame_, 1, nullptr, + [s, to_write](int v) { + s->remaining_command = s->remaining_command.substr(to_write); + s->Callback(v); + }); + } + + void DoRead() { + if (empty_replies >= 5) { + // We will call this a timeout. + transport->Post(std::bind(callback, ETIMEDOUT)); + return; + } else if (empty_replies >= 2) { + // Sleep before each subsequent read. + ::usleep(controller->options_.diagnostic_retry_sleep_ns / 1000); + } + + DiagnosticRead::Command read; + output_frame_ = controller->DefaultFrame(kReplyRequired); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticRead::Make(&write_frame, read, {}); + + auto s = shared_from_this(); + + transport->Cycle( + &output_frame_, 1, &replies, + [s](int v) { + s->Callback(v); + }); + } + + bool ProcessReplies() { + for (const auto& reply : replies) { + if (reply.source != controller->options_.id || + reply.destination != controller->options_.source || + reply.can_prefix != controller->options_.can_prefix) { + continue; + } + + const auto parsed = DiagnosticResponse::Parse(reply.data, reply.size); + if (parsed.channel != 1) { continue; } + + if (parsed.size == 0) { + empty_replies++; + } else { + empty_replies = 0; + } + + current_line += std::string( + reinterpret_cast(parsed.data), parsed.size); + } + + size_t first_newline = std::string::npos; + while ((first_newline = current_line.find_first_of("\r\n")) + != std::string::npos) { + const auto this_line = current_line.substr(0, first_newline); + if (reply_mode == kExpectSingleLine) { + *result = this_line; + return true; + } else if (this_line == "OK") { + *result = output.str(); + return true; + } else { + output.write(current_line.data(), first_newline + 1); + current_line = current_line.substr(first_newline + 1); + } + } + replies.clear(); + return false; + } + }; + + struct AsyncDiagnosticWriteContext + : public std::enable_shared_from_this { + std::string message; + int channel = 0; + Controller* controller = nullptr; + Transport* transport = nullptr; + CompletionCallback callback; + + CanFdFrame output_frame_; + + void Start() { + DoWrite(); + } + + void DoWrite() { + DiagnosticWrite::Command write; + write.channel = channel; + write.data = message.data(); + const auto to_write = std::min(48, message.size()); + write.size = to_write; + + output_frame_ = controller->DefaultFrame(kNoReply); + WriteCanData write_frame(output_frame_.data, &output_frame_.size); + DiagnosticWrite::Make(&write_frame, write, {}); + + auto s = shared_from_this(); + + controller->transport()->Cycle( + &output_frame_, 1, nullptr, + [s, to_write](int v) { + s->message = s->message.substr(to_write); + s->Callback(v); + }); + } + + void Callback(int v) { + if (message.empty()) { + transport->Post(std::bind(callback, v)); + } else { + DoWrite(); + } + } + }; + + static void CheckRegisterMapVersion(const Result& result) { + if (result.values.extra[0].register_number != + Register::kRegisterMapVersion) { + throw std::runtime_error("Malformed response to schema version query"); + } + + const auto int_version = static_cast(result.values.extra[0].value); + if (kCurrentRegisterMapVersion != int_version) { + std::ostringstream ostr; + ostr << "Register map version mismatch device is " + << int_version + << " but library requires " + << kCurrentRegisterMapVersion; + + throw std::runtime_error(ostr.str()); + } + } + + Optional FindResult(const std::vector& replies) const { + // Pick off the last reply we got from our target ID. + for (auto it = replies.rbegin(); it != replies.rend(); ++it) { + if (it->source == options_.id && + it->destination == options_.source && + it->can_prefix == options_.can_prefix) { + + Result result; + result.frame = *it; + result.values = Query::Parse(it->data, it->size); + return result; + } + } + + // We didn't get anything. + return {}; + } + + enum ReplyMode { + kNoReply, + kReplyRequired, + }; + + CanFdFrame DefaultFrame(ReplyMode reply_mode = kReplyRequired) { + CanFdFrame result; + result.destination = options_.id; + result.reply_required = (reply_mode == kReplyRequired); + + result.arbitration_id = + (result.destination) | + (result.source << 8) | + (result.reply_required ? 0x8000 : 0x0000) | + (options_.can_prefix << 16); + result.bus = options_.bus; + + return result; + } + + template + CanFdFrame MakeFrame(const CommandType&, + const typename CommandType::Command& cmd, + const typename CommandType::Format& fmt, + const Query::Format* query_format_override = nullptr) { + auto result = DefaultFrame( + query_format_override != nullptr ? kReplyRequired : + options_.default_query ? kReplyRequired : kNoReply); + + WriteCanData write_frame(result.data, &result.size); + result.expected_reply_size = CommandType::Make(&write_frame, cmd, fmt); + + if (query_format_override) { + result.expected_reply_size = + Query::Make(&write_frame, *query_format_override); + } else if (options_.default_query) { + std::memcpy(&result.data[result.size], + &query_frame_.data[0], + query_frame_.size); + result.size += query_frame_.size; + result.expected_reply_size = query_reply_size_; + } + + return result; + } + + const Options options_; + std::shared_ptr transport_; + CanData query_frame_; + uint8_t query_reply_size_ = 0; + CanFdFrame output_frame_; + + // This is a member variable so we can avoid re-allocating it on + // every call. + std::vector single_command_replies_; +}; + + +} // namespace moteus +} // namespace mjbots diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h new file mode 100644 index 00000000..2f676fce --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h @@ -0,0 +1,674 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include + +#ifdef min +#undef min +#endif + +#ifdef max +#undef max +#endif + +namespace mjbots { +namespace moteus { + +/// Each value can be sent or received as one of the following. +enum Resolution : int8_t { + kInt8 = 0, + kInt16 = 1, + kInt32 = 2, + kFloat = 3, + kIgnore, +}; + +/// A vocabulary type for the basic data in a CAN-FD frame. +struct CanData { + uint8_t data[64] = {}; + uint8_t size = 0; +}; + +enum Multiplex : uint16_t { + kWriteBase = 0x00, + kWriteInt8 = 0x00, + kWriteInt16 = 0x04, + kWriteInt32 = 0x08, + kWriteFloat = 0x0c, + + kReadBase = 0x10, + kReadInt8 = 0x10, + kReadInt16 = 0x14, + kReadInt32 = 0x18, + kReadFloat = 0x1c, + + kReplyBase = 0x20, + kReplyInt8 = 0x20, + kReplyInt16 = 0x24, + kReplyInt32 = 0x28, + kReplyFloat = 0x2c, + + kWriteError = 0x30, + kReadError = 0x31, + + // # Tunneled Stream # + kClientToServer = 0x40, + kServerToClient = 0x41, + kClientPollServer = 0x42, + + kNop = 0x50, +}; + +namespace detail { +template +class numeric_limits { + public: +}; + +template <> +class numeric_limits { + public: + static int8_t max() { return 127; } + static int8_t min() { return -128; } +}; + +template <> +class numeric_limits { + public: + static int16_t max() { return 32767; } + static int16_t min() { return -32768; } +}; + +template <> +class numeric_limits { + public: + static int32_t max() { return 2147483647; } + static int32_t min() { return -2147483648; } +}; + +template <> +class numeric_limits { + public: +}; + +template +T max(T lhs, T rhs) { + return (lhs >= rhs) ? lhs : rhs; +} + +template +T min(T lhs, T rhs) { + return (lhs <= rhs) ? lhs : rhs; +} + +} + +namespace { +template +T Saturate(double value, double scale) { + // TODO: Implement without numeric_limits + if (!::isfinite(value)) { + return detail::numeric_limits::min(); + } + + const double scaled = value / scale; + const auto max = detail::numeric_limits::max(); + + const double double_max = static_cast(max); + // We purposefully limit to +- max, rather than to min. The minimum + // value for our two's complement types is reserved for NaN. + if (scaled < -double_max) { return -max; } + if (scaled > double_max) { return max; } + return static_cast(scaled); +} +} + +/// This class can be used to append values to the end of a CAN-FD +/// frame. +class WriteCanData { + public: + WriteCanData(CanData* frame) : data_(&frame->data[0]), size_(&frame->size) {} + WriteCanData(uint8_t* data, uint8_t* size) : data_(data), size_(size) {} + + uint8_t size() const { return *size_; } + + template + void Write(X value_in) { +#ifndef __ORDER_LITTLE_ENDIAN__ +#error "only little endian architectures supported" +#endif + + T value = static_cast(value_in); + if (sizeof(value) + *size_ > 64) { + abort(); + } + + ::memcpy(&data_[*size_], + reinterpret_cast(&value), + sizeof(value)); + *size_ += sizeof(value); + } + + void WriteVaruint(uint32_t value) { + while (true) { + auto this_byte = value & 0x7f; + value >>= 7; + this_byte |= ((value != 0) ? 0x80 : 0x00); + Write(this_byte); + + if (value == 0) { break; } + } + } + + void Write(const char* data, uint16_t size) { + if ((size + *size_) > 64) { + abort(); + } + ::memcpy(&data_[*size_], data, size); + *size_ += size; + } + + void WriteInt(int32_t value, Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(detail::max( + -127, detail::min(127, value))); + break; + } + case Resolution::kInt16: { + Write(detail::max( + -32767, detail::min(32767, value))); + break; + } + case Resolution::kInt32: { + Write(value); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WriteMapped( + double value, + double int8_scale, double int16_scale, double int32_scale, + Resolution res) { + switch (res) { + case Resolution::kInt8: { + Write(Saturate(value, int8_scale)); + break; + } + case Resolution::kInt16: { + Write(Saturate(value, int16_scale)); + break; + } + case Resolution::kInt32: { + Write(Saturate(value, int32_scale)); + break; + } + case Resolution::kFloat: { + Write(static_cast(value)); + break; + } + case Resolution::kIgnore: { + abort(); + } + } + } + + void WritePosition(double value, Resolution res) { + WriteMapped(value, 0.01, 0.0001, 0.00001, res); + } + + void WriteVelocity(double value, Resolution res) { + WriteMapped(value, 0.1, 0.00025, 0.00001, res); + } + + void WriteAccel(double value, Resolution res) { + WriteMapped(value, 0.05, 0.001, 0.00001, res); + } + + void WriteTorque(double value, Resolution res) { + WriteMapped(value, 0.5, 0.01, 0.001, res); + } + + void WritePwm(double value, Resolution res) { + WriteMapped(value, + 1.0 / 127.0, + 1.0 / 32767.0, + 1.0 / 2147483647.0, + res); + } + + void WriteVoltage(double value, Resolution res) { + WriteMapped(value, 0.5, 0.1, 0.001, res); + } + + void WriteTemperature(float value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + void WriteTime(double value, Resolution res) { + WriteMapped(value, 0.01, 0.001, 0.000001, res); + } + + void WriteCurrent(double value, Resolution res) { + WriteMapped(value, 1.0, 0.1, 0.001, res); + } + + private: + uint8_t* const data_; + uint8_t* const size_; +}; + +/// Read typed values from a CAN frame. +class MultiplexParser { + public: + MultiplexParser(const CanData* frame) + : data_(&frame->data[0]), + size_(frame->size) {} + MultiplexParser(const uint8_t* data, uint8_t size) + : data_(data), + size_(size) {} + + uint16_t ReadVaruint() { + uint16_t result = 0; + uint16_t shift = 0; + + for (int8_t i = 0; i < 5; i++) { + if (remaining() == 0) { return result; } + + const auto this_byte = static_cast(Read()); + result |= (this_byte & 0x7f) << shift; + shift += 7; + + if ((this_byte & 0x80) == 0) { + return result; + } + } + return result; + } + + struct Result { + bool done = true; + uint16_t value = 0; + Resolution resolution = kIgnore; + + Result(bool done_in, uint16_t value_in, Resolution resolution_in) + : done(done_in), value(value_in), resolution(resolution_in) {} + + Result() {} + }; + + Result next() { + if (offset_ >= size_) { + // We are done. + return Result(true, 0, Resolution::kInt8); + } + + if (remaining_) { + remaining_--; + const auto this_register = current_register_++; + + // Do we actually have enough data? + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, this_register, current_resolution_); + } + + // We need to look for another command. + while (offset_ < size_) { + const auto cmd = data_[offset_++]; + if (cmd == Multiplex::kNop) { continue; } + + // We are guaranteed to still need data. + if (offset_ >= size_) { + // Nope, we are out. + break; + } + + if (cmd >= 0x20 && cmd < 0x30) { + // This is a regular reply of some sort. + const auto id = (cmd >> 2) & 0x03; + current_resolution_ = [id]() { + switch (id) { + case 0: return Resolution::kInt8; + case 1: return Resolution::kInt16; + case 2: return Resolution::kInt32; + case 3: return Resolution::kFloat; + } + // we cannot reach this point + return Resolution::kInt8; + }(); + int8_t count = cmd & 0x03; + if (count == 0) { + count = data_[offset_++]; + + // We still need more data. + if (offset_ >= size_) { + break; + } + } + + if (count == 0) { + // Empty, guess we can ignore. + continue; + } + + current_register_ = ReadVaruint(); + remaining_ = count - 1; + + if (offset_ + ResolutionSize(current_resolution_) > size_) { + return Result(true, 0, Resolution::kInt8); + } + + return Result(false, current_register_++, current_resolution_); + } + + // For anything else, we'll just assume it is an error of some + // sort and stop parsing. + offset_ = size_; + break; + } + return Result(true, 0, Resolution::kInt8); + } + + template + T Read() { + if (offset_ + sizeof(T) > size_) { + abort(); + } + + T result = {}; + ::memcpy(&result, &data_[offset_], sizeof(T)); + offset_ += sizeof(T); + return result; + } + + template + double Nanify(T value) { + if (value == detail::numeric_limits::min()) { + return NaN; + } + return value; + } + + double ReadMapped(Resolution res, + double int8_scale, + double int16_scale, + double int32_scale) { + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + abort(); + } + + static constexpr int8_t kInt = 0; + static constexpr int8_t kPosition = 1; + static constexpr int8_t kVelocity = 2; + static constexpr int8_t kTorque = 3; + static constexpr int8_t kPwm = 4; + static constexpr int8_t kVoltage = 5; + static constexpr int8_t kTemperature = 6; + static constexpr int8_t kTime = 7; + static constexpr int8_t kCurrent = 8; + static constexpr int8_t kTheta = 9; + static constexpr int8_t kPower = 10; + static constexpr int8_t kAcceleration = 11; + + double ReadConcrete(Resolution res, int8_t concrete_type) { +#ifndef ARDUINO + static constexpr double kMappingValues[] = { +#else + static constexpr float PROGMEM kMappingValues[] = { +#endif + 1.0, 1.0, 1.0, // kInt + 0.01, 0.0001, 0.00001, // kPosition + 0.1, 0.00025, 0.00001, // kVelocity + 0.5, 0.01, 0.001, // kTorque + 1.0 / 127.0, 1.0 / 32767.0, 1.0 / 2147483647.0, // kPwm + 0.5, 0.1, 0.001, // kVoltage + 1.0, 0.1, 0.001, // kTemperature + 0.01, 0.001, 0.000001, // kTime + 1.0, 0.1, 0.001, // kCurrent + 1.0 / 127.0 * M_PI, 1.0 / 32767.0 * M_PI, 1.0 / 2147483647.0 * M_PI, // kTheta + 10.0, 0.05, 0.0001, // kPower + 0.05, 0.001, 0.00001, // kAcceleration + }; + +#ifndef ARDUINO + const double int8_scale = kMappingValues[concrete_type * 3 + 0]; + const double int16_scale = kMappingValues[concrete_type * 3 + 1]; + const double int32_scale = kMappingValues[concrete_type * 3 + 2]; +#else + const float int8_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 0); + const float int16_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 1); + const float int32_scale = pgm_read_float_near(kMappingValues + concrete_type * 3 + 2); +#endif + + switch (res) { + case Resolution::kInt8: { + return Nanify(Read()) * int8_scale; + } + case Resolution::kInt16: { + return Nanify(Read()) * int16_scale; + } + case Resolution::kInt32: { + return Nanify(Read()) * int32_scale; + } + case Resolution::kFloat: { + return Read(); + } + default: { + break; + } + } + + abort(); + } + + int ReadInt(Resolution res) { + return static_cast(ReadConcrete(res, kInt)); + } + + double ReadPosition(Resolution res) { + return ReadConcrete(res, kPosition); + } + + double ReadVelocity(Resolution res) { + return ReadConcrete(res, kVelocity); + } + + double ReadTorque(Resolution res) { + return ReadConcrete(res, kTorque); + } + + double ReadPwm(Resolution res) { + return ReadConcrete(res, kPwm); + } + + double ReadVoltage(Resolution res) { + return ReadConcrete(res, kVoltage); + } + + double ReadTemperature(Resolution res) { + return ReadConcrete(res, kTemperature); + } + + double ReadTime(Resolution res) { + return ReadConcrete(res, kTime); + } + + double ReadCurrent(Resolution res) { + return ReadConcrete(res, kCurrent); + } + + double ReadPower(Resolution res) { + return ReadConcrete(res, kPower); + } + + void Ignore(Resolution res) { + offset_ += ResolutionSize(res); + } + + void ReadRaw(uint8_t* output, uint16_t size) { + if ((offset_ + size) > size_) { ::abort(); } + ::memcpy(output, &data_[offset_], size); + offset_ += size; + } + + uint16_t remaining() const { + return size_ - offset_; + } + + static int8_t ResolutionSize(Resolution res) { + switch (res) { + case Resolution::kInt8: return 1; + case Resolution::kInt16: return 2; + case Resolution::kInt32: return 4; + case Resolution::kFloat: return 4; + default: { break; } + } + return 1; + } + + private: + const uint8_t* const data_; + const uint8_t size_; + uint16_t offset_ = 0; + + int8_t remaining_ = 0; + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t current_register_ = 0; +}; + +/// Determines how to group registers when encoding them to minimize +/// the required bytes. +class WriteCombiner { + public: + WriteCombiner(WriteCanData* frame, + int8_t base_command, + uint16_t start_register, + const Resolution* resolutions, + uint16_t resolutions_size) + : frame_(frame), + base_command_(base_command), + start_register_(start_register), + resolutions_(resolutions), + resolutions_size_(resolutions_size) {} + + ~WriteCombiner() { + if (offset_ != resolutions_size_) { + ::abort(); + } + } + + uint8_t reply_size() const { return reply_size_; } + + bool MaybeWrite() { + const auto this_offset = offset_; + offset_++; + + if (current_resolution_ == resolutions_[this_offset]) { + // We don't need to write any register operations here, and the + // value should go out only if requested. + return current_resolution_ != Resolution::kIgnore; + } + // We need to do some kind of framing. See how far ahead the new + // resolution goes. + const auto new_resolution = resolutions_[this_offset]; + current_resolution_ = new_resolution; + + // We are now in a new block of ignores. + if (new_resolution == Resolution::kIgnore) { + return false; + } + + int16_t count = 1; + for (uint16_t i = this_offset + 1; + i < resolutions_size_ && resolutions_[i] == new_resolution; + i++) { + count++; + } + + int8_t write_command = base_command_ + [&]() { + switch (new_resolution) { + case Resolution::kInt8: return 0x00; + case Resolution::kInt16: return 0x04; + case Resolution::kInt32: return 0x08; + case Resolution::kFloat: return 0x0c; + case Resolution::kIgnore: { + abort(); + } + } + return 0x00; + }(); + + const auto start_size = frame_->size(); + if (count <= 3) { + // Use the shorthand formulation. + frame_->Write(write_command + count); + } else { + // Nope, the long form. + frame_->Write(write_command); + frame_->Write(count); + } + frame_->WriteVaruint(start_register_ + this_offset); + const auto end_size = frame_->size(); + + reply_size_ += (end_size - start_size); + reply_size_ += count * MultiplexParser::ResolutionSize(new_resolution); + + return true; + } + + private: + WriteCanData* const frame_; + int8_t base_command_ = 0; + uint16_t start_register_ = 0; + const Resolution* const resolutions_; + uint16_t resolutions_size_ = 0; + + Resolution current_resolution_ = Resolution::kIgnore; + uint16_t offset_ = 0; + uint8_t reply_size_ = 0; +}; + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h new file mode 100644 index 00000000..3aa263d5 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h @@ -0,0 +1,53 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +namespace mjbots { +namespace moteus { + +// A stripped down optional class for C++11 environments. +template +class Optional { + public: + Optional() : dummy_(0), engaged_(false) {} + Optional(const T& t) : val_(t), engaged_(true) {} + + ~Optional() { + if (engaged_) { val_.~T(); } + } + + Optional& operator=(const T& val) { + engaged_ = true; + val_ = val; + return *this; + } + + bool has_value() const { return engaged_; } + + T& operator*() noexcept { return val_; } + T* operator->() noexcept { return &val_; } + const T& operator*() const noexcept { return val_; } + const T* operator->() const noexcept { return &val_; } + + explicit operator bool() const noexcept { return engaged_; } + bool operator!() const noexcept { return !engaged_; } + + private: + union { char dummy_; T val_; }; + bool engaged_; +}; + +} // namespace moteus +} // namespace mjbots diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h new file mode 100644 index 00000000..48b401b2 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h @@ -0,0 +1,1289 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// This file contains primitives used to encode and decode CAN-FD +/// messages for the mjbots moteus brushless controller. While C++, +/// it is designed to be usable in "minimal-C++" environments like +/// Arduino, in addition to fully standards conforming environments. + +#pragma once + +#include + +#ifndef ARDUINO + +#include +#include +#define NaN std::numeric_limits::quiet_NaN() + +#else + +#define NaN (0.0 / 0.0) + +#endif + + +#include "moteus_multiplex.h" + +namespace mjbots { +namespace moteus { + +/// This is a single CAN-FD frame, its headers, and other associated +/// metadata, like which bus the message was sent or received from in +/// multi-bus systems. +struct CanFdFrame { + /////////////////////////////////////////// + /// First are the raw data from the bus. + + // Non-zero if this transport supports multiple busses. + int8_t bus = 0; + + uint32_t arbitration_id = 0; + uint8_t data[64] = {}; + uint8_t size = 0; + + enum Toggle { + kDefault, + kForceOff, + kForceOn, + }; + + Toggle brs = kDefault; + Toggle fdcan_frame = kDefault; + + ////////////////////////////////////////// + /// Next are parsed items for moteus frames. These are not required + /// to be set when sending a frame to a transport, but they will be + /// filled in on receipt. Higher level layers than the transport + /// layer may use them to populate the bus-level data. + + // If true, then the ID used is not calculated from destination and + // source, but is instead determined directly from arbitration_id. + bool raw = false; + + int8_t destination = 0; + int8_t source = 0; + uint16_t can_prefix = 0x0000; // A 13 bit CAN prefix + + //////////////////////////////////////// + /// Finally other hinting data. + + // Whether this frame is expected to elicit a response. + bool reply_required = false; + + // If this frame does elicit a response, how large is it expected to + // be. + uint8_t expected_reply_size = 0; +}; + + +/// The expected version associated with register 0x102. If it +/// differs from this, then semantics of one or more registers may +/// have changed. +enum { + kCurrentRegisterMapVersion = 5, +}; + +enum Register : uint16_t { + kMode = 0x000, + kPosition = 0x001, + kVelocity = 0x002, + kTorque = 0x003, + kQCurrent = 0x004, + kDCurrent = 0x005, + kAbsPosition = 0x006, + kPower = 0x007, + + kMotorTemperature = 0x00a, + kTrajectoryComplete = 0x00b, + kHomeState = 0x00c, + kVoltage = 0x00d, + kTemperature = 0x00e, + kFault = 0x00f, + + kPwmPhaseA = 0x010, + kPwmPhaseB = 0x011, + kPwmPhaseC = 0x012, + + kVoltagePhaseA = 0x014, + kVoltagePhaseB = 0x015, + kVoltagePhaseC = 0x016, + + kVFocTheta = 0x018, + kVFocVoltage = 0x019, + kVoltageDqD = 0x01a, + kVoltageDqQ = 0x01b, + + kCommandQCurrent = 0x01c, + kCommandDCurrent = 0x01d, + + kCommandPosition = 0x020, + kCommandVelocity = 0x021, + kCommandFeedforwardTorque = 0x022, + kCommandKpScale = 0x023, + kCommandKdScale = 0x024, + kCommandPositionMaxTorque = 0x025, + kCommandStopPosition = 0x026, + kCommandTimeout = 0x027, + kCommandVelocityLimit = 0x028, + kCommandAccelLimit = 0x029, + kCommandFixedVoltageOverride = 0x02a, + kCommandIlimitScale = 0x02b, + + kPositionKp = 0x030, + kPositionKi = 0x031, + kPositionKd = 0x032, + kPositionFeedforward = 0x033, + kPositionCommand = 0x034, + + kControlPosition = 0x038, + kControlVelocity = 0x039, + kControlTorque = 0x03a, + kControlPositionError = 0x03b, + kControlVelocityError = 0x03c, + kControlTorqueError = 0x03d, + + kCommandStayWithinLowerBound = 0x040, + kCommandStayWithinUpperBound = 0x041, + kCommandStayWithinFeedforwardTorque = 0x042, + kCommandStayWithinKpScale = 0x043, + kCommandStayWithinKdScale = 0x044, + kCommandStayWithinPositionMaxTorque = 0x045, + kCommandStayWithinTimeout = 0x046, + kCommandStayWithinIlimitScale = 0x047, + + kEncoder0Position = 0x050, + kEncoder0Velocity = 0x051, + kEncoder1Position = 0x052, + kEncoder1Velocity = 0x053, + kEncoder2Position = 0x054, + kEncoder2Velocity = 0x055, + + kEncoderValidity = 0x058, + + kAux1GpioCommand = 0x05c, + kAux2GpioCommand = 0x05d, + kAux1GpioStatus = 0x05e, + kAux2GpioStatus = 0x05f, + + kAux1AnalogIn1 = 0x060, + kAux1AnalogIn2 = 0x061, + kAux1AnalogIn3 = 0x062, + kAux1AnalogIn4 = 0x063, + kAux1AnalogIn5 = 0x064, + + kAux2AnalogIn1 = 0x068, + kAux2AnalogIn2 = 0x069, + kAux2AnalogIn3 = 0x06a, + kAux2AnalogIn4 = 0x06b, + kAux2AnalogIn5 = 0x06c, + + kMillisecondCounter = 0x070, + kClockTrim = 0x071, + + kRegisterMapVersion = 0x102, + kSerialNumber = 0x120, + kSerialNumber1 = 0x120, + kSerialNumber2 = 0x121, + kSerialNumber3 = 0x122, + + kRezero = 0x130, + kSetOutputNearest = 0x130, + kSetOutputExact = 0x131, + kRequireReindex = 0x132, + kRecapturePositionVelocity = 0x133, + + kDriverFault1 = 0x140, + kDriverFault2 = 0x141, +}; + +enum class Mode { + kStopped = 0, + kFault = 1, + kEnabling = 2, + kCalibrating = 3, + kCalibrationComplete = 4, + kPwm = 5, + kVoltage = 6, + kVoltageFoc = 7, + kVoltageDq = 8, + kCurrent = 9, + kPosition = 10, + kPositionTimeout = 11, + kZeroVelocity = 12, + kStayWithin = 13, + kMeasureInd = 14, + kBrake = 15, + kNumModes, +}; + +enum class HomeState { + kRelative = 0, + kRotor = 1, + kOutput = 2, +}; + + +//////////////////////////////////////////////////////////////// +// Each command that can be issued is represented by a structure below +// with a few different sub-structs. Possibilities are: +// +// 'Command' items that are serialized in an outgoing message +// 'Format' the corresponding resolution for each item in 'Command' +// 'Result' the deserialized version of a response +// +// Additionally, there are two static methods possible: +// +// 'Make' - take a Command and Format, and serialize it +// 'Parse' - deserialize CAN data into a 'Result' structure + +struct EmptyMode { + struct Command {}; + struct Format {}; + static uint8_t Make(WriteCanData*, const Command&, const Format&) { + return 0; + } +}; + +struct Query { + struct ItemValue { + int16_t register_number = detail::numeric_limits::max(); + double value = 0.0; + }; + +#ifndef ARDUINO + static constexpr int16_t kMaxExtra = 16; +#else + static constexpr int16_t kMaxExtra = 8; +#endif + + struct Result { + Mode mode = Mode::kStopped; + double position = NaN; + double velocity = NaN; + double torque = NaN; + double q_current = NaN; + double d_current = NaN; + double abs_position = NaN; + double power = NaN; + double motor_temperature = NaN; + bool trajectory_complete = false; + HomeState home_state = HomeState::kRelative; + double voltage = NaN; + double temperature = NaN; + int8_t fault = 0; + + int8_t aux1_gpio = 0; + int8_t aux2_gpio = 0; + + // Before gcc-12, initializating non-POD array types can be + // painful if done in the idiomatic way with ={} inline. Instead + // we do it in the constructor. + // + // https://gcc.gnu.org/bugzilla/show_bug.cgi?id=92385 + ItemValue extra[kMaxExtra]; + + Result() : extra() {} + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = moteus::kIgnore; + }; + + struct Format { + Resolution mode = kInt8; + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution torque = kFloat; + Resolution q_current = kIgnore; + Resolution d_current = kIgnore; + Resolution abs_position = kIgnore; + Resolution power = kIgnore; + Resolution motor_temperature = kIgnore; + Resolution trajectory_complete = kIgnore; + Resolution home_state = kIgnore; + Resolution voltage = kInt8; + Resolution temperature = kInt8; + Resolution fault = kInt8; + + Resolution aux1_gpio = kIgnore; + Resolution aux2_gpio = kIgnore; + + // Any values here must be sorted by register number. + ItemFormat extra[kMaxExtra]; + + // gcc bug 92385 again + Format() : extra() {} + }; + + static uint8_t Make(WriteCanData* frame, const Format& format) { + uint8_t reply_size = 0; + + { + const Resolution kResolutions[] = { + format.mode, + format.position, + format.velocity, + format.torque, + format.q_current, + format.d_current, + format.abs_position, + format.power, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMode, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.motor_temperature, + format.trajectory_complete, + format.home_state, + format.voltage, + format.temperature, + format.fault, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kMotorTemperature, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const Resolution kResolutions[] = { + format.aux1_gpio, + format.aux2_gpio, + }; + const uint16_t kResolutionsSize = sizeof(kResolutions) / sizeof(*kResolutions); + WriteCombiner combiner( + frame, 0x10, Register::kAux1GpioStatus, + kResolutions, kResolutionsSize); + for (uint16_t i = 0; i < kResolutionsSize; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxExtra; i++) { + if (format.extra[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxExtra; + }(); + + if (size == 0) { return reply_size; } + const int16_t min_register_number = format.extra[0].register_number; + const int16_t max_register_number = format.extra[size - 1].register_number; + + const uint16_t required_registers = + max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.extra[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.extra[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + reply_size += combiner.reply_size(); + } + + return reply_size; + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t extra_index = 0; + + while (true) { + const auto current = parser->next(); + if (current.done) { return result; } + + const auto res = current.resolution; + + switch (static_cast(current.value)) { + case Register::kMode: { + result.mode = static_cast(parser->ReadInt(res)); + break; + } + case Register::kPosition: { + result.position = parser->ReadPosition(res); + break; + } + case Register::kVelocity: { + result.velocity = parser->ReadVelocity(res); + break; + } + case Register::kTorque: { + result.torque = parser->ReadTorque(res); + break; + } + case Register::kQCurrent: { + result.q_current = parser->ReadCurrent(res); + break; + } + case Register::kDCurrent: { + result.d_current = parser->ReadCurrent(res); + break; + } + case Register::kAbsPosition: { + result.abs_position = parser->ReadPosition(res); + break; + } + case Register::kPower: { + result.power = parser->ReadPower(res); + break; + } + case Register::kMotorTemperature: { + result.motor_temperature = parser->ReadTemperature(res); + break; + } + case Register::kTrajectoryComplete: { + result.trajectory_complete = parser->ReadInt(res) != 0; + break; + } + case Register::kHomeState: { + result.home_state = static_cast(parser->ReadInt(res)); + break; + } + case Register::kVoltage: { + result.voltage = parser->ReadVoltage(res); + break; + } + case Register::kTemperature: { + result.temperature = parser->ReadTemperature(res); + break; + } + case Register::kFault: { + result.fault = parser->ReadInt(res); + break; + } + case Register::kAux1GpioStatus: { + result.aux1_gpio = parser->ReadInt(res); + break; + } + case Register::kAux2GpioStatus: { + result.aux2_gpio = parser->ReadInt(res); + break; + } + default: { + if (extra_index < kMaxExtra) { + result.extra[extra_index].register_number = current.value; + result.extra[extra_index].value = + ParseGeneric(parser, current.value, res); + extra_index++; + } else { + // Ignore this value. + parser->ReadInt(res); + } + break; + } + } + } + } + + static double ParseGeneric(MultiplexParser* parser, + int16_t register_number, + Resolution resolution) { + const auto res = resolution; + + using R = Register; + using MP = MultiplexParser; + + struct RegisterDefinition { + uint16_t register_number; + uint8_t block_size; + int8_t concrete; + }; +#ifndef ARDUINO + static constexpr RegisterDefinition kRegisterDefinitions[] = { +#else + static constexpr RegisterDefinition PROGMEM kRegisterDefinitions[] = { +#endif + { R::kMode, 1, MP::kInt, }, + { R::kPosition, 1, MP::kPosition, }, + { R::kVelocity, 1, MP::kVelocity, }, + { R::kTorque, 1, MP::kTorque, }, + { R::kQCurrent, 2, MP::kCurrent, }, + // { R::kDCurrent, 1, MP::kCurrent, }, + { R::kAbsPosition, 1, MP::kPosition, }, + { R::kPower, 1, MP::kPower, }, + { R::kMotorTemperature, 1, MP::kTemperature, }, + { R::kTrajectoryComplete, 2, MP::kInt, }, + // { R::kHomeState, 1, MP::kInt, }, + { R::kVoltage, 1, MP::kVoltage, }, + { R::kTemperature, 1, MP::kTemperature, }, + { R::kFault, 1, MP::kInt, }, + + { R::kPwmPhaseA, 3, MP::kPwm, }, + // { R::kPwmPhaseB, 1, MP::kPwm, }, + // { R::kPwmPhaseC, 1, MP::kPwm, }, + + { R::kVoltagePhaseA, 3, MP::kVoltage, }, + // { R::kVoltagePhaseB, 1, MP::kVoltage, }, + // { R::kVoltagePhaseC, 1, MP::kVoltage, }, + + { R::kVFocTheta, 1, MP::kTheta, }, + { R::kVFocVoltage, 3, MP::kVoltage, }, + // { R::kVoltageDqD, 1, MP::kVoltage, }, + // { R::kVoltageDqQ, 1, MP::kVoltage, }, + + { R::kCommandQCurrent, 2, MP::kCurrent, }, + // { R::kCommandDCurrent, 1, MP::kCurrent, }, + + { R::kCommandPosition, 1, MP::kPosition, }, + { R::kCommandVelocity, 1, MP::kVelocity, }, + { R::kCommandFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandKpScale, 2, MP::kPwm, }, + // { R::kCommandKdScale, 1, MP::kPwm, }, + { R::kCommandPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStopPosition, 1, MP::kPosition, }, + { R::kCommandTimeout, 1, MP::kTime, }, + { R::kCommandVelocityLimit, 1, MP::kVelocity, }, + { R::kCommandAccelLimit, 1, MP::kAcceleration, }, + { R::kCommandFixedVoltageOverride, 1, MP::kVoltage }, + + { R::kPositionKp, 5, MP::kTorque, }, + // { R::kPositionKi, 1, MP::kTorque, }, + // { R::kPositionKd, 1, MP::kTorque, }, + // { R::kPositionFeedforward, 1, MP::kTorque, }, + // { R::kPositionCommand, 1, MP::kTorque, }, + + { R::kControlPosition, 1, MP::kPosition, }, + { R::kControlVelocity, 1, MP::kVelocity, }, + { R::kControlTorque, 1, MP::kTorque, }, + { R::kControlPositionError, 1, MP::kPosition, }, + { R::kControlVelocityError, 1, MP::kVelocity, }, + { R::kControlTorqueError, 1, MP::kTorque, }, + + { R::kCommandStayWithinLowerBound, 2, MP::kPosition, }, + // { R::kCommandStayWithinUpperBound, 1, MP::kPosition, }, + { R::kCommandStayWithinFeedforwardTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinKpScale, 2, MP::kPwm, }, + // { R::kCommandStayWithinKdScale, 1, MP::kPwm, }, + { R::kCommandStayWithinPositionMaxTorque, 1, MP::kTorque, }, + { R::kCommandStayWithinTimeout, 1, MP::kTime, }, + { R::kCommandStayWithinIlimitScale, 1, MP::kPwm }, + + { R::kEncoder0Position, 1, MP::kPosition, }, + { R::kEncoder0Velocity, 1, MP::kVelocity, }, + { R::kEncoder1Position, 1, MP::kPosition, }, + { R::kEncoder1Velocity, 1, MP::kVelocity, }, + { R::kEncoder2Position, 1, MP::kPosition, }, + { R::kEncoder2Velocity, 1, MP::kVelocity, }, + + { R::kEncoderValidity, 1, MP::kInt, }, + + { R::kAux1GpioCommand, 4, MP::kInt, }, + // { R::kAux2GpioCommand, 1, MP::kInt, }, + // { R::kAux1GpioStatus, 1, MP::kInt, }, + // { R::kAux2GpioStatus, 1, MP::kInt, }, + + { R::kAux1AnalogIn1, 5, MP::kPwm, }, + // { R::kAux1AnalogIn2, 1, MP::kPwm, }, + // { R::kAux1AnalogIn3, 1, MP::kPwm, }, + // { R::kAux1AnalogIn4, 1, MP::kPwm, }, + // { R::kAux1AnalogIn5, 1, MP::kPwm, }, + + { R::kAux2AnalogIn1, 5, MP::kPwm, }, + // { R::kAux2AnalogIn2, 1, MP::kPwm, }, + // { R::kAux2AnalogIn3, 1, MP::kPwm, }, + // { R::kAux2AnalogIn4, 1, MP::kPwm, }, + // { R::kAux2AnalogIn5, 1, MP::kPwm, }, + + { R::kMillisecondCounter, 2, MP::kInt, }, + // { R::kClockTrim, 1, MP::kInt, }, + + { R::kRegisterMapVersion, 1, MP::kInt, }, + { R::kSerialNumber1, 3, MP::kInt, }, + // { R::kSerialNumber2, 1, MP::kInt, }, + // { R::kSerialNumber3, 1, MP::kInt, }, + + { R::kSetOutputNearest, 3, MP::kInt, }, + // { R::kSetOutputExact, 1, MP::kInt, }, + // { R::kRequireReindex, 1, MP::kInt, }, + // { R::kRecapturePositionVelocity, 1, MP::kInt, } + + { R::kDriverFault1, 2, MP::kInt, }, + // { R::kDriverFault2, 1, MP::kInt, }, + + }; + for (uint16_t i = 0; + i < sizeof(kRegisterDefinitions) / sizeof (*kRegisterDefinitions); + i ++) { + +#ifndef ARDUINO + const int16_t start_reg = kRegisterDefinitions[i].register_number; + const uint8_t block_size = kRegisterDefinitions[i].block_size; + const int8_t concrete_type = kRegisterDefinitions[i].concrete; +#else + const int16_t start_reg = pgm_read_word_near(&kRegisterDefinitions[i].register_number); + const uint8_t block_size = pgm_read_byte_near(&kRegisterDefinitions[i].block_size); + const int8_t concrete_type = pgm_read_byte_near(&kRegisterDefinitions[i].concrete); +#endif + if (register_number >= start_reg && + register_number < (start_reg + block_size)) { + return parser->ReadConcrete(res, concrete_type); + } + } + return parser->ReadInt(res); + } +}; + +struct GenericQuery { + struct Command {}; + + struct ItemValue { + int16_t register_number = -1; + double value = 0.0; + }; + + // A CAN-FD frame can only have 64 bytes, so we definitely can't + // have more than 64 items in a single one ever. + static constexpr int16_t kMaxItems = 64; + + struct Result { + ItemValue values[kMaxItems] = {}; + }; + + struct ItemFormat { + int16_t register_number = detail::numeric_limits::max(); + Resolution resolution = kIgnore; + }; + + struct Format { + // These values must be sorted by register number. + ItemFormat values[kMaxItems] = {}; + }; + + static int ItemFormatSort(const void* lhs_in, const void* rhs_in) { + const auto* lhs = reinterpret_cast(lhs_in); + const auto* rhs = reinterpret_cast(rhs_in); + + return lhs->register_number - rhs->register_number; + } + + static uint8_t Make(WriteCanData* frame, const Command&, const Format& format) { + const int16_t size = [&]() { + for (int16_t i = 0; i < kMaxItems; i++) { + if (format.values[i].register_number == + detail::numeric_limits::max()) { + return i; + } + } + return kMaxItems; + }(); + + if (size == 0) { return 0; } + const int16_t min_register_number = format.values[0].register_number; + const int16_t max_register_number = format.values[size - 1].register_number; + + const uint16_t required_registers = max_register_number - min_register_number + 1; + + // An arbitrary limit to constrain the amount of stack we use + // below. + if (required_registers > 512) { ::abort(); } + +#ifndef ARDUINO + std::vector resolutions(required_registers); +#else + Resolution resolutions[required_registers]; +#endif + ::memset(&resolutions[0], 0, sizeof(Resolution) * required_registers); + + for (int16_t this_register = min_register_number, index = 0; + this_register <= max_register_number; + this_register++) { + if (format.values[index].register_number == this_register) { + resolutions[this_register - min_register_number] = + format.values[index].resolution; + index++; + } else { + resolutions[this_register - min_register_number] = kIgnore; + } + } + WriteCombiner combiner( + frame, 0x10, min_register_number, + &resolutions[0], required_registers); + for (uint16_t i = 0; i < required_registers; i++) { + combiner.MaybeWrite(); + } + + return combiner.reply_size(); + } + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + + return Parse(&parser); + } + + static Result Parse(const CanData* frame) { + MultiplexParser parser(frame); + + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + + int16_t index = 0; + + while (true) { + if (index >= kMaxItems) { return result; } + + const auto current = parser->next(); + if (current.done) { return result; } + + result.values[index].register_number = current.value; + result.values[index].value = + Query::ParseGeneric(parser, current.value, current.resolution); + + index++; + } + } +}; + +struct PositionMode { + struct Command { + double position = 0.0; + double velocity = 0.0; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = NaN; + double stop_position = NaN; + double watchdog_timeout = NaN; + double velocity_limit = NaN; + double accel_limit = NaN; + double fixed_voltage_override = NaN; + double ilimit_scale = 1.0; + }; + + struct Format { + Resolution position = kFloat; + Resolution velocity = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution stop_position = kIgnore; + Resolution watchdog_timeout = kIgnore; + Resolution velocity_limit = kIgnore; + Resolution accel_limit = kIgnore; + Resolution fixed_voltage_override = kIgnore; + Resolution ilimit_scale = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kPosition); + + // Now we use some heuristics to try and group consecutive registers + // of the same resolution together into larger writes. + const Resolution kResolutions[] = { + format.position, + format.velocity, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.stop_position, + format.watchdog_timeout, + format.velocity_limit, + format.accel_limit, + format.fixed_voltage_override, + format.ilimit_scale, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kCommandPosition, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.position, format.position); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity, format.velocity); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.stop_position, format.stop_position); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.velocity_limit, format.velocity_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteAccel(command.accel_limit, format.accel_limit); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.fixed_voltage_override, + format.fixed_voltage_override); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } + return 0; + } +}; + +/// The voltage-mode FOC command. +struct VFOCMode { + struct Command { + double theta_rad = 0.0; + double voltage = 0.0; + double theta_rad_rate = 0.0; + }; + + struct Format { + Resolution theta_rad = kFloat; + Resolution voltage = kFloat; + Resolution theta_rad_rate = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kVoltageFoc); + + const Resolution kResolutions[] = { + format.theta_rad, + format.voltage, + kIgnore, + kIgnore, + kIgnore, + kIgnore, + format.theta_rad_rate, + }; + WriteCombiner combiner( + frame, 0x00, + Register::kVFocTheta, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePwm(command.theta_rad / M_PI, format.theta_rad); + } + if (combiner.MaybeWrite()) { + frame->WriteVoltage(command.voltage, format.voltage); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + abort(); + } + if (combiner.MaybeWrite()) { + frame->WriteVelocity(command.theta_rad_rate / M_PI, format.theta_rad_rate); + } + + return 0; + } +}; + +/// DQ phase current command. +struct CurrentMode { + struct Command { + double d_A = 0.0; + double q_A = 0.0; + }; + + struct Format { + Resolution d_A = kFloat; + Resolution q_A = kFloat; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kCurrent); + + const Resolution kResolutions[] = { + format.d_A, + format.q_A, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandQCurrent, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.q_A, format.q_A); + } + if (combiner.MaybeWrite()) { + frame->WriteCurrent(command.d_A, format.d_A); + } + + return 0; + } +}; + +struct StayWithinMode { + struct Command { + double lower_bound = NaN; + double upper_bound = NaN; + double feedforward_torque = 0.0; + double kp_scale = 1.0; + double kd_scale = 1.0; + double maximum_torque = 0.0; + double watchdog_timeout = NaN; + double ilimit_scale = 1.0; + }; + + struct Format { + Resolution lower_bound = kFloat; + Resolution upper_bound = kFloat; + Resolution feedforward_torque = kIgnore; + Resolution kp_scale = kIgnore; + Resolution kd_scale = kIgnore; + Resolution maximum_torque = kIgnore; + Resolution watchdog_timeout = kIgnore; + Resolution ilimit_scale = kIgnore; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStayWithin); + + const Resolution kResolutions[] = { + format.lower_bound, + format.upper_bound, + format.feedforward_torque, + format.kp_scale, + format.kd_scale, + format.maximum_torque, + format.watchdog_timeout, + format.ilimit_scale, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kCommandStayWithinLowerBound, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WritePosition(command.lower_bound, format.lower_bound); + } + if (combiner.MaybeWrite()) { + frame->WritePosition(command.upper_bound, format.upper_bound); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.feedforward_torque, + format.feedforward_torque); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kp_scale, format.kp_scale); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.kd_scale, format.kd_scale); + } + if (combiner.MaybeWrite()) { + frame->WriteTorque(command.maximum_torque, format.maximum_torque); + } + if (combiner.MaybeWrite()) { + frame->WriteTime(command.watchdog_timeout, format.watchdog_timeout); + } + if (combiner.MaybeWrite()) { + frame->WritePwm(command.ilimit_scale, format.ilimit_scale); + } + return 0; + } +}; + +struct BrakeMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kBrake); + return 0; + } +}; + +struct StopMode { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, + const Command&, + const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->Write(Register::kMode); + frame->Write(Mode::kStopped); + return 0; + } +}; + +struct GpioWrite { + struct Command { + int8_t aux1 = 0; + int8_t aux2 = 0; + }; + + struct Format { + Resolution aux1 = kInt8; + Resolution aux2 = kInt8; + }; + + static uint8_t Make(WriteCanData* frame, + const Command& command, + const Format& format) { + const Resolution kResolutions[] = { + format.aux1, + format.aux2, + }; + + WriteCombiner combiner( + frame, 0x00, + Register::kAux1GpioCommand, + kResolutions, + sizeof(kResolutions) / sizeof(*kResolutions)); + + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux1, format.aux1); + } + if (combiner.MaybeWrite()) { + frame->WriteInt(command.aux2, format.aux2); + } + return 0; + } +}; + +struct OutputNearest { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputNearest); + frame->Write(command.position); + return 0; + } +}; + +struct OutputExact { + struct Command { + double position = 0.0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteFloat | 0x01); + frame->WriteVaruint(Register::kSetOutputExact); + frame->Write(command.position); + return 0; + } +}; + +struct RequireReindex { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRequireReindex); + frame->Write(1); + return 0; + } +}; + +struct RecapturePositionVelocity { + struct Command {}; + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command&, const Format&) { + frame->Write(Multiplex::kWriteInt8 | 0x01); + frame->WriteVaruint(Register::kRecapturePositionVelocity); + frame->Write(1); + return 0; + } +}; + +struct DiagnosticWrite { + struct Command { + int8_t channel = 1; + const char* data = nullptr; + int8_t size = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientToServer); + frame->Write(command.channel); + frame->Write(command.size); + frame->Write(command.data, command.size); + return 0; + } +}; + +struct DiagnosticRead { + struct Command { + int8_t channel = 1; + int8_t max_length = 48; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kClientPollServer); + frame->Write(command.channel); + frame->Write(command.max_length); + return command.max_length + 3; + } +}; + +struct DiagnosticResponse { + struct Result { + int8_t channel = 1; + uint8_t data[64] = {}; + int8_t size = 0; + }; + + static Result Parse(const uint8_t* data, uint8_t size) { + MultiplexParser parser(data, size); + return Parse(&parser); + } + + static Result Parse(MultiplexParser* parser) { + Result result; + result.channel = -1; + + if (parser->remaining() < 3) { return result; } + + const auto action = parser->Read(); + if (action != Multiplex::kServerToClient) { return result; } + const auto channel = parser->Read(); + + const uint16_t size = parser->ReadVaruint(); + if (parser->remaining() < size) { return result; } + + result.channel = channel; + result.size = size; + parser->ReadRaw(result.data, size); + + return result; + } +}; + +struct ClockTrim { + struct Command { + int32_t trim = 0; + }; + + struct Format {}; + + static uint8_t Make(WriteCanData* frame, const Command& command, const Format&) { + frame->Write(Multiplex::kWriteInt32 | 0x01); + frame->WriteVaruint(Register::kClockTrim); + frame->Write(command.trim); + return 0; + } +}; + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h new file mode 100644 index 00000000..502ee969 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h @@ -0,0 +1,62 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +namespace mjbots { +namespace moteus { +namespace detail { + +class Tokenizer { + public: + Tokenizer(const std::string& source, const char* delimiters) + : source_(source), + delimiters_(delimiters), + position_(source_.cbegin()) {} + + std::string next() { + if (position_ == source_.end()) { return std::string(); } + + const auto start = position_; + auto my_next = position_; + bool found = false; + for (; my_next != source_.end(); ++my_next) { + if (std::strchr(delimiters_, *my_next) != nullptr) { + position_ = my_next; + ++position_; + found = true; + break; + } + } + if (!found) { position_ = my_next; } + return std::string(&*start, my_next - start); + } + + std::string remaining() const { + return std::string(&*position_, source_.end() - position_); + } + + private: + const std::string source_; + const char* const delimiters_; + std::string::const_iterator position_; +}; + + +} // namespace detail +} // namespace moteus +} // namespace mjbots diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h new file mode 100644 index 00000000..822c3c9d --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h @@ -0,0 +1,1128 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "moteus_protocol.h" +#include "moteus_tokenizer.h" + +#ifdef CANFD_FDF +#define MJBOTS_MOTEUS_ENABLE_SOCKETCAN 1 +#endif + +namespace mjbots { +namespace moteus { + +using CompletionCallback = std::function; + +/// Turn async methods into synchronous ones. +class BlockingCallback { + public: + /// Pass the result of this to a singular async call. + CompletionCallback callback() { + return [&](int v) { + std::unique_lock lock(mutex_); + done_.store(true); + result_.store(v); + cv_.notify_one(); + }; + } + + /// Then call this to perform the blocking. + int Wait() { + cv_.wait(lock_, [&]() { return done_.load(); }); + return result_.load(); + } + + private: + std::atomic done_{false}; + std::atomic result_{0}; + std::recursive_mutex mutex_; + std::condition_variable_any cv_; + std::unique_lock lock_{mutex_}; +}; + +class Transport { + public: + virtual ~Transport() {} + + /// Start sending all the frames in the @p frames / @p size list. + /// + /// Any replies are collated in the @p replies result. + /// + /// Upon completion, invoke @p completed_callback from an arbitrary + /// thread that may or may not be the calling one and may or may not + /// be invoked rentrantly from within this call. + virtual void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) = 0; + + /// The same operation as above, but block until it is complete. + /// + /// A default implementation is provided which delegates to the + /// asynchronous version. + virtual void BlockingCycle(const CanFdFrame* frames, + size_t size, + std::vector* replies) { + BlockingCallback cbk; + + this->Cycle(frames, size, replies, cbk.callback()); + + cbk.Wait(); + } + + /// Schedule the given callback to be invoked at a later time. This + /// will either be invoked from an arbitrary thread, or from within + /// another call to "Cycle or BlockingCycle". + virtual void Post(std::function callback) = 0; +}; + +namespace details { +/// This is just a simple RAII class for managing file descriptors. +class FileDescriptor { + public: + FileDescriptor() {} + FileDescriptor(int fd) { fd_ = fd; } + ~FileDescriptor() { + if (fd_ >= 0) { ::close(fd_); } + } + + FileDescriptor& operator=(int fd) { + if (fd_ >= 0) { ::close(fd_); } + fd_ = fd; + return *this; + } + + bool operator==(const FileDescriptor& rhs) const { + return fd_ == rhs.fd_; + } + + operator int() const { + return fd_; + } + + int release() { + const auto result = fd_; + fd_ = -1; + return result; + } + + private: + int fd_ = -1; +}; + +/// A basic event loop implemented using C++11 primitives. +class ThreadedEventLoop { + public: + ThreadedEventLoop() { + thread_ = std::thread(std::bind(&ThreadedEventLoop::CHILD_Run, this)); + } + + ~ThreadedEventLoop() { + { + std::unique_lock lock(mutex_); + done_ = true; + do_something_ = true; + something_cv_.notify_one(); + } + thread_.join(); + } + + // This is purely to catch out of control queues earlier, as + // typically there will just be 1 outstanding event at a time. + static constexpr int kMaxQueueSize = 2; + + void Post(std::function callback) { + std::unique_lock lock(mutex_); + event_queue_.push_back(std::move(callback)); + if (event_queue_.size() > kMaxQueueSize) { + throw std::runtime_error("There should never be more than one!"); + } + do_something_ = true; + something_cv_.notify_one(); + } + + private: + void CHILD_Run() { + std::unique_lock lock(mutex_); + + while (true) { + something_cv_.wait(lock, [&]() { + return do_something_ || !event_queue_.empty(); + }); + do_something_ = false; + + if (done_) { + return; + } + + // Do at most one event. + if (!event_queue_.empty()) { + auto top = event_queue_.front(); + event_queue_.pop_front(); + top(); + } + } + } + + std::thread thread_; + + // The following variables are controlled by 'something_mutex'. + std::recursive_mutex mutex_; + std::condition_variable_any something_cv_; + + bool do_something_ = false; + bool done_ = false; + + std::deque> event_queue_; +}; + +/// A helper base class for transports that want to manage timeout +/// behavior in a similar manner. +class TimeoutTransport : public Transport { + public: + struct Options { + bool disable_brs = false; + + uint32_t min_ok_wait_ns = 1000000; + uint32_t min_rcv_wait_ns = 5000000; + + uint32_t rx_extra_wait_ns = 5000000; + + // Send at most this many frames before waiting for responses. -1 + // means no limit. + int max_pipeline = -1; + + Options() {} + }; + + TimeoutTransport(const Options& options) : t_options_(options) {} + + virtual void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) override { + // The event loop should never be empty here, but we make a copy + // just to assert that. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + FailIf(!copy, "unexpected null event loop"); + copy->Post( + std::bind(&TimeoutTransport::CHILD_Cycle, + this, frames, size, replies, completed_callback)); + } + + virtual void Post(std::function callback) override { + // We might have an attempt to post an event while we are being + // destroyed. In that case, just ignore it. + auto copy = std::atomic_load(&UNPROTECTED_event_loop_); + if (copy) { + copy->Post(callback); + } + } + + static int64_t GetNow() { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) * 1000000000ll + + static_cast(ts.tv_nsec); + } + + static void Fail(const std::string& message) { + throw std::runtime_error(message); + } + + static void FailIf(bool terminate, const std::string& message) { + if (terminate) { + Fail(message); + } + } + + static void FailIfErrno(bool terminate) { + if (terminate) { + Fail(::strerror(errno)); + } + } + + + protected: + virtual int CHILD_GetReadFd() const = 0; + virtual void CHILD_SendCanFdFrame(const CanFdFrame&) = 0; + + struct ConsumeCount { + int rcv = 0; + int ok = 0; + }; + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int expected_ok_count, + std::vector* expected_reply_count) = 0; + virtual void CHILD_FlushTransmit() = 0; + + void CHILD_Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + CompletionCallback completed_callback) { + if (replies) { replies->clear(); } + CHILD_CheckReplies(replies, kFlush, 0, nullptr); + + const auto advance = t_options_.max_pipeline < 0 ? + size : t_options_.max_pipeline; + + for (size_t start = 0; start < size; start += advance) { + int expected_ok_count = 0; + for (auto& v : expected_reply_count_) { v = 0; } + + for (size_t i = start; i < (start + advance) && i < size; i++) { + expected_ok_count++; + CHILD_SendCanFdFrame(frames[i]); + if (frames[i].reply_required) { + if ((frames[i].destination + 1) > + static_cast(expected_reply_count_.size())) { + expected_reply_count_.resize(frames[i].destination + 1); + } + expected_reply_count_[frames[i].destination]++; + } + } + + CHILD_FlushTransmit(); + + CHILD_CheckReplies(replies, + kWait, + expected_ok_count, + &expected_reply_count_); + } + + Post(std::bind(completed_callback, 0)); + } + + enum ReadDelay { + kWait, + kFlush, + }; + + void CHILD_CheckReplies(std::vector* replies, + ReadDelay read_delay, + int expected_ok_count, + std::vector* expected_reply_count) { + const auto start = GetNow(); + + const auto any_reply_checker = [&]() { + if (!expected_reply_count) { return false; } + for (auto v : *expected_reply_count) { + if (v) { return true; } + } + return false; + }; + auto end_time = + start + + (read_delay == kWait ? + std::max(expected_ok_count != 0 ? t_options_.min_ok_wait_ns : 0, + any_reply_checker() ? t_options_.min_rcv_wait_ns : 0) : + 5000); + + struct pollfd fds[1] = {}; + fds[0].fd = CHILD_GetReadFd(); + fds[0].events = POLLIN; + + int ok_count = 0; + + while (true) { + const auto now = GetNow(); + fds[0].revents = 0; + + struct timespec tmo = {}; + const auto to_sleep_ns = std::max(0, end_time - now); + tmo.tv_sec = to_sleep_ns / 1000000000; + tmo.tv_nsec = to_sleep_ns % 1000000000; + + const int poll_ret = ::ppoll(&fds[0], 1, &tmo, nullptr); + if (poll_ret < 0) { + if (errno == EINTR) { + // Go back and try again. + continue; + } + FailIfErrno(true); + } + if (poll_ret == 0) { return; } + + const auto consume_count = CHILD_ConsumeData( + replies, expected_ok_count, expected_reply_count); + + ok_count += consume_count.ok; + + if (read_delay != kFlush && + !any_reply_checker() && ok_count >= expected_ok_count) { + // Once we have the expected number of CAN replies and OKs, + // return immediately. + return; + } + + if (consume_count.rcv || consume_count.ok) { + const auto finish_time = GetNow(); + end_time = finish_time + t_options_.rx_extra_wait_ns; + } + } + } + + // This is protected, because derived classes need to delete it + // before freeing any file descriptors. The public methods of the + // ThreadedEventLoop require no locking, but the shared_ptr itself + // requires either synchronization or access using the atomic std + // library methods. We'll exclusively use the atomic std library + // methods. + std::shared_ptr UNPROTECTED_event_loop_ = + std::make_shared(); + + private: + const Options t_options_; + + std::vector expected_reply_count_; +}; +} + +class Fdcanusb : public details::TimeoutTransport { + public: + struct Options : details::TimeoutTransport::Options { + Options() {} + }; + + // If @p device is empty, attempt to auto-detect a fdcanusb in the + // system. + Fdcanusb(const std::string& device_in, const Options& options = {}) + : details::TimeoutTransport(options), + options_(options) { + Open(device_in); + } + + // This constructor overload is intended for use in unit tests, + // where the file descriptors will likely be pipes. + Fdcanusb(int read_fd, int write_fd, const Options& options = {}) + : details::TimeoutTransport(options), + options_(options) { + Open(read_fd, write_fd); + } + + virtual ~Fdcanusb() { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + + if (read_fd_ == write_fd_) { + write_fd_.release(); + } + } + + static std::string DetectFdcanusb() { + // For now, we'll only do linux like systems. + { + std::ifstream inf("/dev/fdcanusb"); + if (inf.is_open()) { return "/dev/fdcanusb"; } + } + + { + glob_t glob_data = {}; + const int result = ::glob( + "/dev/serial/by-id/*fdcanusb*", 0, + nullptr, + &glob_data); + + std::string maybe_path; + + if (result == 0 && glob_data.gl_pathc > 0) { + maybe_path = glob_data.gl_pathv[0]; + } + + globfree(&glob_data); + + if (!maybe_path.empty()) { return maybe_path; } + } + + return ""; + } + + private: + void Open(const std::string& device_in) { + std::string device = device_in; + if (device.empty()) { + device = DetectFdcanusb(); + if (device.empty()) { + throw std::runtime_error("Could not detect fdcanusb"); + } + } + + const int fd = ::open(device.c_str(), O_RDWR | O_NOCTTY); + FailIfErrno(fd == -1); + +#ifndef _WIN32 + { + struct serial_struct serial; + FailIfErrno(::ioctl(fd, TIOCGSERIAL, &serial) < 0); + serial.flags |= ASYNC_LOW_LATENCY; + FailIfErrno(::ioctl(fd, TIOCSSERIAL, &serial) < 0); + + struct termios toptions; + FailIfErrno(::tcgetattr(fd, &toptions) < 0); + + // Turn off things that could munge our byte stream to the + // device. + toptions.c_lflag &= ~(ICANON | ECHO | ECHOE | ISIG); + toptions.c_oflag &= ~OPOST; + + FailIfErrno(::tcsetattr(fd, TCSANOW, &toptions) < 0); + FailIfErrno(::tcsetattr(fd, TCSAFLUSH, &toptions) < 0); + } +#else // _WIN32 + { + // Windows is likely broken for many other reasons, but if we do + // fix all the other problems, this will be necessary. + COMMTIMEOUTS new_timeouts = {MAXDWORD, 0, 0, 0, 0}; + SetCommTimeouts(fd, &new_timeouts); + } +#endif + + Open(fd, fd); + } + + void Open(int read_fd, int write_fd) { + read_fd_ = read_fd; + write_fd_ = write_fd; + } + + virtual int CHILD_GetReadFd() const override { + return read_fd_; + } + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int /* expected_ok_count */, + std::vector* expected_reply_count) override { + // Read into our line buffer. + const int to_read = sizeof(line_buffer_) - line_buffer_pos_; + const int read_ret = ::read( + read_fd_, &line_buffer_[line_buffer_pos_], to_read); + if (read_ret < 0) { + if (errno == EINTR || errno == EAGAIN) { return {}; } + FailIfErrno(true); + } + line_buffer_pos_ += read_ret; + + const auto consume_count = CHILD_ConsumeLines( + replies, expected_reply_count); + if (line_buffer_pos_ >= sizeof(line_buffer_)) { + // We overran our line buffer. For now, just drop everything + // and start from 0. + line_buffer_pos_ = 0; + } + + return consume_count; + } + + /// Return the number of CAN frames received. + ConsumeCount CHILD_ConsumeLines(std::vector* replies, + std::vector* expected_reply_count) { + const auto start_size = replies ? replies->size() : 0; + ConsumeCount result; + while (CHILD_ConsumeLine(replies, &result.ok, expected_reply_count)) {} + result.rcv = replies ? (replies->size() - start_size) : 0; + return result; + } + + bool CHILD_ConsumeLine(std::vector* replies, int* ok_count, + std::vector* expected_reply_count) { + const auto line_end = [&]() -> int { + for (size_t i = 0; i < line_buffer_pos_; i++) { + if (line_buffer_[i] == '\r' || line_buffer_[i] == '\n') { return i; } + } + return -1; + }(); + if (line_end < 0) { return false; } + + CHILD_ProcessLine(std::string(&line_buffer_[0], line_end), replies, + ok_count, expected_reply_count); + + std::memmove(&line_buffer_[0], &line_buffer_[line_end + 1], + line_buffer_pos_ - line_end - 1); + line_buffer_pos_ -= (line_end + 1); + + return true; + } + + void CHILD_ProcessLine(const std::string& line, + std::vector* replies, + int* ok_count, + std::vector* expected_reply_count) { + if (line == "OK") { + (*ok_count)++; + return; + } + + // The only line we actually do anything with is a "rcv" line. + detail::Tokenizer tokenizer(line, " "); + const auto start = tokenizer.next(); + if (start != "rcv") { return; } + const auto address = tokenizer.next(); + const auto data = tokenizer.next(); + + if (address.empty() || data.empty()) { return; } + + CanFdFrame this_frame; + this_frame.arbitration_id = std::stoul(address, nullptr, 16); + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.size = ParseCanData(data, this_frame.data); + + while (true) { + const auto maybe_flags = tokenizer.next(); + if (maybe_flags.empty()) { break; } + if (maybe_flags.size() != 1) { continue; } + for (const char c : maybe_flags) { + if (c == 'b') { this_frame.brs = CanFdFrame::kForceOff; } + if (c == 'B') { this_frame.brs = CanFdFrame::kForceOn; } + if (c == 'f') { this_frame.fdcan_frame = CanFdFrame::kForceOff; } + if (c == 'F') { this_frame.fdcan_frame = CanFdFrame::kForceOn; } + } + } + + if (expected_reply_count) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } + + if (replies) { + replies->emplace_back(std::move(this_frame)); + } + } + + struct Printer { + Printer(char* buf, size_t capacity) : buf_(buf), capacity_(capacity) {}; + + const char* buf() { return buf_; } + size_t size() const { return pos_; } + size_t remaining() const { return capacity_ - pos_ - 1; } + + void operator()(const char* fmt, ...) { + va_list ap; + va_start(ap, fmt); + auto n = ::vsnprintf(&buf_[pos_], remaining(), fmt, ap); + va_end(ap); + if (n < 0) { ::abort(); } + pos_ += n; + }; + + char* const buf_; + size_t pos_ = 0; + const size_t capacity_; + }; + + virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { + char buf[256] = {}; + + Printer p(buf, sizeof(buf)); + + p("can send %04x ", frame.arbitration_id); + + const auto dlc = RoundUpDlc(frame.size); + for (size_t i = 0; i < frame.size; i++) { + p("%02x", static_cast(frame.data[i])); + } + for (size_t i = frame.size; i < dlc; i++) { + p("50"); + } + + if (options_.disable_brs || frame.brs == CanFdFrame::kForceOff) { + p(" b"); + } else if (frame.brs == CanFdFrame::kForceOn) { + p(" B"); + } + if (frame.fdcan_frame == CanFdFrame::kForceOff) { + p(" f"); + } else if (frame.fdcan_frame == CanFdFrame::kForceOn) { + p(" F"); + } + p("\n"); + + if (p.size() > (sizeof(tx_buffer_) - tx_buffer_size_)) { + CHILD_FlushTransmit(); + } + + std::memcpy(&tx_buffer_[tx_buffer_size_], &buf[0], p.size()); + tx_buffer_size_ += p.size(); + } + + virtual void CHILD_FlushTransmit() override { + for (size_t n = 0; n < tx_buffer_size_; ) { + int ret = ::write(write_fd_, &tx_buffer_[n], tx_buffer_size_ - n); + if (ret < 0) { + if (errno == EINTR || errno == EAGAIN) { continue; } + + FailIfErrno(true); + } else { + n += ret; + } + } + tx_buffer_size_ = 0; + } + + static size_t RoundUpDlc(size_t size) { + if (size <= 8) { return size; } + if (size <= 12) { return 12; } + if (size <= 16) { return 16; } + if (size <= 20) { return 20; } + if (size <= 24) { return 24; } + if (size <= 32) { return 32; } + if (size <= 48) { return 48; } + if (size <= 64) { return 64; } + return size; + } + + static int ParseHexNybble(char c) { + if (c >= '0' && c <= '9') { return c - '0'; } + if (c >= 'a' && c <= 'f') { return c - 'a' + 10; } + if (c >= 'A' && c <= 'F') { return c - 'A' + 10; } + return -1; + } + + static int ParseHexByte(const char* value) { + const int high = ParseHexNybble(value[0]); + if (high < 0) { return high; } + const int low = ParseHexNybble(value[1]); + if (low < 0) { return low; } + return (high << 4) | low; + } + + static int ParseCanData(const std::string& data, uint8_t* out) { + size_t to_read = std::min(64 * 2, data.size()); + for (size_t i = 0; i < to_read; i+= 2) { + out[i / 2] = ParseHexByte(&data[i]); + } + return to_read / 2; + } + + // This is set in the parent, then used in the child. + const Options options_; + + // We have these scoped file descriptors first in our member list, + // so they will only be closed after the threaded event loop has + // been destroyed during destruction. + details::FileDescriptor read_fd_; + details::FileDescriptor write_fd_; + + // The following variables are only used in the child. + char line_buffer_[4096] = {}; + size_t line_buffer_pos_ = 0; + + char tx_buffer_[4096] = {}; + size_t tx_buffer_size_ = 0; +}; + + +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN +class Socketcan : public details::TimeoutTransport { + public: + struct Options : details::TimeoutTransport::Options { + std::string ifname = "can0"; + + Options() {} + }; + + Socketcan(const Options& options) + : details::TimeoutTransport(options), + options_(options) { + socket_ = Open(options_.ifname); + } + + virtual ~Socketcan() { + std::atomic_store(&UNPROTECTED_event_loop_, {}); + } + + private: + static void SetNonblock(int fd) { + int flags = ::fcntl(fd, F_GETFL, 0); + FailIf(flags < 0, "error getting flags"); + flags |= O_NONBLOCK; + FailIf(::fcntl(fd, F_SETFL, flags), "error setting flags"); + } + + static int Open(const std::string& ifname) { + const int fd = ::socket(PF_CAN, SOCK_RAW, CAN_RAW); + FailIf(fd < 0, "error opening CAN socket"); + + SetNonblock(fd); + + struct ifreq ifr = {}; + std::strncpy(&ifr.ifr_name[0], ifname.c_str(), + sizeof(ifr.ifr_name) - 1); + FailIf(::ioctl(fd, SIOCGIFINDEX, &ifr) < 0, + "could not find CAN: " + ifname); + + const int enable_canfd = 1; + FailIf(::setsockopt(fd, SOL_CAN_RAW, CAN_RAW_FD_FRAMES, + &enable_canfd, sizeof(enable_canfd)) != 0, + "could not set CAN-FD mode"); + + struct sockaddr_can addr = {}; + addr.can_family = AF_CAN; + addr.can_ifindex = ifr.ifr_ifindex; + FailIf(::bind(fd, + reinterpret_cast(&addr), + sizeof(addr)) < 0, + "could not bind to CAN if"); + + return fd; + } + + virtual int CHILD_GetReadFd() const override { + return socket_; + } + + virtual void CHILD_SendCanFdFrame(const CanFdFrame& frame) override { + struct canfd_frame send_frame = {}; + send_frame.can_id = frame.arbitration_id; + if (send_frame.can_id >= 0x7ff) { + // Set the frame format flag if we need an extended ID. + send_frame.can_id |= (1 << 31); + } + send_frame.len = frame.size; + std::memcpy(send_frame.data, frame.data, frame.size); + + using F = CanFdFrame; + + send_frame.flags = + ((frame.fdcan_frame == F::kDefault || + frame.fdcan_frame == F::kForceOn) ? CANFD_FDF : 0) | + (((frame.brs == F::kDefault && !options_.disable_brs) || + frame.brs == F::kForceOn) ? CANFD_BRS : 0); + + FailIf(::write(socket_, &send_frame, sizeof(send_frame)) < 0, + "error writing CAN"); + } + + virtual ConsumeCount CHILD_ConsumeData( + std::vector* replies, + int /* expected_ok_count */, + std::vector* expected_reply_count) override { + struct canfd_frame recv_frame = {}; + FailIf(::read(socket_, &recv_frame, sizeof(recv_frame)) < 0, + "error reading CAN frame"); + + CanFdFrame this_frame; + this_frame.arbitration_id = recv_frame.can_id & 0x1fffffff; + this_frame.destination = this_frame.arbitration_id & 0x7f; + this_frame.source = (this_frame.arbitration_id >> 8) & 0x7f; + this_frame.can_prefix = (this_frame.arbitration_id >> 16); + + this_frame.brs = (recv_frame.flags & CANFD_BRS) ? + CanFdFrame::kForceOn : CanFdFrame::kForceOff; + this_frame.fdcan_frame = (recv_frame.flags & CANFD_FDF) ? + CanFdFrame::kForceOn : CanFdFrame::kForceOff; + + std::memcpy(this_frame.data, recv_frame.data, recv_frame.len); + this_frame.size = recv_frame.len; + + if (expected_reply_count) { + if (this_frame.source < + static_cast(expected_reply_count->size())) { + (*expected_reply_count)[this_frame.source] = std::max( + (*expected_reply_count)[this_frame.source] - 1, 0); + } + } + + if (replies) { + replies->emplace_back(std::move(this_frame)); + } + + ConsumeCount result; + result.ok = 1; + result.rcv = 1; + return result; + } + + virtual void CHILD_FlushTransmit() override {} + + const Options options_; + details::FileDescriptor socket_; +}; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN + +/// A factory which can create transports given an optional set of +/// commandline arguments. +class TransportFactory { + public: + virtual ~TransportFactory() {} + + virtual int priority() = 0; + virtual std::string name() = 0; + using TransportArgPair = std::pair, + std::vector>; + virtual TransportArgPair make(const std::vector&) = 0; + + struct Argument { + std::string name; + int nargs = 1; + std::string help; + + bool operator<(const Argument& rhs) const { + if (name < rhs.name) { return true; } + if (name > rhs.name) { return false; } + return help < rhs.help; + } + + Argument(const std::string& name_in, + int nargs_in, + const std::string& help_in) + : name(name_in), + nargs(nargs_in), + help(help_in) {} + }; + + virtual std::vector cmdline_arguments() = 0; + + virtual bool is_args_set(const std::vector&) = 0; +}; + +class FdcanusbFactory : public TransportFactory { + public: + virtual ~FdcanusbFactory() {} + + virtual int priority() override { return 10; } + virtual std::string name() override { return "fdcanusb"; } + + virtual TransportArgPair make(const std::vector& args_in) override { + auto args = args_in; + + Fdcanusb::Options options; + std::string device; + + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) { + options.disable_brs = true; + args.erase(it); + } + } + + { + auto it = std::find(args.begin(), args.end(), "--fdcanusb"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + device = *(it + 1); + args.erase(it, it + 2); + } else { + throw std::runtime_error("--fdcanusb requires a path"); + } + } + } + + auto result = std::make_shared(device, options); + return TransportArgPair(result, args); + } + + virtual std::vector cmdline_arguments() override { + return { + { "--fdcanusb", 1, "path to fdcanusb device" }, + { "--can-disable-brs", 0, "do not set BRS" }, + }; + } + + virtual bool is_args_set(const std::vector& args) override { + for (const auto& arg : args) { + if (arg == "--fdcanusb") { return true; } + } + return false; + } +}; + +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN +class SocketcanFactory : public TransportFactory { + public: + virtual ~SocketcanFactory() {} + + virtual int priority() override { return 11; } + virtual std::string name() override { return "socketcan"; } + + virtual TransportArgPair make(const std::vector& args_in) override { + auto args = args_in; + + Socketcan::Options options; + std::string device; + + { + auto it = std::find(args.begin(), args.end(), "--can-disable-brs"); + if (it != args.end()) { + options.disable_brs = true; + args.erase(it); + } + } + + { + auto it = std::find(args.begin(), args.end(), "--socketcan-iface"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + options.ifname = *(it + 1); + args.erase(it, it + 2); + } else { + throw std::runtime_error("--socketcan-iface requires an interface name"); + } + } + } + + auto result = std::make_shared(options); + return TransportArgPair(result, args); + } + + virtual std::vector cmdline_arguments() override { + return { + { "--socketcan-iface", 1, "socketcan iface name" }, + { "--can-disable-brs", 0, "do not set BRS" }, + }; + } + + virtual bool is_args_set(const std::vector& args) override { + for (const auto& arg : args) { + if (arg == "--socketcan-iface") { return true; } + } + return false; + } +}; +#endif // MJBOTS_MOTEUS_ENABLE_SOCKETCAN + +class TransportRegistry { + public: + template + void Register() { + items_.push_back(std::make_shared()); + } + + static TransportRegistry& singleton() { + static TransportRegistry reg; + return reg; + } + + std::vector cmdline_arguments() const { + std::vector result; + std::set uniqifier; + + result.push_back(TransportFactory::Argument( + "--force-transport", 1, + "force the given transport type to be used")); + uniqifier.insert(result.back()); + + for (const auto& item : items_) { + const auto item_args = item->cmdline_arguments(); + for (const auto& arg : item_args) { + if (uniqifier.count(arg) == 0) { + result.push_back(arg); + uniqifier.insert(arg); + } + } + } + + return result; + } + + TransportFactory::TransportArgPair make(const std::vector& args_in) const { + auto args = args_in; + auto to_try = items_; + + std::sort(to_try.begin(), to_try.end(), + [](const std::shared_ptr& lhs, + const std::shared_ptr& rhs) { + return lhs->priority() < rhs->priority(); + }); + + // Is the transport forced? + const auto it = std::find(args.begin(), args.end(), "--force-transport"); + if (it != args.end()) { + if ((it + 1) != args.end()) { + to_try = {}; + const auto name_to_find = *(it + 1); + for (auto item : items_) { + if (item->name() == name_to_find) { to_try.push_back(item); } + } + args.erase(it, it + 2); + } else { + throw std::runtime_error("--force-transport requires an argument"); + } + } else { + std::vector> options_set; + for (auto item : items_) { + if (item->is_args_set(args)) { + options_set.push_back(item); + } + } + + if (!options_set.empty()) { to_try = options_set; } + } + + std::string errors; + for (auto factory : to_try) { + try { + auto maybe_result = factory->make(args); + if (maybe_result.first) { + return maybe_result; + } + } catch (std::runtime_error& re) { + if (!errors.empty()) { errors += ", "; } + errors += factory->name() + ": " + re.what(); + } + } + throw std::runtime_error("Unable to find a default transport: " + errors); + } + + private: + TransportRegistry() { + Register(); +#ifdef MJBOTS_MOTEUS_ENABLE_SOCKETCAN + Register(); +#endif + } + + std::vector> items_; +}; + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD new file mode 100644 index 00000000..66f01b98 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD @@ -0,0 +1,75 @@ +# -*- python -*- + +# Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +# +# Licensed under the Apache License, Version 2.0 (the "License"); +# you may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +package(default_visibility = ["//visibility:public"]) + +filegroup( + name = "src", + srcs = [ + "pi3hat.h", + "pi3hat.cc", + "pi3hat_moteus_transport.h", + "realtime.h", + ], +) + +cc_library( + name = "headers", + hdrs = ["pi3hat.h"], + include_prefix = "mjbots/pi3hat", +) + +cc_library( + name = "libpi3hat", + hdrs = ["pi3hat.h"], + srcs = ["pi3hat.cc"], + deps = [":headers", "@raspberrypi-firmware//:bcm_host"], +) + +cc_binary( + name = "pi3hat_tool", + srcs = ["pi3hat_tool.cc"], + deps = [ + ":libpi3hat", + "@org_llvm_libcxx//:libcxx", + ], +) + +filegroup( + name = "pi3hat", + srcs = [ + ":libpi3hat", + ":pi3hat_tool", + ], +) + +cc_library( + name = "realtime", + hdrs = ["realtime.h"], +) + +cc_library( + name = "transport", + hdrs = [ + "pi3hat_moteus_transport.h", + ], + deps = [ + ":realtime", + ":libpi3hat", + "@moteus//lib/cpp/mjbots/moteus:moteus", + ], + includes = ["."], +) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc new file mode 100644 index 00000000..91fc5668 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc @@ -0,0 +1,1703 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +// We purposefully don't use the full path here so that this file can +// be compiled in a wide range of build configurations. +#include "pi3hat.h" + +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include + +char g_data_block[4096] = {}; + +void copy_data(void* ptr) { + ::memcpy(g_data_block, ptr, sizeof(g_data_block)); +} + +namespace mjbots { +namespace pi3hat { + +namespace { +/////////////////////////////////////////////// +/// Random utility functions + +size_t RoundUpDlc(size_t value) { + if (value == 0) { return 0; } + if (value == 1) { return 1; } + if (value == 2) { return 2; } + if (value == 3) { return 3; } + if (value == 4) { return 4; } + if (value == 5) { return 5; } + if (value == 6) { return 6; } + if (value == 7) { return 7; } + if (value == 8) { return 8; } + if (value <= 12) { return 12; } + if (value <= 16) { return 16; } + if (value <= 20) { return 20; } + if (value <= 24) { return 24; } + if (value <= 32) { return 32; } + if (value <= 48) { return 48; } + if (value <= 64) { return 64; } + return 0; +} + +char g_format_buf[2048] = {}; + +const char* Format(const char* fmt, ...) __attribute__((format(printf, 1, 2))); + +const char* Format(const char* fmt, ...) { + va_list args1; + va_start(args1, fmt); + ::vsnprintf(g_format_buf, sizeof(g_format_buf) - 1, fmt, args1); + va_end(args1); + return &g_format_buf[0]; +} + +template +void ThrowIf(bool value, ErrorGenerator error_generator) { + if (!value) { return; } + throw Error(error_generator()); +} + +char g_error_buf[2048] = {}; + +void ThrowIfErrno(bool value, const std::string& message = "") { + if (!value) { return; } + + // Just to be on the safe side. + g_error_buf[0] = 0; + const auto result = strerror_r(errno, g_error_buf, sizeof(g_error_buf)); + // For portability. + (void)result; + + throw Error(message + " : " + std::string(g_error_buf)); +} + +int64_t GetNow() { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) * 1000000000ll + + static_cast(ts.tv_nsec); +} + +void BusyWaitUs(int64_t us) { + // We wait to ensure that setup and hold times are properly + // enforced. Allowing data stores and loads to be re-ordered around + // the wait would defeat their purpose. Thus, use barriers to force + // a complete synchronization event on either side of our waits. +#ifdef __ARM_ARCH_ISA_A64 + asm volatile("dsb sy"); +#elif __ARM_ARCH_7A__ + asm volatile("dsb"); +#elif __ARM_ARCH_8A__ + asm volatile("dsb"); +#elif __ARM_ARCH_6__ +# error "RPI 1/2 are unsupported. Perhaps you need '-march=native -mcpu=native -mtune=native'?" +#else +# error "Unknown architecture" +#endif + + const auto start = GetNow(); + const auto end = start + us * 1000; + while (GetNow() <= end); + +#ifdef __ARM_ARCH_ISA_A64 + asm volatile("dsb sy"); +#elif __ARM_ARCH_7A__ + asm volatile("dsb"); +#elif __ARM_ARCH_8A__ + asm volatile("dsb"); +#elif __ARM_ARCH_6__ +# error "RPI 1/2 are unsupported. Perhaps you need '-march=native -mcpu=native -mtune=native'?" +#else +# error "Unknown architecture" +#endif +} + +std::string ReadContents(const std::string& filename) { + std::ifstream inf(filename); + std::ostringstream ostr; + ostr << inf.rdbuf(); + return ostr.str(); +} + +bool StartsWith(const std::string& value, const std::string& maybe_prefix) { + return value.substr(0, maybe_prefix.size()) == maybe_prefix; +} + +/////////////////////////////////////////////// +/// Random utility classes + +/// Manages ownership of a system file descriptor. +class SystemFd { + public: + SystemFd() : fd_(-1) {} + SystemFd(int fd) : fd_(fd) {} + + SystemFd(SystemFd&& rhs) { + fd_ = rhs.fd_; + rhs.fd_ = -1; + } + + SystemFd& operator=(SystemFd&& rhs) { + fd_ = rhs.fd_; + rhs.fd_ = -1; + return *this; + } + + ~SystemFd() { + if (fd_ >= 0) { + ::close(fd_); + } + } + + SystemFd(const SystemFd&) = delete; + SystemFd& operator=(const SystemFd&) = delete; + + operator int() { return fd_; } + + private: + int fd_ = -1; +}; + +/// Manages ownership of an mmap'ed region of a given file descriptor. +class SystemMmap { + public: + SystemMmap() {} + + SystemMmap(int fd, size_t size, uint64_t offset) { + ptr_ = ::mmap(0, size, PROT_READ | PROT_WRITE, MAP_SHARED, fd, offset); + size_ = size; + ThrowIfErrno(ptr_ == MAP_FAILED); + } + + ~SystemMmap() { + if (ptr_ != MAP_FAILED) { + ThrowIfErrno(::munmap(ptr_, size_) < 0); + } + } + + SystemMmap(SystemMmap&& rhs) { + std::swap(ptr_, rhs.ptr_); + std::swap(size_, rhs.size_); + } + + SystemMmap& operator=(SystemMmap&& rhs) { + std::swap(ptr_, rhs.ptr_); + std::swap(size_, rhs.size_); + return *this; + } + + SystemMmap(const SystemMmap&) = delete; + SystemMmap& operator=(const SystemMmap&) = delete; + + void* ptr() { return ptr_; } + + // Since this is intended to be whatever, we just allow it to be + // converted to any old pointer at will without extra hoops. + template + operator T*() { return ptr_; } + + template + operator const T*() const { return ptr_; } + + private: + void* ptr_ = MAP_FAILED; + size_t size_ = 0; +}; + + +/////////////////////////////////////////////// +/// Drivers for the Raspberry Pi hardware + +class Rpi3Gpio { + public: + static constexpr uint32_t GPIO_BASE = 0x00200000; + + // static constexpr uint32_t INPUT = 0; + static constexpr uint32_t OUTPUT = 1; + static constexpr uint32_t ALT_0 = 4; + // static constexpr uint32_t ALT_1 = 5; + // static constexpr uint32_t ALT_2 = 6; + // static constexpr uint32_t ALT_3 = 7; + static constexpr uint32_t ALT_4 = 3; + // static constexpr uint32_t ALT_5 = 2; + + Rpi3Gpio(int dev_mem_fd) + : mmap_(dev_mem_fd, 4096, bcm_host_get_peripheral_address() + GPIO_BASE), + gpio_(reinterpret_cast(mmap_.ptr())) {} + + void SetGpioMode(uint32_t gpio, uint32_t function) { + uint32_t reg_offset = gpio / 10; + uint32_t bit = (gpio % 10) * 3; + const auto value = gpio_[reg_offset]; + gpio_[reg_offset] = (value & ~(0x7 << bit)) | ((function & 0x7) << bit); + } + + void SetGpioOutput(uint32_t gpio, bool value) { + if (value) { + const uint32_t reg_offset = gpio / 32 + 7; + gpio_[reg_offset] = 1 << (gpio % 32); + } else { + const uint32_t reg_offset = gpio / 32 + 10; + gpio_[reg_offset] = 1 << (gpio % 32); + } + } + + volatile uint32_t& operator[](int index) { return gpio_[index]; } + const volatile uint32_t& operator[](int index) const { return gpio_[index]; } + + class ActiveLow { + public: + ActiveLow(Rpi3Gpio* parent, uint32_t gpio) : parent_(parent), gpio_(gpio) { + parent_->SetGpioOutput(gpio_, false); + } + + ~ActiveLow() { + parent_->SetGpioOutput(gpio_, true); + } + + private: + Rpi3Gpio* const parent_; + const uint32_t gpio_; + }; + + private: + SystemMmap mmap_; + volatile uint32_t* const gpio_; +}; + + +constexpr uint32_t kSpi0CS0 = 8; +constexpr uint32_t kSpi0CS1 = 7; +constexpr uint32_t kSpi0CS[] = {kSpi0CS0, kSpi0CS1}; + +constexpr uint32_t SPI_BASE = 0x204000; +constexpr uint32_t SPI_CS_TA = 1 << 7; +constexpr uint32_t SPI_CS_DONE = 1 << 16; +constexpr uint32_t SPI_CS_RXD = 1 << 17; +constexpr uint32_t SPI_CS_TXD = 1 << 18; + + +/// This class interacts with the SPI0 device on a raspberry pi using +/// the BCM2835/6/7's registers directly. The kernel driver must not +/// be active (it can be loaded, as long as you're not using it), and +/// this must be run as root or otherwise have access to /dev/mem. +class PrimarySpi { + public: + struct Options { + int speed_hz = 10000000; + // We actually only need hold times of around 3us. However, the + // linux aarch64 kernel sometimes returns up to 8us of difference + // in consecutive calls to clock_gettime when in a tight busy loop + // (and <1 us of wall clock time has actually passed as measured + // by an oscilloscope). This doesn't seem to be a problem on the + // armv7l kernel. + int cs_hold_us = 3; + int address_hold_us = 3; + + Options() {} + }; + + PrimarySpi(const Options& options = Options()) { + fd_ = ::open("/dev/mem", O_RDWR | O_SYNC); + ThrowIfErrno(fd_ < 0, "pi3hat: could not open /dev/mem"); + + spi_mmap_ = SystemMmap( + fd_, 4096, bcm_host_get_peripheral_address() + SPI_BASE); + spi_ = reinterpret_cast( + static_cast(spi_mmap_.ptr())); + + gpio_.reset(new Rpi3Gpio(fd_)); + + gpio_->SetGpioOutput(kSpi0CS0, true); + gpio_->SetGpioOutput(kSpi0CS1, true); + + gpio_->SetGpioMode(kSpi0CS0, Rpi3Gpio::OUTPUT); // We'll do CS in SW + gpio_->SetGpioMode(kSpi0CS1, Rpi3Gpio::OUTPUT); + gpio_->SetGpioMode(9, Rpi3Gpio::ALT_0); + gpio_->SetGpioMode(10, Rpi3Gpio::ALT_0); + gpio_->SetGpioMode(11, Rpi3Gpio::ALT_0); + + spi_->cs = ( + 0 + | (0 << 25) // LEn_LONG + | (0 << 24) // DMA_LEN + | (0 << 23) // CSPOL2 + | (0 << 22) // CSPOL1 + | (0 << 21) // CSPOL0 + | (0 << 13) // LEN + | (0 << 12) // REN + | (0 << 11) // ADCS + | (0 << 10) // INTR + | (0 << 9) // INTD + | (0 << 8) // DMAEN + | (0 << 7) // TA + | (0 << 6) // CSPOL + | (0 << 4) // CLEAR + | (0 << 3) // CPOL + | (0 << 2) // CPHA + | (0 << 0) // CS + ); + + // Configure the SPI peripheral. + const int clkdiv = + std::max(0, std::min(65535, 400000000 / options.speed_hz)); + spi_->clk = clkdiv; + } + + ~PrimarySpi() {} + + PrimarySpi(const PrimarySpi&) = delete; + PrimarySpi& operator=(const PrimarySpi&) = delete; + + Rpi3Gpio* gpio() { + return gpio_.get(); + } + + void Write(int cs, int address, const char* data, size_t size) { + BusyWaitUs(options_.cs_hold_us); + Rpi3Gpio::ActiveLow cs_holder(gpio_.get(), kSpi0CS[cs]); + BusyWaitUs(options_.cs_hold_us); + + spi_->cs = (spi_->cs | (SPI_CS_TA | (3 << 4))); // CLEAR + + spi_->fifo = address & 0xff; + + // We are done when we have received one byte back. + while ((spi_->cs & SPI_CS_RXD) == 0); + (void) spi_->fifo; + + if (size != 0) { + // Wait our address hold time. + BusyWaitUs(options_.address_hold_us); + + size_t offset = 0; + while (offset < size) { + while ((spi_->cs & SPI_CS_TXD) == 0); + spi_->fifo = data[offset]; + offset++; + } + + // Wait until we are no longer busy. + while ((spi_->cs & SPI_CS_DONE) == 0) { + if (spi_->cs & SPI_CS_RXD) { + (void) spi_->fifo; + } + } + } + + spi_->cs = (spi_->cs & (~SPI_CS_TA)); + } + + void Read(int cs, int address, char* data, size_t size) { + BusyWaitUs(options_.cs_hold_us); + Rpi3Gpio::ActiveLow cs_holder(gpio_.get(), kSpi0CS[cs]); + BusyWaitUs(options_.cs_hold_us); + + spi_->cs = (spi_->cs | (SPI_CS_TA | (3 << 4))); // CLEAR + + spi_->fifo = (address & 0x00ff); + + // We are done when we have received one byte back. + while ((spi_->cs & SPI_CS_RXD) == 0); + (void) spi_->fifo; + + if (size != 0) { + // Wait our address hold time. + BusyWaitUs(options_.address_hold_us); + + // Now we write out dummy values, reading values in. + std::size_t remaining_read = size; + std::size_t remaining_write = remaining_read; + char* ptr = data; + while (remaining_read) { + // Make sure we don't write more than we have read spots remaining + // so that we can never overflow the RX fifo. + const bool can_write = (remaining_read - remaining_write) < 16; + if (can_write && + remaining_write && (spi_->cs & SPI_CS_TXD) != 0) { + spi_->fifo = 0x00; + remaining_write--; + } + + if (remaining_read && (spi_->cs & SPI_CS_RXD) != 0) { + *ptr = spi_->fifo & 0xff; + ptr++; + remaining_read--; + } + } + } + + spi_->cs = (spi_->cs & (~SPI_CS_TA)); + } + + private: + // This is the memory layout of the SPI peripheral. + struct Bcm2835Spi { + uint32_t cs; + uint32_t fifo; + uint32_t clk; + uint32_t dlen; + uint32_t ltoh; + uint32_t dc; + }; + + const Options options_; + SystemFd fd_; + SystemMmap spi_mmap_; + volatile Bcm2835Spi* spi_ = nullptr; + + std::unique_ptr gpio_; +}; + +constexpr uint32_t AUX_BASE = 0x00215000; +constexpr uint32_t kSpi1CS0 = 18; +constexpr uint32_t kSpi1CS1 = 17; +constexpr uint32_t kSpi1CS2 = 16; +constexpr uint32_t kSpi1CS[] = { + kSpi1CS0, + kSpi1CS1, + kSpi1CS2, +}; + +constexpr int AUXSPI_STAT_TX_FULL = 1 << 10; +constexpr int AUXSPI_STAT_TX_EMPTY = 1 << 9; +constexpr int AUXSPI_STAT_RX_EMPTY = 1 << 7; +constexpr int AUXSPI_STAT_BUSY = 1 << 6; + +/// This class interacts with the AUX SPI1 device on a raspberry pi +/// using the BCM2835/6/7's registers directly. The kernel driver +/// must not be active, and this must be run as root or otherwise have +/// access to /dev/mem. +class AuxSpi { + public: + struct Options { + int speed_hz = 10000000; + // We actually only need hold times of around 3us, these are + // larger for the same reasons as in PrimarySpi. + int cs_hold_us = 3; + int address_hold_us = 3; + + Options() {} + }; + + static constexpr int kPack = 3; + + AuxSpi(const Options& options = Options()) { + fd_ = ::open("/dev/mem", O_RDWR | O_SYNC); + ThrowIfErrno(fd_ < 0, "rpi3_aux_spi: could not open /dev/mem"); + + spi_mmap_ = SystemMmap( + fd_, 4096, bcm_host_get_peripheral_address() + AUX_BASE); + auxenb_ = reinterpret_cast( + static_cast(spi_mmap_.ptr()) + 0x04); + spi_ = reinterpret_cast( + static_cast(spi_mmap_.ptr()) + 0x80); + + gpio_.reset(new Rpi3Gpio(fd_)); + + gpio_->SetGpioOutput(kSpi1CS0, true); + gpio_->SetGpioOutput(kSpi1CS1, true); + gpio_->SetGpioOutput(kSpi1CS2, true); + + gpio_->SetGpioMode(kSpi1CS0, Rpi3Gpio::OUTPUT); // We'll do CS in SW + gpio_->SetGpioMode(kSpi1CS1, Rpi3Gpio::OUTPUT); + gpio_->SetGpioMode(kSpi1CS2, Rpi3Gpio::OUTPUT); + gpio_->SetGpioMode(19, Rpi3Gpio::ALT_4); + gpio_->SetGpioMode(20, Rpi3Gpio::ALT_4); + gpio_->SetGpioMode(21, Rpi3Gpio::ALT_4); + + // Start by disabling it to try and get to a known good state. + *auxenb_ = (*auxenb_ & (~0x02)); + + BusyWaitUs(10); + + // Enable the SPI peripheral. + *auxenb_ = (*auxenb_ | 0x02); // SPI1 enable + + spi_->cntl1 = 0; + spi_->cntl0 = (1 << 9); // clear fifos + + // Configure the SPI peripheral. + const int clkdiv = + std::max(0, std::min(4095, 250000000 / 2 / options.speed_hz - 1)); + spi_->cntl0 = ( + 0 + | (clkdiv << 20) + | (0 << 17) // chip select defaults + | (0 << 16) // post-input mode + | (0 << 15) // variable CS + | (1 << 14) // variable width + | (2 << 12) // DOUT hold time + | (1 << 11) // enable + | (1 << 10) // in rising? + | (0 << 9) // clear fifos + | (0 << 8) // out rising + | (0 << 7) // invert SPI CLK + | (1 << 6) // MSB first + | (0 << 0) // shift length + ); + + spi_->cntl1 = ( + 0 + | (0 << 8) // CS high time + | (0 << 7) // tx empty IRQ + | (0 << 6) // done IRQ + | (1 << 1) // shift in MS first + | (0 << 0) // keep input + ); + } + + ~AuxSpi() {} + + AuxSpi(const AuxSpi&) = delete; + AuxSpi& operator=(const AuxSpi&) = delete; + + void Write(int cs, int address, const char* data, size_t size) { + BusyWaitUs(options_.cs_hold_us); + Rpi3Gpio::ActiveLow cs_holder(gpio_.get(), kSpi1CS[cs]); + BusyWaitUs(options_.cs_hold_us); + + const uint32_t value = + 0 + | (0 << 29) // CS + | (8 << 24) // data width + | ((address & 0xff) << 16) // data + ; + + if (size != 0) { + spi_->txhold = value; + } else { + spi_->io = value; + } + + while ((spi_->stat & AUXSPI_STAT_TX_EMPTY) == 0); + + if (size == 0) { return; } + + // Wait our address hold time. + BusyWaitUs(options_.address_hold_us); + + size_t offset = 0; + while (offset < size) { + while (spi_->stat & AUXSPI_STAT_TX_FULL); + + const size_t remaining = size - offset; + const size_t to_write = std::min(remaining, kPack); + + // The Auxiliary SPI controller inserts a small dead time + // between each FIFO entry, even if the FIFO is all full up. + // Thus, we work to minimize this by using all 3 available bytes + // of each FIFO entry when possible. + const uint32_t data_value = + 0 + | (0 << 29) // CS + | ((to_write * 8) << 24) // data width + | [&]() { + if (to_write == 1) { + return data[offset] << 16; + } else if (to_write == 2) { + return (data[offset] << 16) | (data[offset + 1] << 8); + } else if (to_write == 3) { + return (data[offset] << 16) | (data[offset + 1] << 8) | data[offset + 2]; + } + // We should never get here. + return 0; + }(); + + if (offset + to_write == size) { + spi_->io = data_value; + } else { + spi_->txhold = data_value; + } + offset += to_write; + } + + // Discard anything in the RX fifo. + while ((spi_->stat & AUXSPI_STAT_RX_EMPTY) == 0) { + (void) spi_->io; + } + + // Wait until we are no longer busy. + while (spi_->stat & AUXSPI_STAT_BUSY); + } + + void Read(int cs, int address, char* data, size_t size) { + BusyWaitUs(options_.cs_hold_us); + Rpi3Gpio::ActiveLow cs_holder(gpio_.get(), kSpi1CS[cs]); + BusyWaitUs(options_.cs_hold_us); + + const uint32_t value = 0 + | (0 << 29) // CS + | (8 << 24) // data width + | ((address & 0xff) << 16) // data + ; + if (size != 0) { + spi_->txhold = value; + } else { + spi_->io = value; + } + + while (true) { + const auto stat = spi_->stat; + if ((stat & AUXSPI_STAT_BUSY) == 0 && + (stat & AUXSPI_STAT_TX_EMPTY) != 0) { + break; + } + } + + if (size == 0) { return; } + + // Wait our address hold time. + BusyWaitUs(options_.address_hold_us); + + // Discard the rx fifo. + while ((spi_->stat & AUXSPI_STAT_RX_EMPTY) == 0) { + (void) spi_->io; + } + + // Now we write out dummy values, reading values in. + std::size_t remaining_read = size; + std::size_t remaining_write = remaining_read; + char* ptr = data; + while (remaining_read) { + // Make sure we don't write more than we have read spots remaining + // so that we can never overflow the RX fifo. + const bool can_write = (remaining_read - remaining_write) < (3 * kPack); + const uint32_t cur_stat = spi_->stat; + const bool tx_full = (cur_stat & AUXSPI_STAT_TX_FULL) != 0; + + if (can_write && remaining_write && !tx_full) { + const size_t to_read = std::min(remaining_write, kPack); + const uint32_t to_write = + 0 + | (0 << 29) // CS + | ((8 * to_read) << 24) // data width + | (0) // data + ; + remaining_write -= to_read; + if (remaining_write == 0) { + spi_->io = to_write; + } else { + spi_->txhold = to_write; + } + } + + if (remaining_read && (spi_->stat & AUXSPI_STAT_RX_EMPTY) == 0) { + const uint32_t value = spi_->io; + + const size_t byte_count = std::min(remaining_read, kPack); + switch (byte_count) { + case 3: + *ptr++ = (value >> 16) & 0xff; + // fall through + case 2: + *ptr++ = (value >> 8) & 0xff; + // fall through + case 1: + *ptr++ = (value >> 0) & 0xff; + } + remaining_read -= byte_count; + } + } + } + + private: + // This is the memory layout of the SPI peripheral. + struct Bcm2835AuxSpi { + uint32_t cntl0; + uint32_t cntl1; + uint32_t stat; + uint32_t peek; + uint32_t ign1[4]; + uint32_t io; + uint32_t ign2; + uint32_t ign3; + uint32_t ign4; + uint32_t txhold; + }; + + const Options options_; + SystemFd fd_; + SystemMmap spi_mmap_; + volatile uint32_t* auxenb_ = nullptr; + volatile Bcm2835AuxSpi* spi_ = nullptr; + + std::unique_ptr gpio_; +}; + +/////////////////////////////////////////////// +/// Structures exchanged with the pi3 hat over SPI + +/// This is the format exported by register 34 on the hat. +struct DeviceAttitudeData { + uint8_t present = 0; + uint8_t update_time_10us = 0; + float w = 0; + float x = 0; + float y = 0; + float z = 0; + float x_dps = 0; + float y_dps = 0; + float z_dps = 0; + float a_x_mps2 = 0; + float a_y_mps2 = 0; + float a_z_mps2 = 0; + float bias_x_dps = 0; + float bias_y_dps = 0; + float bias_z_dps = 0; + float uncertainty_w = 0; + float uncertainty_x = 0; + float uncertainty_y = 0; + float uncertainty_z = 0; + float uncertainty_bias_x_dps = 0; + float uncertainty_bias_y_dps = 0; + float uncertainty_bias_z_dps = 0; + uint8_t padding[4] = {}; +} __attribute__((packed)); + +struct DeviceImuConfiguration { + float roll_deg = 0; + float pitch_deg = 0; + float yaw_deg = 0; + uint32_t rate_hz = 0; + + bool operator==(const DeviceImuConfiguration& rhs) const { + return yaw_deg == rhs.yaw_deg && + pitch_deg == rhs.pitch_deg && + roll_deg == rhs.roll_deg && + rate_hz == rhs.rate_hz; + } + + bool operator!=(const DeviceImuConfiguration& rhs) const { + return !(*this == rhs); + } +} __attribute__((packed)); + +struct DeviceSlotData { + uint32_t age_ms = 0; + uint8_t size = 0; + uint8_t data[16] = {}; +} __attribute__((packed)); + +struct DeviceRfStatus { + uint32_t bitfield = 0; + uint32_t lock_age_ms = 0; +} __attribute__((packed)); + +struct DeviceDeviceInfo { + uint8_t git_hash[20] = {}; + uint8_t dirty = 0; + uint8_t serial_number[12] = {}; +} __attribute__((packed)); + +struct DevicePerformance { + uint32_t cycles_per_ms = 0; + uint32_t min_cycles_per_ms = 0; +} __attribute__((packed)); + +struct DeviceCanRate { + int8_t prescaler = -1; + int8_t sync_jump_width = -1; + int8_t time_seg1 = -1; + int8_t time_seg2 = -1; + + bool operator==(const DeviceCanRate& rhs) const { + return prescaler == rhs.prescaler && + sync_jump_width == rhs.sync_jump_width && + time_seg1 == rhs.time_seg1 && + time_seg2 == rhs.time_seg2; + } + + bool operator!=(const DeviceCanRate& rhs) const { + return !(*this == rhs); + } +} __attribute__((packed)); + +struct DeviceCanConfiguration { + int32_t slow_bitrate = 1000000; + int32_t fast_bitrate = 5000000; + int8_t fdcan_frame = 1; + int8_t bitrate_switch = 1; + int8_t automatic_retransmission = 0; + int8_t restricted_mode = 0; + int8_t bus_monitor = 0; + + // If any members of either 'rate' structure are non-negative, use + // them instead of the 'bitrate' fields above. Each rate applies + // to a base clock rate of 85MHz. + DeviceCanRate std_rate; + DeviceCanRate fd_rate; + + bool operator==(const DeviceCanConfiguration& rhs) const { + return slow_bitrate == rhs.slow_bitrate && + fast_bitrate == rhs.fast_bitrate && + fdcan_frame == rhs.fdcan_frame && + bitrate_switch == rhs.bitrate_switch && + automatic_retransmission == rhs.automatic_retransmission && + restricted_mode == rhs.restricted_mode && + bus_monitor == rhs.bus_monitor && + std_rate == rhs.std_rate && + fd_rate == rhs.fd_rate; + } + + bool operator!=(const DeviceCanConfiguration& rhs) const { + return !(*this == rhs); + } +} __attribute__((packed)); + +template +Pi3Hat::ProcessorInfo GetProcessorInfo(Spi* spi, int cs) { + DeviceDeviceInfo di; + spi->Read(cs, 97, reinterpret_cast(&di), sizeof(di)); + + Pi3Hat::ProcessorInfo result; + ::memcpy(&result.git_hash[0], &di.git_hash[0], sizeof(di.git_hash)); + result.dirty = di.dirty != 0; + ::memcpy(&result.serial_number[0], &di.serial_number[0], + sizeof(di.serial_number)); + return result; +} + +template +Pi3Hat::PerformanceInfo GetPerformance(Spi* spi, int cs) { + DevicePerformance dp; + spi->Read(cs, 100, reinterpret_cast(&dp), sizeof(dp)); + + Pi3Hat::PerformanceInfo result; + result.cycles_per_ms = dp.cycles_per_ms; + result.min_cycles_per_ms = dp.min_cycles_per_ms; + return result; +} +} + + +class Pi3Hat::Impl { + public: + Impl(const Configuration& configuration) + : config_(configuration), + primary_spi_{[&]() { + PrimarySpi::Options options; + options.speed_hz = configuration.spi_speed_hz; + return options; + }()}, + aux_spi_{[&]() { + AuxSpi::Options options; + options.speed_hz = configuration.spi_speed_hz; + return options; + }()} { + + // First, look to see if we have a pi3hat attached by looking for + // the eeprom data. This will prevent us from stomping on the SPI + // registers if it isn't ours. + const auto product_code = + ReadContents("/sys/firmware/devicetree/base/hat/product"); + if (!StartsWith(product_code, "mjbots quad pi3 hat")) { + throw std::runtime_error("No pi3hat detected"); + } + + // Since we directly poke at /dev/mem, nothing good can come of + // multiple instances of this class existing at once on the same + // system. + // + // Thus, we use a lock to ensure that at most one copy runs at a + // time. + LockFile(); + + if (config_.raw_spi_only) { + return; + } + + // Verify the versions of all peripherals we will use. + VerifyVersions(); + + if (config_.enable_aux) { + ConfigureAux(); + } + ConfigureCan(); + } + + ~Impl() { + if (lock_file_fd_ >= 0) { + // Who cares about errors here? + ::close(lock_file_fd_); + } + } + + void ConfigureAux() { + // See if we need to update the IMU configuration. + DeviceImuConfiguration original_imu_configuration; + primary_spi_.Read( + 0, 35, + reinterpret_cast(&original_imu_configuration), + sizeof(original_imu_configuration)); + + DeviceImuConfiguration desired_imu; + desired_imu.yaw_deg = config_.mounting_deg.yaw; + desired_imu.pitch_deg = config_.mounting_deg.pitch; + desired_imu.roll_deg = config_.mounting_deg.roll; + desired_imu.rate_hz = + std::min(1000, config_.attitude_rate_hz); + + if (desired_imu != original_imu_configuration) { + primary_spi_.Write( + 0, 36, + reinterpret_cast(&desired_imu), + sizeof(desired_imu)); + + // Give it some time to work. + ::usleep(1000); + DeviceImuConfiguration config_verify; + primary_spi_.Read( + 0, 35, + reinterpret_cast(&config_verify), + sizeof(config_verify)); + ThrowIf( + desired_imu != config_verify, + [&]() { + return Format( + "IMU config not set properly (%f,%f,%f) %d != (%f,%f,%f) %d", + desired_imu.yaw_deg, + desired_imu.pitch_deg, + desired_imu.roll_deg, + desired_imu.rate_hz, + config_verify.yaw_deg, + config_verify.pitch_deg, + config_verify.roll_deg, + config_verify.rate_hz); + }); + } + + // Configure our RF id if necessary. + uint32_t original_id = 0; + primary_spi_.Read( + 0, 49, + reinterpret_cast(&original_id), sizeof(original_id)); + if (original_id != config_.rf_id) { + primary_spi_.Write( + 0, 50, + reinterpret_cast(&config_.rf_id), sizeof(uint32_t)); + // Changing the ID takes at least a few milliseconds. + ::usleep(10000); + uint32_t id_verify = 0; + primary_spi_.Read( + 0, 49, + reinterpret_cast(&id_verify), sizeof(id_verify)); + ThrowIf( + config_.rf_id != id_verify, + [&]() { + return Format("RF Id not set properly (%08x != %08x)", + config_.rf_id, + id_verify); + }); + } + } + + template + void UpdateCanConfig(Spi* spi, int cs, int canbus, + const CanConfiguration& can_config) { + // If the version is before 3, then we can't config anything. + const auto version = ReadByte(spi, cs, 0); + if (version < 3) { return; } + + // Populate what we want our config to look like. + DeviceCanConfiguration out; + out.slow_bitrate = can_config.slow_bitrate; + out.fast_bitrate = can_config.fast_bitrate; + out.fdcan_frame = can_config.fdcan_frame ? 1 : 0; + out.bitrate_switch = can_config.bitrate_switch ? 1 : 0; + out.automatic_retransmission = can_config.automatic_retransmission ? 1 : 0; + out.bus_monitor = can_config.bus_monitor ? 1 : 0; + + out.std_rate.prescaler = can_config.std_rate.prescaler; + out.std_rate.sync_jump_width = can_config.std_rate.sync_jump_width; + out.std_rate.time_seg1 = can_config.std_rate.time_seg1; + out.std_rate.time_seg2 = can_config.std_rate.time_seg2; + + out.fd_rate.prescaler = can_config.fd_rate.prescaler; + out.fd_rate.sync_jump_width = can_config.fd_rate.sync_jump_width; + out.fd_rate.time_seg1 = can_config.fd_rate.time_seg1; + out.fd_rate.time_seg2 = can_config.fd_rate.time_seg2; + + // Check to see if this is what is already there. + DeviceCanConfiguration original_config; + spi->Read(cs, canbus ? 8 : 7, + reinterpret_cast(&original_config), + sizeof(original_config)); + if (original_config == out) { + // We have nothing to do, so just bail early. + return; + } + + // Update the configuration on the device. + spi->Write(cs, canbus ? 10 : 9, + reinterpret_cast(&out), sizeof(out)); + + // Give it some time to work. + ::usleep(100); + DeviceCanConfiguration verify; + spi->Read(cs, canbus ? 8 : 7, + reinterpret_cast(&verify), sizeof(verify)); + ThrowIf( + out != verify, + [&]() { + return "Could not set CAN configuration properly"; + }); + } + + void ConfigureCan() { + UpdateCanConfig(&aux_spi_, 0, 0, config_.can[0]); + UpdateCanConfig(&aux_spi_, 0, 1, config_.can[1]); + UpdateCanConfig(&aux_spi_, 1, 0, config_.can[2]); + UpdateCanConfig(&aux_spi_, 1, 1, config_.can[3]); + if (config_.enable_aux) { + UpdateCanConfig(&primary_spi_, 0, 0, config_.can[4]); + } + } + + void LockFile() { + struct flock lock = {}; + lock.l_type = F_WRLCK; + lock.l_whence = SEEK_SET; + lock.l_start = 0; + lock.l_len = 0; + lock.l_pid = -1; + + lock_file_fd_ = ::open("/tmp/.pi3hat-lock", O_RDWR | O_CREAT, + S_IRUSR | S_IWUSR | S_IRGRP | S_IWGRP); + ThrowIfErrno(lock_file_fd_ < 0, "pi3hat: could not open lock file"); + + const int ret = ::fcntl(lock_file_fd_, F_SETLK, &lock); + ThrowIfErrno(ret < 0, "pi3hat: could not acquire lock, is another process running?"); + } + + template + uint8_t ReadByte(Spi* spi, int cs, int address) { + uint8_t data = 0; + spi->Read(cs, address, reinterpret_cast(&data), 1); + return data; + } + + template + void TestCan(Spi* spi, int cs, const char* name) { + const auto version = ReadByte(spi, cs, 0); + if (version != 2 && version != 3) { + throw std::runtime_error( + Format( + "Processor '%s' has incorrect CAN SPI version %d != [2,3]", + name, version)); + } + } + + void VerifyVersions() { + constexpr int kAttitudeVersion = 0x20; + constexpr int kRfVersion = 0x10; + + if (config_.enable_aux) { + TestCan(&primary_spi_, 0, "aux"); + } + TestCan(&aux_spi_, 0, "can1"); + TestCan(&aux_spi_, 1, "can2"); + + if (config_.enable_aux) { + const auto attitude_version = ReadByte(&primary_spi_, 0, 32); + if (attitude_version != kAttitudeVersion) { + throw std::runtime_error( + Format( + "Incorrect attitude version %d != %d", + attitude_version, kAttitudeVersion)); + } + + + const auto rf_version = ReadByte(&primary_spi_, 0, 48); + if (rf_version != kRfVersion) { + throw std::runtime_error( + Format( + "Incorrect RF version %d != %d", + rf_version, kRfVersion)); + } + } + } + + DeviceInfo device_info() { + DeviceInfo result; + // Now get the device information from all three processors. + result.can1 = GetProcessorInfo(&aux_spi_, 0); + result.can2 = GetProcessorInfo(&aux_spi_, 1); + if (config_.enable_aux) { + result.aux = GetProcessorInfo(&primary_spi_, 0); + } + return result; + } + + DevicePerformance device_performance() { + DevicePerformance result; + // Now get the device information from all three processors. + result.can1 = GetPerformance(&aux_spi_, 0); + result.can2 = GetPerformance(&aux_spi_, 1); + if (config_.enable_aux) { + result.aux = GetPerformance(&primary_spi_, 0); + } + return result; + } + + void ReadSpi(int spi_bus, int address, char* data, size_t size) { + if (spi_bus == 0) { + aux_spi_.Read(0, address, data, size); + } else if (spi_bus == 1) { + aux_spi_.Read(1, address, data, size); + } else if (spi_bus == 2) { + primary_spi_.Read(0, address, data, size); + } + } + + bool GetAttitude(Attitude* output, bool wait, bool detail) { + device_attitude_ = {}; + + // Busy loop until we get something. + if (wait) { + char buf[2] = {}; + do { + primary_spi_.Read(0, 96, buf, sizeof(buf)); + if (buf[1] == 1) { break; } + // If we spam the STM32 too hard, then it doesn't have any + // cycles left to actually work on the IMU. + BusyWaitUs(20); + } while (true); + } + + do { + primary_spi_.Read( + 0, 34, + reinterpret_cast(&device_attitude_), + detail ? sizeof(device_attitude_) : 42); + } while (wait && ((device_attitude_.present & 0x01) == 0)); + + if ((device_attitude_.present & 0x01) == 0) { + return false; + } + + const auto& da = device_attitude_; + auto& o = *output; + o.attitude = { da.w, da.x, da.y, da.z }; + o.rate_dps = { da.x_dps, da.y_dps, da.z_dps }; + o.accel_mps2 = { da.a_x_mps2, da.a_y_mps2, da.a_z_mps2 }; + o.bias_dps = { da.bias_x_dps, da.bias_y_dps, da.bias_z_dps }; + o.attitude_uncertainty = { + da.uncertainty_w, + da.uncertainty_x, + da.uncertainty_y, + da.uncertainty_z, + }; + o.bias_uncertainty_dps = { + da.uncertainty_bias_x_dps, + da.uncertainty_bias_y_dps, + da.uncertainty_bias_z_dps, + }; + + return true; + } + + template + void SendCanPacketSpi(Spi& spi, + int cs, int cpu_bus, + const CanFrame& can_frame) { + const auto size = RoundUpDlc(can_frame.size); + char buf[70] = {}; + + int spi_address = 0; + int spi_size = 0; + + buf[0] = ((cpu_bus == 1) ? 0x80 : 0x00) | (size & 0x7f); + + if (can_frame.id <= 0xffff) { + // We'll use the 2 byte ID formulation, cmd 5 + spi_address = 5; + buf[1] = (can_frame.id >> 8) & 0xff; + buf[2] = can_frame.id & 0xff; + ::memcpy(&buf[3], can_frame.data, can_frame.size); + for (std::size_t i = 3 + can_frame.size; i < (3 + size); i++) { + buf[i] = 0x50; + } + spi_size = 3 + size; + } else { + // 4 byte formulation, cmd 4 + spi_address = 4; + + buf[1] = (can_frame.id >> 24) & 0xff; + buf[2] = (can_frame.id >> 16) & 0xff; + buf[3] = (can_frame.id >> 8) & 0xff; + buf[4] = (can_frame.id >> 0) & 0xff; + ::memcpy(&buf[5], can_frame.data, can_frame.size); + for (std::size_t i = 5 + can_frame.size; i < (5 + size); i++) { + buf[i] = 0x50; + } + spi_size = 5 + size; + } + + spi.Write(cs, spi_address, buf, spi_size); + } + + void SendCanPacket(const CanFrame& can_frame) { + switch (can_frame.bus) { + case 1: { + SendCanPacketSpi(aux_spi_, 0, 0, can_frame); + break; + } + case 2: { + SendCanPacketSpi(aux_spi_, 0, 1, can_frame); + break; + } + case 3: { + SendCanPacketSpi(aux_spi_, 1, 0, can_frame); + break; + } + case 4: { + SendCanPacketSpi(aux_spi_, 1, 1, can_frame); + break; + } + case 5: { + if (config_.enable_aux) { + SendCanPacketSpi(primary_spi_, 0, 0, can_frame); + } + break; + } + } + } + + struct ExpectedReply { + std::array count = { {} }; + + // How long we expect each bus to take to send the frames. + std::array send_ns = { {} }; + + // How long we expect each bus to take to receive any responses. + std::array receive_ns = { {} }; + }; + + ExpectedReply CalculateExpectedReply(const Input& input) { + ExpectedReply result; + + for (size_t i = 0; i < input.tx_can.size(); i++) { + const auto bus = input.tx_can[i].bus; + + const int64_t arbitration_bitrate = + config_.can[bus - 1].slow_bitrate; + const int64_t data_bitrate = + config_.can[bus - 1].bitrate_switch ? + config_.can[bus - 1].fast_bitrate : + config_.can[bus - 1].slow_bitrate; + + const int64_t can_header_size_bits = + (input.tx_can[i].id >= 2048 ? 32 : 16) + 8; + const int64_t can_data_size_bits = + input.tx_can[i].size * 8 + 28; + const int64_t tx_spi_bits = + (input.tx_can[i].size + 5) * 8; + const int64_t can_send_ns = + 1000000000 * can_header_size_bits / arbitration_bitrate + + 1000000000 * can_data_size_bits / data_bitrate + + 1000000000 * tx_spi_bits / config_.spi_speed_hz; + + result.send_ns[bus] += can_send_ns; + + if (input.tx_can[i].expect_reply) { + const int64_t rx_header_size_bits = 32; + const int64_t rx_data_size_bits = + input.tx_can[i].expected_reply_size * 8 + 28; + const int64_t rx_spi_bits = + (input.tx_can[i].expected_reply_size + 5) * 8; + const int64_t rx_ns = + 1000000000 * rx_header_size_bits / arbitration_bitrate + + 1000000000 * rx_data_size_bits / data_bitrate + + 1000000000 * rx_spi_bits / config_.spi_speed_hz; + + result.receive_ns[bus] += rx_ns; + } + + if (input.tx_can[i].expect_reply) { + result.count[bus]++; + } + } + return result; + } + + void SendCan(const Input& input) { + // We try to send packets on alternating buses if possible, so we + // can reduce the average latency before the first data goes out + // on any bus. + for (auto& bus_packets : can_packets_) { + bus_packets.resize(0); + } + + for (size_t i = 0; i < input.tx_can.size(); i++) { + const auto bus = input.tx_can[i].bus; + can_packets_[bus].push_back(i); + } + + int bus_offset[6] = {}; + while (true) { + // We try to send out packets to buses in this order to minimize + // latency. + bool any_sent = false; + for (const int bus : { 1, 3, 5, 2, 4}) { + auto& offset = bus_offset[bus]; + if (offset >= static_cast(can_packets_[bus].size())) { + continue; + } + const auto& can_packet = input.tx_can[can_packets_[bus][offset]]; + offset++; + + SendCanPacket(can_packet); + any_sent = true; + } + + if (!any_sent) { break; } + } + } + + void SendRf(const Span& slots) { + if (!config_.enable_aux) { return; } + + constexpr int kHeaderSize = 5; + constexpr int kMaxDataSize = 16; + uint8_t buf[kHeaderSize + kMaxDataSize] = {}; + + for (size_t i = 0; i < slots.size(); i++) { + const auto& slot = slots[i]; + buf[0] = slot.slot; + buf[1] = (slot.priority >> 0) & 0xff; + buf[2] = (slot.priority >> 8) & 0xff; + buf[3] = (slot.priority >> 16) & 0xff; + buf[4] = (slot.priority >> 24) & 0xff; + ::memcpy(&buf[kHeaderSize], slot.data, slot.size); + + primary_spi_.Write( + 0, 51, reinterpret_cast(&buf[0]), + kHeaderSize + slot.size); + } + } + + void ReadRf(const Input& input, Output* output) { + if (!config_.enable_aux) { return; } + + DeviceRfStatus rf_status; + primary_spi_.Read( + 0, 52, reinterpret_cast(&rf_status), sizeof(rf_status)); + + output->rf_lock_age_ms = rf_status.lock_age_ms; + + const auto bitfield_delta = rf_status.bitfield ^ last_bitfield_; + if (bitfield_delta == 0) { return; } + + DeviceSlotData slot_data; + + for (int i = 0; i < 15; i++) { + if (output->rx_rf_size >= input.rx_rf.size()) { + // No more room. + return; + } + + if ((bitfield_delta & (3 << (i * 2))) == 0) { + continue; + } + + last_bitfield_ ^= (bitfield_delta & (3 << (i * 2))); + + primary_spi_.Read( + 0, 64 + i, reinterpret_cast(&slot_data), sizeof(slot_data)); + auto& output_slot = input.rx_rf[output->rx_rf_size++]; + output_slot.slot = i; + output_slot.age_ms = slot_data.age_ms; + output_slot.size = slot_data.size; + ::memcpy(output_slot.data, slot_data.data, slot_data.size); + } + } + + template + int ReadCanFrames(Spi& spi, int cs, int bus_start, + const Span* rx_can, Output* output) { + // Is there any room? + if (output->rx_can_size >= rx_can->size()) { return 0; } + + int count = 0; + + // Purposefully not initialized for speed. + uint8_t buf[70]; + + while (true) { + // Read until no more frames are available or until the output + // buffer is full. + uint8_t queue_sizes[6] = {}; + spi.Read(cs, 2, + reinterpret_cast(&queue_sizes[0]), sizeof(queue_sizes)); + + bool any_read = false; + + // Read all we can until our buffer is full. + for (int size : queue_sizes) { + if (output->rx_can_size >= rx_can->size()) { + // We're full and can't read any more. + break; + } + + if (size == 0) { continue; } + if (size > (64 + 5)) { + // This is malformed. Lets just set it to the maximum size for now. + size = 64 + 5; + } + + spi.Read(cs, 3, reinterpret_cast(&buf[0]), size); + + if (buf[0] == 0) { + // Hmmm, this shouldn't happen, but indicates there isn't + // really a frame here. + continue; + } + + auto& output_frame = (*rx_can)[output->rx_can_size++]; + count++; + + output_frame.bus = bus_start + ((buf[0] & 0x80) ? 1 : 0); + output_frame.id = (buf[1] << 24) | + (buf[2] << 16) | + (buf[3] << 8) | + (buf[4] << 0); + output_frame.size = size - 5; + ::memcpy(output_frame.data, &buf[5], size - 5); + } + + if (!any_read) { + break; + } + } + + return count; + } + + void FlushReadCan(const Input& input, const ExpectedReply& expected_replies, + Output* output) { + const auto can1_expected = + (expected_replies.count[1] + expected_replies.count[2]) || + input.force_can_check & 0x06; + + if (can1_expected) { + ReadCanFrames(aux_spi_, 0, 1, &input.rx_can, output); + } + + const auto can2_expected = + (expected_replies.count[3] + expected_replies.count[4]) || + input.force_can_check & 0x18; + if (can2_expected) { + ReadCanFrames(aux_spi_, 1, 3, &input.rx_can, output); + } + + const auto aux_expected = + ((expected_replies.count[5]) || input.force_can_check & 0x20) && + config_.enable_aux; + if (aux_expected) { + ReadCanFrames(primary_spi_, 0, 5, &input.rx_can, output); + } + } + + void ReadCan(const Input& input, const ExpectedReply& expected_replies, + Output* output) { + int bus_replies[] = { + expected_replies.count[1] + expected_replies.count[2], + expected_replies.count[3] + expected_replies.count[4], + expected_replies.count[5], + }; + + const int64_t min_rx_timeout_ns = [&]() { + int64_t biggest = 0; + for (int bus = 1; bus <= 5; bus++) { + const int64_t this_ns = + expected_replies.count[bus] ? + (expected_replies.send_ns[bus] + expected_replies.receive_ns[bus]) : 0; + if (this_ns > biggest) { biggest = this_ns; } + } + return biggest + input.rx_baseline_wait_ns; + }(); + + const bool to_check[] = { + bus_replies[0] || input.force_can_check & 0x06, + bus_replies[1] || input.force_can_check & 0x18, + bus_replies[2] || input.force_can_check & 0x20, + }; + + const auto start_now = GetNow(); + int64_t last_reply = start_now; + + while (true) { + bool any_found = false; + // Then check for CAN responses as necessary. + if (to_check[0]) { + const int count = ReadCanFrames(aux_spi_, 0, 1, &input.rx_can, output); + bus_replies[0] -= count; + if (count) { + last_reply = GetNow(); + any_found = true; + } + } + if (to_check[1]) { + const int count = ReadCanFrames(aux_spi_, 1, 3, &input.rx_can, output); + bus_replies[1] -= count; + if (count) { + last_reply = GetNow(); + any_found = true; + } + } + if (to_check[2] && config_.enable_aux) { + const int count = ReadCanFrames(primary_spi_, 0, 5, &input.rx_can, output); + bus_replies[2] -= count; + if (count) { + last_reply = GetNow(); + any_found = true; + } + } + + if (output->rx_can_size >= input.rx_can.size()) { + // Our buffer is full, so no more frames could have been + // returned. + return; + } + + const auto cur_now = GetNow(); + const auto delta_ns = cur_now - start_now; + const auto since_last_ns = cur_now - last_reply; + + if (bus_replies[0] <= 0 && + bus_replies[1] <= 0 && + bus_replies[2] <= 0 && + delta_ns > input.min_tx_wait_ns && + since_last_ns > input.rx_extra_wait_ns) { + // We've read all the replies we are expecting and have polled + // everything at least once if requested. + return; + } + + if (delta_ns > input.timeout_ns && + !(delta_ns < input.min_tx_wait_ns || + delta_ns < min_rx_timeout_ns || + since_last_ns < input.rx_extra_wait_ns)) { + // The timeout has expired. + return; + } + + if (!any_found) { + // Give the controllers a chance to rest. + BusyWaitUs(10); + } + } + } + + Output Cycle(const Input& input) { + Output result; + + auto expected_replies = CalculateExpectedReply(input); + + // First, ensure there aren't any receive frames sitting around + // before we start for CAN busses we expect to have a reply for. + FlushReadCan(input, expected_replies, &result); + + // Send off all our CAN data to all buses. + SendCan(input); + + // While those are sending, do our other work. + if (input.tx_rf.size()) { + SendRf(input.tx_rf); + } + + if (input.request_rf) { + ReadRf(input, &result); + } + + if (input.request_attitude) { + result.attitude_present = + GetAttitude(input.attitude, input.wait_for_attitude, + input.request_attitude_detail); + } + + ReadCan(input, expected_replies, &result); + + primary_spi_.gpio()->SetGpioMode(13, Rpi3Gpio::OUTPUT); + static bool debug_toggle = false; + primary_spi_.gpio()->SetGpioOutput(13, debug_toggle); + debug_toggle = !debug_toggle; + + return result; + } + + const Configuration config_; + + int lock_file_fd_ = -1; + + PrimarySpi primary_spi_; + AuxSpi aux_spi_; + + DeviceAttitudeData device_attitude_; + + // This is a member variable purely so that in steady state we don't + // have to allocate memory. + // + // It is 1 indexed to match the bus naming. + std::vector can_packets_[6]; + + // To keep track of which RF slots we have processed. + uint32_t last_bitfield_ = 0; +}; + +Pi3Hat::Pi3Hat(const Configuration& configuration) + : impl_(new Impl(configuration)) {} + +Pi3Hat::~Pi3Hat() { + delete impl_; +} + +Pi3Hat::Output Pi3Hat::Cycle(const Input& input) { + return impl_->Cycle(input); +} + +Pi3Hat::DeviceInfo Pi3Hat::device_info() { + return impl_->device_info(); +} + +Pi3Hat::DevicePerformance Pi3Hat::device_performance() { + return impl_->device_performance(); +} + +void Pi3Hat::ReadSpi(int spi_bus, int address, char* data, size_t size) { + impl_->ReadSpi(spi_bus, address, data, size); +} + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h new file mode 100644 index 00000000..a6798b33 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h @@ -0,0 +1,311 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#ifndef _MJBOTS_PI3HAT_PI3HAT_H_ +#define _MJBOTS_PI3HAT_PI3HAT_H_ + +/// @file +/// +/// This header file (and associated C++ file), are intended to be +/// standalone, relying on nothing but a C++11 compiler and standard +/// library. + +#include +#include +#include + +namespace mjbots { +namespace pi3hat { + +// Here are some vocabulary types used in the API. + +template +class Span { + public: + Span(T* ptr, size_t size) : ptr_(ptr), size_(size) {} + Span() : ptr_(nullptr), size_(0) {} + + T* data() { return ptr_; } + T* data() const { return ptr_; } + size_t size() const { return size_; } + bool empty() const { return size_ == 0; } + T& operator[](size_t i) { return ptr_[i]; } + T& operator[](size_t i) const { return ptr_[i]; } + T* begin() { return ptr_; } + T* end() { return ptr_ + size_; } + const T* begin() const { return ptr_; } + const T* end() const { return ptr_ + size_; } + + private: + T* ptr_; + size_t size_; +}; + +struct CanFrame { + uint32_t id = 0; + uint8_t data[64] = {}; + uint8_t size = 0; + + /// Bus 1, 2, 3, 4 are the high speed buses labeled JC1, JC2, JC3, + /// JC4. Bus 5 is the low speed bus labeled JC5. + int bus = 0; + + /// If true, then a reply will be expected for this frame on the + /// same bus. + bool expect_reply = false; + + /// If set, this is used as a hint to increase delays. If unset, + /// then the minimum delay may need to be increased. + uint8_t expected_reply_size = 0; +}; + +struct Quaternion { + double w = 0.0; + double x = 0.0; + double y = 0.0; + double z = 0.0; + + Quaternion() {} + Quaternion(double w_in, double x_in, double y_in, double z_in) + : w(w_in), x(x_in), y(y_in), z(z_in) {} +}; + +struct Point3D { + double x = 0.0; + double y = 0.0; + double z = 0.0; + + Point3D() {} + Point3D(double x_in, double y_in, double z_in) + : x(x_in), y(y_in), z(z_in) {} +}; + +struct Euler { + double yaw = 0.0; + double pitch = 0.0; + double roll = 0.0; + + Euler() {} + Euler(double yaw_in, double pitch_in, double roll_in) + : yaw(yaw_in), pitch(pitch_in), roll(roll_in) {} +}; + +struct Attitude { + Quaternion attitude; + Point3D rate_dps; + Point3D accel_mps2; + + Point3D bias_dps; + Quaternion attitude_uncertainty; + Point3D bias_uncertainty_dps; +}; + +struct RfSlot { + uint8_t slot = 0; + uint32_t priority = 0; + uint8_t size = 0; + uint8_t data[16] = {}; + uint32_t age_ms = 0; +}; + +class Error : public std::runtime_error { + public: + using std::runtime_error::runtime_error; +}; + +/// This class provides the top level interface to an application +/// which wants to use all the features of the mjbots pi3 hat in an +/// integrated fashion at high rate. +/// +/// The primary operations are blocking and busy-loop the CPU. For +/// best timing performance, the following steps should be taken. +/// +/// * The CPU this runs on should be isolated from all other OS +/// operations. This can be accomplished through the isolcpus +/// mechanism and sched_setaffinity. +/// * The thread this is running in should be set to realtime +/// priority. +class Pi3Hat { + public: + /// If any of these fields are non-negative, then they are used + /// instead of the "bitrate" options in the CAN configuration. + struct CanRateOverride { + int prescaler = -1; + int sync_jump_width = -1; + int time_seg1 = -1; + int time_seg2 = -1; + }; + + struct CanConfiguration { + int slow_bitrate = 1000000; + int fast_bitrate = 5000000; + bool fdcan_frame = true; + bool bitrate_switch = true; + bool automatic_retransmission = true; + bool restricted_mode = false; + bool bus_monitor = false; + + CanRateOverride std_rate; + CanRateOverride fd_rate; + }; + + struct Configuration { + int spi_speed_hz = 10000000; + + // All attitude data will be transformed by this mounting angle. + Euler mounting_deg; + + // Only a fixed set of rates are achievable. Valid values are + // 100, 200, 400, 1000. Selecting a higher rate than you need to + // sample at will result in more noise. + uint32_t attitude_rate_hz = 400; + + // RF communication will be with a transmitter having this ID. + uint32_t rf_id = 5678; + + bool enable_aux = true; + + CanConfiguration can[5] = {}; + + // If true, nothing is guaranteed to work but ReadSpi. + bool raw_spi_only = false; + + Configuration() {} + }; + + /// This may throw an instance of `Error` if construction fails for + /// some reason + Pi3Hat(const Configuration&); + ~Pi3Hat(); + + // This object is non-copyable. + Pi3Hat(const Pi3Hat&) = delete; + Pi3Hat& operator=(const Pi3Hat&) = delete; + + struct Input { + Span tx_can; + Span tx_rf; + + /// When waiting for CAN replies, the call will return all data + /// that is available (possibly more than was requested). If no + /// data is available, and the given timeout has been exceeded, + /// then it will return anyway. + /// + /// The timeout is measured from when the reading phase begins. + /// This is not super precise, as the writing process has various + /// queues, so it will need to encompass some amount of the time + /// spend writing as well. + uint32_t timeout_ns = 0; + + /// When waiting for CAN replies, guarantee to wait for at least + /// this many nanoseconds after the final transmission is sent + /// over SPI (not necessarily over the CAN bus). + uint32_t min_tx_wait_ns = 200000; + + /// In addition to the absolute min_tx_wait_ns, there is a + /// parallel calculation that attempts to estimate the total delay + /// path for command-response pairs to set a minimum delay. This + /// value controls the non-calculated part of that estimate. It + /// should be the worst case response latency for a single device. + /// + /// Note, by default this is much longer than a device should + /// actually take, so as to handle a wide range of possible host + /// configurations. i.e. including non-isolcpus or non-chrt + /// operation. If you have a properly configured system running + /// on an isolcpu, this can potentially be as small as 200us. + uint32_t rx_baseline_wait_ns = 1000000; + + /// After each successful receipt, wait this much longer for more. + uint32_t rx_extra_wait_ns = 0; + + bool request_attitude = false; + + // If true, then the bias and uncertainty information will be + // filled in. This increases the overall cycle time. + bool request_attitude_detail = false; + + // If no new attitude is available, wait for it. + bool wait_for_attitude = false; + + bool request_rf = false; + + // A bitmask indicating CAN buses to check for data even if no + // replies are expected. + // * bit 0 is unused, so the used bits start from 1 + uint32_t force_can_check = 0; + + // These are data to store results in. + Span rx_can; + Span rx_rf; + Attitude* attitude = nullptr; + }; + + struct Output { + int error = 0; + bool attitude_present = false; + size_t rx_can_size = 0; + size_t rx_rf_size = 0; + + // This will only be updated if 'Input::request_rf' is true + uint32_t rf_lock_age_ms = 0; + }; + + /// Do some or all of the following: + /// * Send the given CAN frames + /// * Wait for at least the requested number of replies (on a + /// per-bus basis), or the given timeout + /// * Read the current ARS result + /// * Send any desired RF slots + /// * Return any RF slots that may have been received + Output Cycle(const Input& input); + + struct ProcessorInfo { + uint8_t git_hash[20] = {}; + bool dirty = false; + uint8_t serial_number[12] = {}; + }; + + struct DeviceInfo { + ProcessorInfo can1; + ProcessorInfo can2; + ProcessorInfo aux; + }; + + DeviceInfo device_info(); + + struct PerformanceInfo { + uint32_t cycles_per_ms = 0; + uint32_t min_cycles_per_ms = 0; + }; + + struct DevicePerformance { + PerformanceInfo can1; + PerformanceInfo can2; + PerformanceInfo aux; + }; + + DevicePerformance device_performance(); + + /// Read raw SPI data. + void ReadSpi(int spi_bus, int address, char* data, size_t size); + + private: + class Impl; + Impl* const impl_; +}; + +} +} + +#endif diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h new file mode 100644 index 00000000..e9ca17c3 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h @@ -0,0 +1,397 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include +#include +#include + +#include "pi3hat.h" + +#include "moteus_transport.h" +#include "realtime.h" + +namespace mjbots { +namespace pi3hat { + +/// This can be used to connect a pi3hat to software that uses the +/// moteus::Transport mechanism for interacting with moteus +/// controllers. The easiest way to use it is to simply include the +/// code from lib/cpp/examples/pi3hat_moteus_transport_register.cc or +/// link against it. +/// +/// It is also possible to manually construct an instance and pass it +/// to the `Controller::Options.transport` method, or manually +/// construct an instance and use its `Cycle` method with the results +/// of `Controller::MakeFoo` methods. +class Pi3HatMoteusTransport : public moteus::Transport { + public: + using CanFdFrame = moteus::CanFdFrame; + + struct Options : public pi3hat::Pi3Hat::Configuration { + int cpu = -1; + + // If the bus for a command is left at 0, and the servo ID is + // present here, assume it is attached to the given bus. + std::map servo_map; + + // This can be used to configure timeouts. Any pointers used to + // accept results will be ignored. + pi3hat::Pi3Hat::Input default_input; + }; + + Pi3HatMoteusTransport(const Options& options) + : options_(options), + thread_(std::bind(&Pi3HatMoteusTransport::CHILD_Run, this)) { + // Try to clear any stale replies. + std::vector replies; + pi3hat::Pi3Hat::Input input_override; + input_override.force_can_check = (1 << 1) | (1 << 2) | (1 << 3); + + moteus::BlockingCallback cbk; + Cycle(nullptr, 0, &replies, nullptr, nullptr, + &input_override, cbk.callback()); + cbk.Wait(); + } + + virtual ~Pi3HatMoteusTransport() { + { + std::lock_guard lock(mutex_); + done_ = true; + condition_.notify_one(); + } + thread_.join(); + } + + virtual void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + moteus::CompletionCallback completed_callback) override { + Cycle(frames, size, replies, nullptr, nullptr, nullptr, completed_callback); + } + + virtual void Post(std::function callback) override { + std::unique_lock lock(mutex_); + event_queue_.push_back(std::move(callback)); + + condition_.notify_one(); + } + + void Cycle(const CanFdFrame* frames, + size_t size, + std::vector* replies, + pi3hat::Attitude* attitude, + pi3hat::Pi3Hat::Output* pi3hat_output, + pi3hat::Pi3Hat::Input* input_override, + moteus::CompletionCallback completed_callback) { + std::lock_guard lock(mutex_); + if (active_) { + throw std::logic_error( + "Cycle cannot be called until the previous has completed"); + } + + cycle_data_ = CycleData{ + frames, size, + replies, + attitude, + pi3hat_output, + input_override, + completed_callback, + }; + active_ = true; + + condition_.notify_all(); + } + + private: + void CHILD_Run() { + if (options_.cpu >= 0) { + ConfigureRealtime(options_.cpu); + } + + pi3hat_.reset(new pi3hat::Pi3Hat(options_)); + + while (true) { + { + std::unique_lock lock(mutex_); + condition_.wait(lock, [&]() { + return (done_ || + active_ || + !event_queue_.empty()); + }); + + if (done_) { return; } + } + + if (active_) { + CHILD_Cycle(); + moteus::CompletionCallback completed_callback; + { + std::unique_lock lock(mutex_); + active_ = false; + std::swap(completed_callback, cycle_data_.completed_callback); + } + completed_callback(0); + } + + // Check for any events to post. + std::function maybe_callback; + { + std::unique_lock lock(mutex_); + if (!event_queue_.empty()) { + maybe_callback = event_queue_.front(); + event_queue_.pop_front(); + } + } + if (maybe_callback) { maybe_callback(); } + } + } + + void CHILD_Cycle() { + // We want to have room to receive frames even if we aren't + // sending anything. + rx_can_.resize(std::max(5, tx_can_.size() * 2)); + + // Now do our actual transaction. + tx_can_.resize(cycle_data_.size); + int out_idx = 0; + for (size_t i = 0; i < cycle_data_.size; i++) { + const auto& cmd = cycle_data_.frames[i]; + auto& can = tx_can_[out_idx++]; + + can.expect_reply = cmd.reply_required; + can.id = cmd.arbitration_id; + can.bus = CHILD_FindBus(cmd.destination, cmd.bus); + can.size = cmd.size; + std::memcpy(can.data, cmd.data, cmd.size); + can.expected_reply_size = cmd.expected_reply_size; + } + + pi3hat::Pi3Hat::Input input = + cycle_data_.input_override ? + *cycle_data_.input_override : + options_.default_input; + input.tx_can = { tx_can_.data(), tx_can_.size() }; + input.rx_can = { rx_can_.data(), rx_can_.size() }; + input.attitude = cycle_data_.attitude; + if (input.attitude) { input.request_attitude = true; } + + const auto output = pi3hat_->Cycle(input); + + if (cycle_data_.pi3hat_output) { *cycle_data_.pi3hat_output = output; } + if (cycle_data_.replies) { + cycle_data_.replies->clear(); + } + for (size_t i = 0; i < output.rx_can_size; i++) { + const auto& can = rx_can_[i]; + + if (cycle_data_.replies) { + cycle_data_.replies->push_back({}); + auto& reply = cycle_data_.replies->back(); + + reply.arbitration_id = can.id; + reply.destination = can.id & 0x7f; + reply.source = (can.id >> 8) & 0x7f; + reply.can_prefix = can.id >> 16; + reply.size = can.size; + std::memcpy(reply.data, can.data, can.size); + reply.bus = can.bus; + } + } + } + + int CHILD_FindBus(int id, int supplied_bus) const { + if (supplied_bus > 1) { return supplied_bus; } + + auto it = options_.servo_map.find(id); + if (it != options_.servo_map.end()) { return it->second; } + + // Assume 1 if we have nothing else to go on. + return 1; + } + + const Options options_; + + //////////////////////////////////////////////////////////////////// + // This block of variables are all controlled by the mutex. + std::mutex mutex_; + std::condition_variable condition_; + bool active_ = false; + bool done_ = false; + struct CycleData { + const CanFdFrame* frames = nullptr; + size_t size = 0; + std::vector* replies = nullptr; + pi3hat::Attitude* attitude = nullptr; + pi3hat::Pi3Hat::Output* pi3hat_output = nullptr; + pi3hat::Pi3Hat::Input* input_override = nullptr; + moteus::CompletionCallback completed_callback; + }; + CycleData cycle_data_; + std::deque> event_queue_; + + std::thread thread_; + + //////////////////////////////////////////////////////////////////// + // All further variables are only used from within the child thread. + + std::unique_ptr pi3hat_; + + // These are kept persistently so that no memory allocation is + // required in steady state. + std::vector tx_can_; + std::vector rx_can_; +}; + +class Pi3HatMoteusFactory : public moteus::TransportFactory { + public: + virtual ~Pi3HatMoteusFactory() {} + + virtual int priority() override { return 5; } + virtual std::string name() override { return "pi3hat"; } + + virtual TransportFactory::TransportArgPair + make(const std::vector& args_in) override{ + auto args = args_in; + Pi3HatMoteusTransport::Options options; + + auto find_arg_it = [&](const std::string& name) { + return std::find(args.begin(), args.end(), name); + }; + + auto find_bool_arg = [&](const std::string& name) { + auto it = find_arg_it(name); + if (it != args.end()) { + args.erase(it); + return true; + } + return false; + }; + + auto find_arg_option = [&](const std::string& name) -> std::string { + auto it = find_arg_it(name); + if (it != args.end() && (it + 1) != args.end()) { + auto result = *(it + 1); + args.erase(it, it + 2); + return result; + } + return {}; + }; + + + if (find_bool_arg("--can-disable-brs")) { + for (auto& can : options.can) { can.bitrate_switch = false; } + } + + const auto pi3hat_cpu = find_arg_option("--pi3hat-cpu"); + if (!pi3hat_cpu.empty()) { + options.cpu = std::stol(pi3hat_cpu); + } + + const auto pi3hat_spi_hz = find_arg_option("--pi3hat-spi-hz"); + if (!pi3hat_spi_hz.empty()) { + options.spi_speed_hz = std::stol(pi3hat_spi_hz); + } + + const auto pi3hat_cfg = find_arg_option("--pi3hat-cfg"); + if(!pi3hat_cfg.empty()) { + options.servo_map = ParseServoMap(pi3hat_cfg); + } + + const auto pi3hat_disable_aux = find_arg_it("--pi3hat-disable-aux"); + if (pi3hat_disable_aux != args.end()) { + options.enable_aux = false; + } + + return std::make_pair(std::make_shared(options), + args); + } + + virtual std::vector cmdline_arguments() override { + return { + { "--can-disable-brs", 0, "do not set BRS" }, + { "--pi3hat-cpu", 1, "CPU used for busy-looping on pi3hat" }, + { "--pi3hat-spi-hz", 1, "SPI speed to use" }, + { "--pi3hat-cfg", 1, "1=ID1,ID2;N=IDX,IDY..." }, + { "--pi3hat-disable-aux", 0, "Prevent use of the IMU/JC5" }, + }; + } + + virtual bool is_args_set(const std::vector& args) override { + for (const auto& arg : args) { + if (arg == "--pi3hat-cpu" || + arg == "--pi3hat-spi-hz" || + arg == "--pi3hat-cfg" || + arg == "--pi3hat-disable-aux") { + return true; + } + } + return false; + } + + static void Register() { + static bool called = false; + if (called) { return; } + called = true; + moteus::TransportRegistry::singleton().Register(); + } + + private: + + static std::vector Split(const std::string& message, + char delim) { + std::vector result; + size_t pos = 0; + while (pos < message.size()) { + const size_t next = message.find(delim, pos); + if (next == std::string::npos) { + result.push_back(message.substr(pos)); + return result; + } + result.push_back(message.substr(pos, next - pos)); + pos = next + 1; + } + + return result; + } + + static std::map ParseServoMap(const std::string& str_map) { + std::map result; + + const auto buses = Split(str_map, ';'); + for (const auto& bus : buses) { + const auto busid_servoids = Split(bus, '='); + if (busid_servoids.size() != 2) { + throw std::runtime_error("Invalid bus specifier: " + bus); + } + const auto bus_id = std::stol(busid_servoids.at(0)); + const auto servoids = Split(busid_servoids.at(1), ','); + for (const auto& servoid : servoids) { + const auto int_servo_id = std::stol(servoid); + result.insert({int_servo_id, bus_id}); + } + } + + return result; + } + +}; + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc new file mode 100644 index 00000000..96f8924d --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc @@ -0,0 +1,785 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/// @file +/// +/// NOTE: This file can be compiled manually on a raspberry pi using +/// the following command line: +/// +/// g++ -mcpu=cortex-a53 -L /opt/vc/lib -I /opt/vc/include -std=c++11 pi3hat_tool.cc pi3hat.cc -lbcm_host -o pi3hat_tool + + +#include +#include + +#include +#include + +#include +#include +#include +#include +#include +#include +#include + +#include "pi3hat.h" + +namespace mjbots { +namespace pi3hat { + +namespace { +int64_t GetNow() { + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC, &ts); + return static_cast(ts.tv_sec) * 1000000000ll + + static_cast(ts.tv_nsec); +} + +std::vector Split(const std::string& input, const char* delim = ",") { + std::vector result; + size_t pos = 0; + while(pos < input.size()) { + const size_t next = input.find_first_of(delim, pos); + result.push_back(input.substr(pos, next - pos)); + if (next == std::string::npos) { break; } + pos = next + 1; + } + return result; +} + +/// Euler angles are in roll, pitch, then yaw. +/// +roll -> clockwise about X axis +/// +pitch -> clockwise about Y axis +/// +yaw -> clockwise about Z axis +struct Euler { + double roll = 0.0; + double pitch = 0.0; + double yaw = 0.0; +}; + +class Quaternion; +inline Quaternion operator*(const Quaternion& lhs, + const Quaternion& rhs); + +class Quaternion { + public: + Quaternion(double w, double x, double y, double z) + : w_(w), x_(x), y_(y), z_(z) {} + + Quaternion() + : w_(1.0), x_(0.), y_(0.), z_(0.) {} + + std::string str() const; + + Quaternion conjugated() const { + return Quaternion(w_, -x_, -y_, -z_); + } + + double norm() const { + return std::sqrt(w_ * w_ + x_ * x_ + y_ * y_ + z_ * z_); + } + + Quaternion normalized() const { + const double n = norm(); + return Quaternion(w_ / n, x_ / n, y_ / n, z_ / n); + } + + Euler euler_rad() const { + Euler result_rad; + + const double sinp = 2.0 * (w_ * y_ - z_ * x_); + if (sinp >= (1.0 - 1e-8)) { + result_rad.pitch = M_PI_2; + result_rad.roll = 0.0; + result_rad.yaw = -2.0 * std::atan2(x_, w_); + } else if (sinp <= (-1.0 + 1e-8)) { + result_rad.pitch = -M_PI_2; + result_rad.roll = 0.0; + result_rad.yaw = 2.0 * std::atan2(x_, w_); + } else { + result_rad.pitch = std::asin(sinp); + + const double sinr_cosp = 2.0 * (w_ * x_ + y_ * z_); + const double cosr_cosp = 1.0 - 2.0 * (x_ * x_ + y_ * y_); + result_rad.roll = std::atan2(sinr_cosp, cosr_cosp); + + const double siny_cosp = 2.0 * (w_ * z_ + x_ * y_); + const double cosy_cosp = 1.0 - 2.0 * (y_ * y_ + z_ * z_); + result_rad.yaw = std::atan2(siny_cosp, cosy_cosp); + } + + return result_rad; + } + + static Quaternion FromEuler( + double roll_rad, double pitch_rad, double yaw_rad) { + // Quaternions multiply in opposite order, and we want to get into + // roll, pitch, then yaw as standard. + return (Quaternion::FromAxisAngle(yaw_rad, 0, 0, 1) * + Quaternion::FromAxisAngle(pitch_rad, 0, 1, 0) * + Quaternion::FromAxisAngle(roll_rad, 1, 0, 0)); + } + + static Quaternion FromEuler(Euler euler_rad) { + return FromEuler(euler_rad.roll, euler_rad.pitch, euler_rad.yaw); + } + + static Quaternion FromAxisAngle( + double angle_rad, double x, double y, double z) { + double c = std::cos(angle_rad / 2.0); + double s = std::sin(angle_rad / 2.0); + + return Quaternion(c, x * s, y * s, z * s); + } + + double w() const { return w_; } + double x() const { return x_; } + double y() const { return y_; } + double z() const { return z_; } + + private: + double w_; + double x_; + double y_; + double z_; +}; + +inline Quaternion operator*(const Quaternion& lhs, + const Quaternion& rhs) { + double a = lhs.w(); + double b = lhs.x(); + double c = lhs.y(); + double d = lhs.z(); + + double e = rhs.w(); + double f = rhs.x(); + double g = rhs.y(); + double h = rhs.z(); + + return Quaternion(a * e - b * f - c * g - d * h, + b * e + a * f + c * h - d * g, + a * g - b * h + c * e + d * f, + a * h + b * g - c * f + d * e); +} + +struct Arguments { + Arguments(const std::vector& args) { + for (size_t i = 0; i < args.size(); i++) { + const auto& arg = args[i]; + if (arg == "-h" || arg == "--help") { + help = true; + } else if (arg == "--can-help") { + can_help = true; + } else if (arg == "--spi-speed") { + spi_speed_hz = std::stoi(args.at(++i)); + } else if (arg == "--disable-aux") { + disable_aux = true; + } else if (arg == "--mount-y") { + mounting_deg.yaw = std::stod(args.at(++i)); + } else if (arg == "--mount-p") { + mounting_deg.pitch = std::stod(args.at(++i)); + } else if (arg == "--mount-r") { + mounting_deg.roll = std::stod(args.at(++i)); + } else if (arg == "--attitude-rate") { + attitude_rate_hz = std::stol(args.at(++i)); + } else if (arg == "--rf-id") { + rf_id = std::stoul(args.at(++i)); + } else if (arg == "--can-config") { + can_config.push_back(args.at(++i)); + } else if (arg == "-c" || arg == "--write-can") { + write_can.push_back(args.at(++i)); + } else if (arg == "--read-can") { + read_can |= (1 << (std::stoi(args.at(++i)))); + } else if (arg == "--can-timeout-ns") { + can_timeout_ns = std::stoull(args.at(++i)); + } else if (arg == "--can-min-tx-wait-ns") { + can_min_tx_wait_ns = std::stoull(args.at(++i)); + } else if (arg == "--can-rx-extra-wait-ns") { + can_rx_extra_wait_ns = std::stoull(args.at(++i)); + } else if (arg == "--time-log") { + time_log = args.at(++i); + } else if (arg == "--realtime") { + realtime = std::stoull(args.at(++i)); + } else if (arg == "--write-rf") { + write_rf.push_back(args.at(++i)); + } else if (arg == "--read-rf") { + read_rf = true; + } else if (arg == "--read-att") { + read_attitude = true; + } else if (arg == "--read-spi") { + read_spi = args.at(++i); + } else if (arg == "--info") { + info = true; + } else if (arg == "--performance") { + performance = true; + } else if (arg == "-r" || arg == "--run") { + run = true; + } else { + throw std::runtime_error("Unknown argument: " + arg); + } + } + } + + bool help = false; + bool can_help = false; + bool run = false; + std::string time_log; + + int spi_speed_hz = -1; + bool disable_aux = false; + Euler mounting_deg; + uint32_t attitude_rate_hz = 400; + uint32_t rf_id = 5678; + + std::vector can_config; + std::vector write_can; + uint32_t read_can = 0; + int64_t can_timeout_ns = 0; + int64_t can_min_tx_wait_ns = 200000; + int64_t can_rx_extra_wait_ns = 40000; + int realtime = -1; + + std::vector write_rf; + bool read_rf = false; + bool read_attitude = false; + std::string read_spi; + + bool info = false; + bool performance = false; +}; + +void DisplayUsage() { + std::cout << "Usage: pi3hat_tool [options]\n"; + std::cout << "\n"; + std::cout << " -h,--help display this usage\n"; + std::cout << " --can-help display CAN configuration format\n"; + std::cout << "\n"; + std::cout << "Configuration\n"; + std::cout << " --spi-speed HZ set the SPI speed\n"; + std::cout << " --disable-aux disable the auxiliary processor\n"; + std::cout << " --mount-y DEG set the mounting yaw angle\n"; + std::cout << " --mount-p DEG set the mounting pitch angle\n"; + std::cout << " --mount-r DEG set the mounting roll angle\n"; + std::cout << " --attitude-rate HZ set the attitude rate\n"; + std::cout << " --rf-id ID set the RF id\n"; + std::cout << " --can-config CFG configure a specific CAN bus\n"; + std::cout << " --can-timeout-ns T set the receive timeout\n"; + std::cout << " --can-min-wait-ns T set the receive timeout\n"; + std::cout << " --realtime CPU run in a realtime configuration on a CPU\n"; + std::cout << "\n"; + std::cout << "Actions\n"; + std::cout << " -c,--write-can write a CAN frame (can be listed 0+)\n"; + std::cout << " BUS,HEXID,HEXDATA,FLAGS\n"; + std::cout << " FLAGS - r (expect reply)\n"; + std::cout << " --read-can BUS force data to be read from the given bus (1-5)\n"; + std::cout << " --write-rf write an RF slot (can be listed 0+)\n"; + std::cout << " SLOT,PRIORITY,DATA\n"; + std::cout << " --read-rf request any RF data\n"; + std::cout << " --read-att request attitude data\n"; + std::cout << " --read-spi read raw SPI data\n"; + std::cout << " SPIBUS,ADDRESS,SIZE\n"; + std::cout << " --info display device info\n"; + std::cout << " --performance print runtime performance\n"; + std::cout << " -r,--run run a sample high rate cycle\n"; + std::cout << " --time-log F when running, write a delta-t log\n"; +} + +void DisplayCanConfigurationUsage() { + std::cout << " Usage: pi3hat_tool [options]\n"; + std::cout << " --can-config CFG\n"; + std::cout << " CFG : CANBUS,[option,]...\n"; + std::cout << " rN: slow_bitrate\n"; + std::cout << " RN: fast_bitrate\n"; + std::cout << " d/D: fdcan_frame=false/true\n"; + std::cout << " b/B: bitrate_switch=false/true\n"; + std::cout << " t/T: automatic_retransmission=false/true\n"; + std::cout << " m/M: bus_monitor=false/true\n"; + std::cout << " [sf]pN: (std|fd)_rate.prescaler\n"; + std::cout << " [sf]jN: (std|fd)_rate.sync_jump_width\n"; + std::cout << " [sf]1N: (std|fd)_rate.time_seg1\n"; + std::cout << " [sf]2N: (std|fd)_rate.time_seg2\n"; +} + +template +Pi3Hat::CanConfiguration ParseCanConfig(Iterator begin, Iterator end) { + Pi3Hat::CanConfiguration result; + for (auto it = begin; it != end; ++it) { + const auto item = *it; + if (item.at(0) == 'r') { + result.slow_bitrate = std::stoi(item.substr(1)); + } else if (item.at(0) == 'R') { + result.fast_bitrate = std::stoi(item.substr(1)); + } else if (item == "d") { + result.fdcan_frame = false; + } else if (item == "D") { + result.fdcan_frame = true; + } else if (item == "b") { + result.bitrate_switch = false; + } else if (item == "B") { + result.bitrate_switch = true; + } else if (item == "t") { + result.automatic_retransmission = false; + } else if (item == "T") { + result.automatic_retransmission = true; + } else if (item == "m") { + result.bus_monitor = false; + } else if (item == "M") { + result.bus_monitor = true; + } else if (item.at(0) == 's' || + item.at(0) == 'f') { + auto& rate = (item.at(0) == 's') ? result.std_rate : result.fd_rate; + const auto value = std::stoi(item.substr(2)); + if (item.at(1) == 'p') { + rate.prescaler = value; + } else if (item.at(1) == 'j') { + rate.sync_jump_width = value; + } else if (item.at(1) == '1') { + rate.time_seg1 = value; + } else if (item.at(1) == '2') { + rate.time_seg2 = value; + } else { + throw std::runtime_error("Unknown rate code:" + item); + } + } else { + throw std::runtime_error("Unknown CAN option: " + item); + } + } + + return result; +} + +Pi3Hat::Configuration MakeConfig(const Arguments& args) { + Pi3Hat::Configuration config; + if (args.spi_speed_hz >= 0) { + config.spi_speed_hz = args.spi_speed_hz; + } + config.mounting_deg.yaw = args.mounting_deg.yaw; + config.mounting_deg.pitch = args.mounting_deg.pitch; + config.mounting_deg.roll = args.mounting_deg.roll; + config.attitude_rate_hz = args.attitude_rate_hz; + config.enable_aux = !args.disable_aux; + + if (args.rf_id != 0) { + config.rf_id = args.rf_id; + } + + for (const auto& can_config : args.can_config) { + const auto can_fields = Split(can_config); + if (can_fields.size() < 2) { + throw std::runtime_error("Error parsing can config: " + can_config); + } + const int can_bus = std::stoi(can_fields.at(0)); + if (can_bus < 1 || can_bus > 5) { + throw std::runtime_error("Invalid CAN bus: " + can_config); + } + + config.can[can_bus - 1] = + ParseCanConfig(can_fields.begin() + 1, can_fields.end()); + } + + if (!args.read_spi.empty()) { + config.raw_spi_only = true; + } + + return config; +} + +void CheckError(int error) { + if (! error) { return; } + throw std::runtime_error( + "Unexpected pi3hat error: " + std::to_string(error)); +} + +int ParseHexNybble(char c) { + if (c >= '0' && c <= '9') { return c - '0'; } + if (c >= 'a' && c <= 'f') { return c - 'a' + 10; } + if (c >= 'A' && c <= 'F') { return c - 'A' + 10; } + return -1; +} + +int ParseHexByte(const char* value) { + int high = ParseHexNybble(value[0]); + if (high < 0) { return high; } + int low = ParseHexNybble(value[1]); + if (low < 0) { return low; } + return (high << 4) | low; +} + +size_t ParseHexData(uint8_t* output, const char* data, size_t size) { + if ((size % 2) != 0) { + throw std::runtime_error("CAN data invalid length: " + + std::string(data, size)); + } + size_t bytes = 0; + for (size_t i = 0; i < size; i += 2) { + const int value = ParseHexByte(&data[i]); + if (value < 0) { + throw std::runtime_error("Invalid CAN data: " + std::string(data, size)); + } + output[bytes++] = value; + } + + return bytes; +} + +std::string FormatHexBytes(const uint8_t* data, size_t size) { + std::string result; + char buf[10] = {}; + for (size_t i = 0; i < size; i++) { + ::snprintf(buf, sizeof(buf) - 1, "%02x", static_cast(data[i])); + result += std::string(buf); + } + return result; +} + +struct MiniTokenizer { + public: + MiniTokenizer(const std::string& data) : data_(data) { + find_next(); + } + + void find_next() { + next = data_.find(',', pos); + if (next == std::string::npos) { + next = data_.size(); + } + } + + void advance() { + pos = next + 1; + find_next(); + } + + size_t pos = 0; + size_t next = 0; + std::string data_; +}; + +CanFrame ParseCanString(const std::string& can_string) { + MiniTokenizer tok{can_string}; + + CanFrame result; + + result.bus = std::strtoul(&can_string[tok.pos], nullptr, 0); + tok.advance(); + result.id = std::strtoul(&can_string[tok.pos], nullptr, 16); + tok.advance(); + result.size = ParseHexData(result.data, &can_string[tok.pos], + tok.next - tok.pos); + + if (tok.next < can_string.size()) { + tok.advance(); + // Any remaining things are flags + for (; tok.pos < can_string.size(); tok.pos++) { + switch (std::tolower(can_string[tok.pos])) { + case 'r': { + result.expect_reply = true; + break; + } + } + } + } + + return result; +} + +RfSlot ParseRfString(const std::string& rf_string) { + MiniTokenizer tok{rf_string}; + + RfSlot result; + result.slot = std::strtoul(&rf_string[tok.pos], nullptr, 0); + tok.advance(); + result.priority = std::strtoul(&rf_string[tok.pos], nullptr, 16); + tok.advance(); + result.size = ParseHexData(result.data, &rf_string[tok.pos], + tok.next - tok.pos); + + return result; +} + +std::string FormatCanFrame(const CanFrame& can_frame) { + char buf[10] = {}; + std::string result; + result += std::to_string(can_frame.bus) + ","; + ::snprintf(buf, sizeof(buf) - 1, "%X", can_frame.id); + result += std::string(buf) + ","; + result += FormatHexBytes(&can_frame.data[0], can_frame.size); + return result; +} + +double R2D(double value) { + return 180.0 * value / 3.141592653589793; +} + +std::string FormatAttitude(const Attitude& a) { + const auto euler_rad = + Quaternion(a.attitude.w, + a.attitude.x, + a.attitude.y, + a.attitude.z).euler_rad(); + + char buf[2048] = {}; + ::snprintf( + buf, sizeof(buf) - 1, + "rpy=(%6.1f,%6.1f,%6.1f) dps=(%5.1f,%5.1f,%5.1f) " + "a=(%4.1f,%4.1f,%4.1f)", + R2D(euler_rad.roll), R2D(euler_rad.pitch), R2D(euler_rad.yaw), + a.rate_dps.x, a.rate_dps.y, a.rate_dps.z, + a.accel_mps2.x, a.accel_mps2.y, a.accel_mps2.z); + return std::string(buf); +} + +std::string FormatRf(const RfSlot& r) { + std::string result = std::to_string(r.slot) + " "; + result += FormatHexBytes(&r.data[0], r.size); + return result; +} + +struct Input { + Input(const Arguments& args) { + auto& pi = pi3hat_input; + pi.attitude = &attitude; + if (args.read_attitude) { + pi.request_attitude = true; + pi.wait_for_attitude = true; + } + if (args.read_rf) { + pi.request_rf = true; + } + pi.force_can_check = args.read_can; + for (const auto& can_string : args.write_can) { + can_frames.push_back(ParseCanString(can_string)); + } + if (!can_frames.empty()) { + pi.tx_can = { &can_frames[0], can_frames.size() }; + } + + pi.timeout_ns = args.can_timeout_ns; + pi.min_tx_wait_ns = args.can_min_tx_wait_ns; + pi.rx_extra_wait_ns = args.can_rx_extra_wait_ns; + + // Try to make sure we have plenty of room to receive things. + rx_frames.resize(std::max(can_frames.size() * 2, 20u)); + pi.rx_can = { &rx_frames[0], rx_frames.size() }; + + for (const auto& rf_string : args.write_rf) { + rf_slots.push_back(ParseRfString(rf_string)); + } + if (!rf_slots.empty()) { + pi.tx_rf = { &rf_slots[0], rf_slots.size() }; + } + + rx_rf_data.resize(16); + pi.rx_rf = { &rx_rf_data[0], rx_rf_data.size() }; + + } + + // We alias our pointers into the input structure. + Input(const Input&) = delete; + Input& operator=(const Input&) = delete; + + Pi3Hat::Input pi3hat_input; + + Attitude attitude; + std::vector can_frames; + std::vector rx_frames; + std::vector rf_slots; + std::vector rx_rf_data; +}; + +void Run(Pi3Hat* pi3hat, const Arguments& args) { + Input input{args}; + + std::unique_ptr time_of; + if (!args.time_log.empty()) { + time_of.reset(new std::ofstream(args.time_log)); + } + + char buf[2048] = {}; + double filtered_period_s = 0.0; + int64_t old_now = GetNow(); + + while (true) { + const auto result = pi3hat->Cycle(input.pi3hat_input); + CheckError(result.error); + + { + const auto now = GetNow(); + const auto delta_ns = now - old_now; + const double delta_s = delta_ns * 1e-9; + const double alpha = 0.98; + filtered_period_s = alpha * filtered_period_s + (1.0 - alpha) * delta_s; + old_now = now; + + if (time_of) { + *time_of << delta_ns << "\n"; + } + } + + if (input.pi3hat_input.wait_for_attitude && !result.attitude_present) { + throw std::runtime_error("Missing attitude data"); + } + + if (result.attitude_present) { + std::cout << FormatAttitude(input.attitude) << " "; + } + + { + ::snprintf( + buf, sizeof(buf) - 1, + "%5.1f Hz \r", + 1.0 / filtered_period_s); + std::cout << buf; + std::cout.flush(); + } + + ::usleep(50); + } +} + +std::string FormatProcessorInfo(const Pi3Hat::ProcessorInfo& pi) { + std::string result = FormatHexBytes(&pi.git_hash[0], 20) + " "; + result += (pi.dirty ? "dirty" : "clean"); + result += " " + FormatHexBytes(&pi.serial_number[0], 12); + return result; +} + +void DoInfo(Pi3Hat* pi3hat) { + const auto di = pi3hat->device_info(); + std::cout << "CAN1: " << FormatProcessorInfo(di.can1) << "\n"; + std::cout << "CAN2: " << FormatProcessorInfo(di.can2) << "\n"; + std::cout << "AUX: " << FormatProcessorInfo(di.aux) << "\n"; +} + +void ReadSpi(Pi3Hat* pi3hat, const std::string& command) { + const std::vector args = Split(command); + const auto spi_bus = std::stoi(args.at(0)); + const auto address = std::stoi(args.at(1)); + const auto size = std::stoi(args.at(2)); + + std::vector data; + data.resize(size); + pi3hat->ReadSpi(spi_bus, address, &data[0], data.size()); + std::cout << FormatHexBytes( + reinterpret_cast(&data[0]), data.size()) << "\n"; +} + +std::string FormatPerformance(const Pi3Hat::PerformanceInfo& p) { + return + "average:" + std::to_string(p.cycles_per_ms) + " " + + "min:" + std::to_string(p.min_cycles_per_ms); +} + +void DoPerformance(Pi3Hat* pi3hat) { + const auto dp = pi3hat->device_performance(); + std::cout << "CAN1: " << FormatPerformance(dp.can1) << "\n"; + std::cout << "CAN2: " << FormatPerformance(dp.can2) << "\n"; + std::cout << "AUX: " << FormatPerformance(dp.aux) << "\n"; +} + +void SingleCycle(Pi3Hat* pi3hat, const Arguments& args) { + Input input{args}; + + const auto result = pi3hat->Cycle(input.pi3hat_input); + CheckError(result.error); + + for (size_t i = 0; i < result.rx_can_size; i++) { + std::cout << "CAN " << FormatCanFrame(input.rx_frames.at(i)) << "\n"; + } + + if (args.read_rf) { + std::cout << "RFLOCK: " << result.rf_lock_age_ms << "\n"; + } + + for (size_t i = 0; i < result.rx_rf_size; i++) { + std::cout << "RF " << FormatRf(input.rx_rf_data.at(i)) << "\n"; + } + + if (result.attitude_present) { + std::cout << "ATT " << FormatAttitude(input.attitude) << "\n"; + } +} + +void ConfigureRealtime(const Arguments& args) { + { + cpu_set_t cpuset = {}; + CPU_ZERO(&cpuset); + CPU_SET(args.realtime, &cpuset); + + const int r = ::sched_setaffinity(0, sizeof(cpu_set_t), &cpuset); + if (r < 0) { + throw std::runtime_error("Error setting CPU affinity"); + } + + std::cout << "Affinity set to " << args.realtime << "\n"; + } + + { + struct sched_param params = {}; + params.sched_priority = 10; + const int r = ::sched_setscheduler(0, SCHED_RR, ¶ms); + if (r < 0) { + throw std::runtime_error("Error setting realtime scheduler"); + } + } + + { + const int r = ::mlockall(MCL_CURRENT | MCL_FUTURE); + if (r < 0) { + throw std::runtime_error("Error locking memory"); + } + } +} + +int do_main(int argc, char** argv) { + Arguments args({argv + 1, argv + argc}); + + if (args.help) { + DisplayUsage(); + return 0; + } + if (args.can_help) { + DisplayCanConfigurationUsage(); + return 0; + } + + Pi3Hat pi3hat{MakeConfig(args)}; + + if (args.realtime >= 0) { + ConfigureRealtime(args); + } + + if (args.run) { + Run(&pi3hat, args); + } else if (args.info) { + DoInfo(&pi3hat); + } else if (!args.read_spi.empty()) { + ReadSpi(&pi3hat, args.read_spi); + } else if (args.performance) { + DoPerformance(&pi3hat); + } else { + SingleCycle(&pi3hat, args); + } + + return 0; +} + +} // namespace +} // namespace pi3hat +} // namespace mjbots + +int main(int argc, char** argv) { + return mjbots::pi3hat::do_main(argc, argv); +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h new file mode 100644 index 00000000..0b94b512 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h @@ -0,0 +1,46 @@ +// Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace mjbots { +namespace pi3hat { + +inline void ConfigureRealtime(int cpu) { + { + cpu_set_t cpuset = {}; + CPU_ZERO(&cpuset); + CPU_SET(cpu, &cpuset); + + const int r = ::sched_setaffinity(0, sizeof(cpu_set_t), &cpuset); + if (r < 0) { + throw std::runtime_error("Error setting CPU affinity"); + } + } + + { + struct sched_param params = {}; + params.sched_priority = 10; + const int r = ::sched_setscheduler(0, SCHED_RR, ¶ms); + if (r < 0) { + throw std::runtime_error( + "Error setting realtime scheduler, try running as root (use sudo)"); + } + } +} + +} +} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/package.xml b/src/meldog_hardware/pi3hat_hardware_interface/package.xml new file mode 100644 index 00000000..6c59bdec --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/package.xml @@ -0,0 +1,18 @@ + + + + pi3hat_hardware_interface + 0.0.0 + TODO: Package description + bartek + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + + ament_cmake + + From 9f03de1a5aeef05db3187c826e6a9688a82558f1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 5 Jul 2024 14:42:41 +0200 Subject: [PATCH 002/137] Removed unused files --- .../include/moteus/BUILD | 51 ------------- .../include/pi3hat/BUILD | 75 ------------------- 2 files changed, 126 deletions(-) delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD b/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD deleted file mode 100644 index 2685f2db..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/BUILD +++ /dev/null @@ -1,51 +0,0 @@ -# -*- python -*- - -# Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -package(default_visibility = ["//visibility:public"]) - -filegroup( - name = "src", - srcs = [ - "moteus_multiplex.h", - "moteus_optional.h", - "moteus_protocol.h", - "moteus_tokenizer.h", - "moteus_transport.h", - "moteus.h", - ], -) - -cc_library( - name = "moteus", - hdrs = [":src"], -) - -cc_test( - name = "test", - srcs = [ - "test/moteus_test.cc", - "test/moteus_protocol_test.cc", - "test/test_main.cc", - ], - copts = [ - "-Ilib/cpp", - ], - deps = [ - ":moteus", - "@boost//:test", - ], - size = "small", -) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD deleted file mode 100644 index 66f01b98..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/BUILD +++ /dev/null @@ -1,75 +0,0 @@ -# -*- python -*- - -# Copyright 2023 mjbots Robotic Systems, LLC. info@mjbots.com -# -# Licensed under the Apache License, Version 2.0 (the "License"); -# you may not use this file except in compliance with the License. -# You may obtain a copy of the License at -# -# http://www.apache.org/licenses/LICENSE-2.0 -# -# Unless required by applicable law or agreed to in writing, software -# distributed under the License is distributed on an "AS IS" BASIS, -# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. -# See the License for the specific language governing permissions and -# limitations under the License. - -package(default_visibility = ["//visibility:public"]) - -filegroup( - name = "src", - srcs = [ - "pi3hat.h", - "pi3hat.cc", - "pi3hat_moteus_transport.h", - "realtime.h", - ], -) - -cc_library( - name = "headers", - hdrs = ["pi3hat.h"], - include_prefix = "mjbots/pi3hat", -) - -cc_library( - name = "libpi3hat", - hdrs = ["pi3hat.h"], - srcs = ["pi3hat.cc"], - deps = [":headers", "@raspberrypi-firmware//:bcm_host"], -) - -cc_binary( - name = "pi3hat_tool", - srcs = ["pi3hat_tool.cc"], - deps = [ - ":libpi3hat", - "@org_llvm_libcxx//:libcxx", - ], -) - -filegroup( - name = "pi3hat", - srcs = [ - ":libpi3hat", - ":pi3hat_tool", - ], -) - -cc_library( - name = "realtime", - hdrs = ["realtime.h"], -) - -cc_library( - name = "transport", - hdrs = [ - "pi3hat_moteus_transport.h", - ], - deps = [ - ":realtime", - ":libpi3hat", - "@moteus//lib/cpp/mjbots/moteus:moteus", - ], - includes = ["."], -) From a7aafa8664d359cbd52e575b56e0ff884f8240f6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 5 Jul 2024 18:29:04 +0200 Subject: [PATCH 003/137] Added standard exceptions --- .../pi3hat_hardware_interface/include/pi3hat/realtime.h | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h index 0b94b512..2c56108a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h @@ -13,7 +13,7 @@ // limitations under the License. #pragma once - +#include #include namespace mjbots { From dd71d0a06f28d8d2c29158b7c254f3d473950d63 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 5 Jul 2024 18:30:29 +0200 Subject: [PATCH 004/137] Added base wrapper and for moteus actuator --- .../include/wrappers/ActuatorWrapperBase.hpp | 36 +++++++++++++++++++ .../include/wrappers/MoteusWrapper.hpp | 27 ++++++++++++++ .../src/wrappers/MoteusWrapper.cpp | 26 ++++++++++++++ 3 files changed, 89 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp new file mode 100644 index 00000000..7d3c026e --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp @@ -0,0 +1,36 @@ +#ifndef _ACTUATOR_WRAPPER_BASE_ +#define _ACTUATOR_WRAPPER_BASE_ + +#include "pi3hat/pi3hat.h" + +template +class ActuatorWrapperBase +{ + + protected: + /* pi3hat CAN frames */ + mjbots::pi3hat::CanFrame& tx_frame_; + mjbots::pi3hat::CanFrame& rx_frame_; + public: + + /* Constructor: takes CanFrame for later editing*/ + ActuatorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame): + tx_frame_(tx_frame), rx_frame_(rx_frame) {}; + + /* Used for CRTP interface */ + Derived& derived() + { + return static_cast(*this); + }; + + /* Static virtual method for preparing TX CAN frame */ + void make_position(double position, double velocity, double feedforward_torque) + { + derived().make_position(position, velocity, feedforward_torque); + }; + +}; + + + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp new file mode 100644 index 00000000..af53934f --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp @@ -0,0 +1,27 @@ +#ifndef _MOTEUS_ACTUATOR_WRAPPER_ +#define _MOTEUS_ACTUATOR_WRAPPER_ + + +#include "moteus/moteus.h" +#include "ActuatorWrapperBase.hpp" + +class MoteusWrapper: public ActuatorWrapperBase, protected mjbots::moteus::Controller +{ + private: + /* Command structure for moteus object*/ + mjbots::moteus::PositionMode::Command position_command_; + + public: + /* Create Moteus Wrapper from existing frames */ + MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, + mjbots::pi3hat::CanFrame& tx_frame,mjbots::pi3hat::CanFrame& rx_frame, + mjbots::moteus::PositionMode::Command command); + + /* Static override */ + void make_position(double position, double velocity, double feedforward_torque); +}; + + + + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp new file mode 100644 index 00000000..ecb08df6 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp @@ -0,0 +1,26 @@ +#include "../include/wrappers/MoteusWrapper.hpp" + + +MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, +mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, +mjbots::moteus::PositionMode::Command command): +ActuatorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controller(options), position_command_(command) +{ + ActuatorWrapperBase::tx_frame_.id = options.id; + ActuatorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure + + ActuatorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus +} + + +void MoteusWrapper::make_position(double position, double velocity, double feedforward_torque) +{ + position_command_.position = position; + position_command_.velocity = velocity; + position_command_.feedforward_torque = feedforward_torque; + + mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); + + tx_frame_.size = can_fd_frame.size; + std::memcpy(tx_frame_.data, can_fd_frame.data, can_fd_frame.size); +} \ No newline at end of file From 8f7950c7e1b3a686aeb2f8ba33c2b0ff5bd90fdc Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 6 Jul 2024 13:55:18 +0200 Subject: [PATCH 005/137] Changed directory to actuator_wrappers --- .../JointActuatorTransform.hpp | 24 +++++++++++++++++++ .../ActuatorWrapperBase.hpp | 2 +- .../MoteusWrapper.hpp | 0 .../src/wrappers/MoteusWrapper.cpp | 2 +- 4 files changed, 26 insertions(+), 2 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp rename src/meldog_hardware/pi3hat_hardware_interface/include/{wrappers => actuator_wrappers}/ActuatorWrapperBase.hpp (98%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{wrappers => actuator_wrappers}/MoteusWrapper.hpp (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp new file mode 100644 index 00000000..8614a2aa --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp @@ -0,0 +1,24 @@ +#ifndef _JOINT_ACTUATOR_TRANSFORM_ +#define _JOINT_ACTUATOR_TRANSFORM_ + +#include +#include +#include +class JointActuatorTransform +{ + private: + std::vector& motor_states_; + std::vector& joint_states_; // Dodaj strukture + public: + + JointActuatorTransform(); + void make_transform_matrix(std::vector gearbox_ratios, std::vector& relations); // Dodaj klase do relacji między jointami + void to_joint_space(); + void to_actuator_space(); +}; + + +#endif + +// TODO: +// STATE DLA JOINTA I SILNIKA, KLASA RELACJI I GEARBOXA, TWORZENIE MACIERZY TRANSFORMACJI W EIGENIE \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp similarity index 98% rename from src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 7d3c026e..a4dd10e9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -6,7 +6,7 @@ template class ActuatorWrapperBase { - + // aaaaaaaa protected: /* pi3hat CAN frames */ mjbots::pi3hat::CanFrame& tx_frame_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/wrappers/MoteusWrapper.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp index ecb08df6..4e014119 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp @@ -1,4 +1,4 @@ -#include "../include/wrappers/MoteusWrapper.hpp" +#include "../include/actuator_wrappers/MoteusWrapper.hpp" MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, From bb0752e93b006a8b4948be76d6c7862d01c1041e Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 6 Jul 2024 17:05:37 +0200 Subject: [PATCH 006/137] Created Transmission and JointActuatorTransform classes --- .../JointActuatorTransform.hpp | 24 -------- .../JointActuatorTransform.hpp | 52 ++++++++++++++++++ .../Transmissions.hpp | 55 +++++++++++++++++++ 3 files changed, 107 insertions(+), 24 deletions(-) delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp deleted file mode 100644 index 8614a2aa..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/JointActuatorTransform/JointActuatorTransform.hpp +++ /dev/null @@ -1,24 +0,0 @@ -#ifndef _JOINT_ACTUATOR_TRANSFORM_ -#define _JOINT_ACTUATOR_TRANSFORM_ - -#include -#include -#include -class JointActuatorTransform -{ - private: - std::vector& motor_states_; - std::vector& joint_states_; // Dodaj strukture - public: - - JointActuatorTransform(); - void make_transform_matrix(std::vector gearbox_ratios, std::vector& relations); // Dodaj klase do relacji między jointami - void to_joint_space(); - void to_actuator_space(); -}; - - -#endif - -// TODO: -// STATE DLA JOINTA I SILNIKA, KLASA RELACJI I GEARBOXA, TWORZENIE MACIERZY TRANSFORMACJI W EIGENIE \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp new file mode 100644 index 00000000..f006fe74 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp @@ -0,0 +1,52 @@ +#ifndef _JOINT_ACTUATOR_TRANSFORM_ +#define _JOINT_ACTUATOR_TRANSFORM_ + +#include +#include +#include "Transmissions.hpp" + +namespace joint_actuator_transform +{ + + class JointActuatorTransform + { + private: + + using vector = Eigen::Vector; + using TransmissionVector = std::vector>; + + /* vectors of joint states */ + vector& joint_positions_; + vector& joint_velocities_; + vector& joint_torques_; + + /* vetors of actuator states */ + vector& actuator_positions_; + vector& actuator_velocities_; + vector& actuator_torques_; + /* actuator -> joint transform matrixes */ + Eigen::SparseMatrix joint_position_velocity_matrix_; + Eigen::SparseMatrix joint_torque_matrix_; + /* joint -> actuator matrixes */ + Eigen::SparseMatrix actuator_position_velocity_matrix_; + Eigen::SparseMatrix actuator_torque_matrix_; + + public: + /* Constructor */ + JointActuatorTransform(vector joint_positions, vector& joint_velocities, vector& joint_torques, + vector& actuator_positions, vector& actuator_velocities, vector& actuator_torques); + + /* Create all transform matrixes */ + void make_transform_matrixes(const TransmissionVector& transmission_vector); + + /* joint <-> actuator transformations */ + void positions_to_joint_space(); + void positions_to_actuator_space(); + void velocities_to_joint_space(); + void velocities_to_actuator_space(); + void torques_to_joint_space(); + void torques_to_actuator_space(); + }; +}; + +#endif diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp new file mode 100644 index 00000000..4bb95169 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp @@ -0,0 +1,55 @@ +#ifndef _TRANSMISSIONS_ +#define _TRANSMISSIONS_ + +#include +#include +#include + + +namespace joint_actuator_transform +{ + class Transmission + { + public: + using T = Eigen::Triplet; + + /* Calculates new triplets for sparse transmission matrix */ + virtual void calculate_transmission_triplets(std::vector& triplet_vector) = 0; + virtual ~Transmission() = default; + }; + + /* Simple transmission (gearboxes, simple pulley systems etc.): + http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1SimpleTransmission.html + */ + class SimpleTransmission: Transmission + { + private: + size_t joint_index_; + double reduction_; + + public: + SimpleTransmission(size_t joint_index, double reduction); + void calculate_transmission_triplets(std::vector& triplet_vector) override; + + }; + + /* More complicated systems where second actuator is coupled with first one, like shown here: + http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1FourBarLinkageTransmission.html + */ + class FourBarLinkageTransmission: Transmission + { + private: + std::vector joint_indexes_; + std::vector actuator_reductions_; + std::vector joint_reductions_; + + public: + FourBarLinkageTransmission(const std::vector& joint_indexes, + std::vector actuator_reductions, std::vector joint_reductions); + void calculate_transmission_triplets(std::vector& triplet_vector) override; + + }; +}; + + +#endif \ No newline at end of file From 51daf546dbff0fe59003514599a2082327436734 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 6 Jul 2024 17:06:06 +0200 Subject: [PATCH 007/137] Modified wrapper classes --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 1 - .../actuator_wrappers}/MoteusWrapper.cpp | 0 .../JointActuatorTransform.cpp | 86 +++++++++++++++++++ .../Transmissions.cpp | 40 +++++++++ 4 files changed, 126 insertions(+), 1 deletion(-) rename src/meldog_hardware/pi3hat_hardware_interface/src/{wrappers => actuator_wrappers/actuator_wrappers}/MoteusWrapper.cpp (100%) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index a4dd10e9..276db5dd 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -6,7 +6,6 @@ template class ActuatorWrapperBase { - // aaaaaaaa protected: /* pi3hat CAN frames */ mjbots::pi3hat::CanFrame& tx_frame_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/actuator_wrappers/MoteusWrapper.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/wrappers/MoteusWrapper.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/actuator_wrappers/MoteusWrapper.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp new file mode 100644 index 00000000..49cc759e --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp @@ -0,0 +1,86 @@ +#include "../include/joint_actuator_transform/JointActuatorTransform.hpp" + + +using namespace joint_actuator_transform; + +JointActuatorTransform::JointActuatorTransform(vector joint_positions, vector& joint_velocities, vector& joint_torques, + vector& actuator_positions, vector& actuator_velocities, vector& actuator_torques): + joint_positions_(joint_positions), joint_velocities_(joint_velocities), + joint_torques_(joint_torques), actuator_positions_(actuator_positions), + actuator_velocities_(actuator_velocities), actuator_torques_(actuator_torques) {} + + +void JointActuatorTransform::make_transform_matrixes(const TransmissionVector& transmission_vector) +{ + /* Create sparse matrix for transforming joint position and velocities to actuator space: + - Q_actuator = A * Q_joint + - V_actuator = A * V_joint + */ + std::vector> transmission_triplets; + for(const auto& transmission: transmission_vector) + { + transmission->calculate_transmission_triplets(transmission_triplets); + } + actuator_position_velocity_matrix_.setFromTriplets(transmission_triplets.begin(), transmission_triplets.end()); + + /* Prepare solvers for calculating other sparse matrixes*/ + Eigen::SimplicialLDLT> solver; + Eigen::SparseMatrix I(actuator_position_velocity_matrix_.rows(),actuator_position_velocity_matrix_.cols()); + I.setIdentity(); + + /* Create sparse matrix for transforming actuator position and velocities to joint space: + - Q_joint = B * Q_actuator + - V_joint = B * V_actuator + - B = (A)^(-1) + */ + solver.compute(actuator_position_velocity_matrix_); + joint_position_velocity_matrix_ = solver.solve(I); + + /* Create sparse matrix for transforming actuator torques to joint space: + - Tau_joint = C * Tau_actuator + - C = (A)^transpose + */ + joint_torque_matrix_ = actuator_position_velocity_matrix_.transpose(); + + /* Create sparse matrix for transforming joint torques to actuator space: + - Tau_actuator = D * Tau_joint + - D = (C)^(-1) + */ + solver.compute(joint_torque_matrix_); + actuator_torque_matrix_ = solver.solve(I); + + /* Compressing all sparse matrixes */ + actuator_position_velocity_matrix_.makeCompressed(); + actuator_torque_matrix_.makeCompressed(); + joint_position_velocity_matrix_.makeCompressed(); + joint_torque_matrix_.makeCompressed(); +} + +void JointActuatorTransform:: positions_to_joint_space() +{ + joint_positions_ = joint_position_velocity_matrix_ * actuator_positions_; +} + +void JointActuatorTransform:: positions_to_actuator_space() +{ + actuator_positions_ = actuator_position_velocity_matrix_ * joint_positions_; +} + +void JointActuatorTransform:: velocities_to_joint_space() +{ + joint_velocities_ = joint_position_velocity_matrix_ * actuator_velocities_; +} + +void JointActuatorTransform:: velocities_to_actuator_space() +{ + actuator_velocities_= actuator_position_velocity_matrix_ * joint_velocities_; +} + +void JointActuatorTransform:: torques_to_joint_space() +{ + joint_torques_ = joint_torque_matrix_ * actuator_torques_; +} +void JointActuatorTransform:: torques_to_actuator_space() +{ + actuator_torques_ = actuator_torque_matrix_ * joint_torques_; +} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp new file mode 100644 index 00000000..54571123 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp @@ -0,0 +1,40 @@ +#include "../include/joint_actuator_transform/Transmissions.hpp" + +using namespace joint_actuator_transform; + +SimpleTransmission::SimpleTransmission(size_t joint_index, double reduction): +joint_index_(joint_index), reduction_(reduction) {} + +void SimpleTransmission::calculate_transmission_triplets(std::vector& triplet_vector) +{ + /* + Simple q_motor = reduction * q_joint + A[joint_index, joint_index] = reduction + */ + triplet_vector.push_back(T(joint_index_, joint_index_, reduction_)); +} + + +FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector& joint_indexes, + std::vector actuator_reductions, std::vector joint_reductions): + joint_indexes_(joint_indexes), actuator_reductions_(actuator_reductions), + joint_reductions_(joint_reductions) {} + +void FourBarLinkageTransmission::calculate_transmission_triplets(std::vector& triplet_vector) +{ + /* + q_motor_1 = a_reduction_1 * j_reduction_1 * q_joint_1 + q_motor_2 = a_reduction_2 * q_joint_1 + a_reduction_2 * j_reduction_2 * q_joint_2 + A[joint_index_1, joint_index_1] = a_reduction_1 * j_reduction_1 + A[joint_index_2, joint_index_1] = a_reduction_2 + A[joint_index_2, joint_index_2] = a_reduction_2 * j_reduction_2 + */ + triplet_vector.push_back(T(joint_indexes_[0], joint_indexes_[0], + actuator_reductions_[0]*joint_reductions_[0])); + + triplet_vector.push_back(T(joint_indexes_[1], joint_indexes_[0], + actuator_reductions_[1])); + + triplet_vector.push_back(T(joint_indexes_[1], joint_indexes_[1], + actuator_reductions_[1]*joint_reductions_[1])); +} \ No newline at end of file From 4130827752dceb90654bd2f2c943e51b686befaf Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 6 Jul 2024 17:11:17 +0200 Subject: [PATCH 008/137] Fixed directories for source code --- .../actuator_wrappers/{actuator_wrappers => }/MoteusWrapper.cpp | 0 .../joint_actuator_transform/JointActuatorTransform.cpp | 0 .../joint_actuator_transform/Transmissions.cpp | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/{actuator_wrappers => }/MoteusWrapper.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{actuator_wrappers => }/joint_actuator_transform/JointActuatorTransform.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{actuator_wrappers => }/joint_actuator_transform/Transmissions.cpp (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/actuator_wrappers/MoteusWrapper.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/JointActuatorTransform.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/Transmissions.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/joint_actuator_transform/Transmissions.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/Transmissions.cpp From dc7abb37f74a1c028afa8d16dd7cd49643a27b35 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 6 Jul 2024 17:26:26 +0200 Subject: [PATCH 009/137] Added comments to code --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 25 +++++++++++++------ .../JointActuatorTransform.hpp | 5 +++- .../Transmissions.hpp | 11 +++++--- .../src/actuator_wrappers/MoteusWrapper.cpp | 9 +++++++ 4 files changed, 39 insertions(+), 11 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 276db5dd..805e6838 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -3,25 +3,36 @@ #include "pi3hat/pi3hat.h" + +/* + Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface + to create CAN frames for pi3hat Input structure. Note that it uses static polymorphism, + so remember to change instance of your derived class in pi3hat hardware interface files + (only for creation of an object). +*/ template class ActuatorWrapperBase { + private: + + /* Used for CRTP interface */ + Derived& derived() + { + return static_cast(*this); + }; + protected: + /* pi3hat CAN frames */ mjbots::pi3hat::CanFrame& tx_frame_; mjbots::pi3hat::CanFrame& rx_frame_; - public: + public: + /* Constructor: takes CanFrame for later editing*/ ActuatorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame): tx_frame_(tx_frame), rx_frame_(rx_frame) {}; - /* Used for CRTP interface */ - Derived& derived() - { - return static_cast(*this); - }; - /* Static virtual method for preparing TX CAN frame */ void make_position(double position, double velocity, double feedforward_torque) { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp index f006fe74..9c47c0e2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp @@ -7,7 +7,10 @@ namespace joint_actuator_transform { - + /* + Class for transforming joint space to actuator space and actuator space to joint space. + It uses Transmission objects to create sparse matrixes used in transformations. + */ class JointActuatorTransform { private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp index 4bb95169..e3f69c61 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp @@ -7,7 +7,11 @@ namespace joint_actuator_transform -{ +{ + /* + Transmission interface class. Its job is to create triplets for sparse transformation matrixes. + + */ class Transmission { public: @@ -18,7 +22,7 @@ namespace joint_actuator_transform virtual ~Transmission() = default; }; - /* Simple transmission (gearboxes, simple pulley systems etc.): + /* Simple Transmission: (gearboxes, simple pulley systems etc.): http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1SimpleTransmission.html */ class SimpleTransmission: Transmission @@ -33,7 +37,8 @@ namespace joint_actuator_transform }; - /* More complicated systems where second actuator is coupled with first one, like shown here: + /* FourBarLinkage Transmission: more complicated systems where second + actuator is coupled with first one, like shown here: http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1FourBarLinkageTransmission.html */ class FourBarLinkageTransmission: Transmission diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 4e014119..ab6990e1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -1,11 +1,17 @@ #include "../include/actuator_wrappers/MoteusWrapper.hpp" +/* + Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. + https://github.com/mjbots/moteus + +*/ MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, mjbots::moteus::PositionMode::Command command): ActuatorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controller(options), position_command_(command) { + /* Prepare CAN tx frame*/ ActuatorWrapperBase::tx_frame_.id = options.id; ActuatorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure @@ -15,12 +21,15 @@ ActuatorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controll void MoteusWrapper::make_position(double position, double velocity, double feedforward_torque) { + /* Change command values */ position_command_.position = position; position_command_.velocity = velocity; position_command_.feedforward_torque = feedforward_torque; + /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); + /* Copy data from CANFD frame to CAN frame*/ tx_frame_.size = can_fd_frame.size; std::memcpy(tx_frame_.data, can_fd_frame.data, can_fd_frame.size); } \ No newline at end of file From 0a5502730a5383781a44c5f6b96b63a5b9f6d999 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 7 Jul 2024 17:47:21 +0200 Subject: [PATCH 010/137] Added offsets to joints --- .../JointActuatorTransform.hpp | 7 +++++++ .../JointActuatorTransform.cpp | 21 ++++++++++++------- 2 files changed, 20 insertions(+), 8 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp index 9c47c0e2..a0c54a2a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp @@ -34,6 +34,8 @@ namespace joint_actuator_transform Eigen::SparseMatrix actuator_position_velocity_matrix_; Eigen::SparseMatrix actuator_torque_matrix_; + vector joint_offsets_; + public: /* Constructor */ JointActuatorTransform(vector joint_positions, vector& joint_velocities, vector& joint_torques, @@ -42,6 +44,11 @@ namespace joint_actuator_transform /* Create all transform matrixes */ void make_transform_matrixes(const TransmissionVector& transmission_vector); + /* Add joint offsets to transform + q_actuator = 0 and q_joint = q_joint_start + */ + void add_offsets(vector&& joint_offsets); + /* joint <-> actuator transformations */ void positions_to_joint_space(); void positions_to_actuator_space(); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp index 49cc759e..642851f7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp @@ -56,31 +56,36 @@ void JointActuatorTransform::make_transform_matrixes(const TransmissionVector& t joint_torque_matrix_.makeCompressed(); } -void JointActuatorTransform:: positions_to_joint_space() +void JointActuatorTransform::positions_to_joint_space() { - joint_positions_ = joint_position_velocity_matrix_ * actuator_positions_; + joint_positions_ = joint_position_velocity_matrix_ * actuator_positions_ + joint_offsets_; } -void JointActuatorTransform:: positions_to_actuator_space() +void JointActuatorTransform::positions_to_actuator_space() { - actuator_positions_ = actuator_position_velocity_matrix_ * joint_positions_; + actuator_positions_ = actuator_position_velocity_matrix_ * (joint_positions_ - joint_offsets_); } -void JointActuatorTransform:: velocities_to_joint_space() +void JointActuatorTransform::velocities_to_joint_space() { joint_velocities_ = joint_position_velocity_matrix_ * actuator_velocities_; } -void JointActuatorTransform:: velocities_to_actuator_space() +void JointActuatorTransform::velocities_to_actuator_space() { actuator_velocities_= actuator_position_velocity_matrix_ * joint_velocities_; } -void JointActuatorTransform:: torques_to_joint_space() +void JointActuatorTransform::torques_to_joint_space() { joint_torques_ = joint_torque_matrix_ * actuator_torques_; } -void JointActuatorTransform:: torques_to_actuator_space() +void JointActuatorTransform::torques_to_actuator_space() { actuator_torques_ = actuator_torque_matrix_ * joint_torques_; +} + +void JointActuatorTransform::add_offsets(vector&& joint_offsets) +{ + joint_offsets_ = std::move(joint_offsets); } \ No newline at end of file From 23ccbdbb7e07e9c7d9df507d5fdfa53a022b396b Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 7 Jul 2024 19:56:55 +0200 Subject: [PATCH 011/137] Added first iteration of header file --- .../pi3hat_hardware_interface.hpp | 106 ++++++++++++++++++ 1 file changed, 106 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp new file mode 100644 index 00000000..58cf7a31 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -0,0 +1,106 @@ +#ifndef PI3HAT_HARDWARE_INTERFACE__PI3HAT_HARDWARE_INTERFACE_ +#define PI3HAT_HARDWARE_INTERFACE__PI3HAT_HARDWARE_INTERFACE_ + + +#include +#include +#include + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" +#include "rclcpp/clock.hpp" +#include "rclcpp/duration.hpp" +#include "rclcpp/macros.hpp" +#include "rclcpp/time.hpp" +#include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" +#include "rclcpp_lifecycle/state.hpp" + +#include "actuator_wrappers/MoteusWrapper.hpp" +#include "joint_actuator_transform/JointActuatorTransform.hpp" + +#include "pi3hat/pi3hat.h" +#include "pi3hat/realtime.h" + + +namespace pi3hat_hardware_interface +{ + class Pi3HatHardwareInterface : public hardware_interface::SystemInterface + { + public: + RCLCPP_SHARED_PTR_DEFINITIONS(Pi3HatHardwareInterface) + + hardware_interface::CallbackReturn on_init( + const hardware_interface::HardwareInfo &info) override; + + hardware_interface::CallbackReturn on_configure( + const rclcpp_lifecycle::State &previous_state) override; + + std::vector export_state_interfaces() override; + + std::vector export_command_interfaces() override; + + hardware_interface::CallbackReturn on_activate( + const rclcpp_lifecycle::State &previous_state) override; + + hardware_interface::CallbackReturn on_deactivate( + const rclcpp_lifecycle::State &previous_state) override; + + hardware_interface::return_type read( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + hardware_interface::return_type write( + const rclcpp::Time &time, const rclcpp::Duration &period) override; + + private: + + mjbots::pi3hat::Pi3Hat& pi3hat_; + mjbots::pi3hat::Pi3Hat::Input pi3hat_input_; + mjbots::pi3hat::Attitude attitude_; + std::vector tx_can_frames_; //Pomysl jeszcze nad tym! + std::vector rx_can_frames_; + + /* IMU states */ + std::array hw_state_imu_orientation_; // x, y, z, w + std::array hw_state_imu_angular_velocity_; // x, y, z + std::array hw_state_imu_linear_acceleration_; // x, y, z + + /* Actuator CAN config */ + std::vector hw_actuator_can_channels_; + std::vector hw_actuator_can_ids_; + + /* Actuator parameters */ + std::vector hw_actuator_position_scales_; + std::vector hw_actuator_velocity_scales_; + std::vector hw_actuator_effort_scales_; + std::vector hw_actuator_kp_scales_; + std::vector hw_actuator_kd_scales_; + std::vector hw_actuator_axis_directions_; + std::vector hw_actuator_position_offsets_; + + // Actuator limits + std::vector hw_actuator_position_mins_; + std::vector hw_actuator_position_maxs_; + std::vector hw_actuator_velocity_maxs_; + std::vector hw_actuator_effort_maxs_; + std::vector hw_actuator_kp_maxs_; + std::vector hw_actuator_kd_maxs_; + + using eigen_vector = Eigen::Vector; + + /* Joint states */ + eigen_vector hw_joint_positions_; + eigen_vector hw_joint_velocities_; + eigen_vector hw_joint_torques_; + + // Actuator commands + eigen_vector hw_command_positions_; + eigen_vector hw_command_velocities_; + eigen_vector hw_command_torques_; + + }; + +} + +#endif From 61b9d904a8d1aa3c29f2ed991823cf1e79a82ed6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 7 Jul 2024 20:03:30 +0200 Subject: [PATCH 012/137] Added visibility control --- .../pi3hat_hardware_interface.hpp | 11 +++- .../visibility_control.hpp | 56 +++++++++++++++++++ 2 files changed, 66 insertions(+), 1 deletion(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/visibility_control.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 58cf7a31..53b724a2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -23,6 +23,7 @@ #include "pi3hat/pi3hat.h" #include "pi3hat/realtime.h" +#include "visibility_control.hpp" namespace pi3hat_hardware_interface { @@ -31,25 +32,33 @@ namespace pi3hat_hardware_interface public: RCLCPP_SHARED_PTR_DEFINITIONS(Pi3HatHardwareInterface) + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_init( const hardware_interface::HardwareInfo &info) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State &previous_state) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC std::vector export_state_interfaces() override; - + + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC std::vector export_command_interfaces() override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_activate( const rclcpp_lifecycle::State &previous_state) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::CallbackReturn on_deactivate( const rclcpp_lifecycle::State &previous_state) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::return_type read( const rclcpp::Time &time, const rclcpp::Duration &period) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::return_type write( const rclcpp::Time &time, const rclcpp::Duration &period) override; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/visibility_control.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/visibility_control.hpp new file mode 100644 index 00000000..6fae4a91 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/visibility_control.hpp @@ -0,0 +1,56 @@ +// Copyright 2021 ros2_control Development Team +// +// Licensed under the Apache License, Version 2.0 (the "License"); +// you may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +/* This header must be included by all rclcpp headers which declare symbols + * which are defined in the rclcpp library. When not building the rclcpp + * library, i.e. when using the headers in other package's code, the contents + * of this header change the visibility of certain symbols which the rclcpp + * library cannot have, but the consuming code must have inorder to link. + */ + +#ifndef ROS2_CONTROL_PI3HAT_HARDWARE__VISIBILITY_CONTROL_H_ +#define ROS2_CONTROL_PI3HAT_HARDWARE__VISIBILITY_CONTROL_H_ + +// This logic was borrowed (then namespaced) from the examples on the gcc wiki: +// https://gcc.gnu.org/wiki/Visibility + +#if defined _WIN32 || defined __CYGWIN__ +#ifdef __GNUC__ +#define ROS2_CONTROL_PI3HAT_HARDWARE_EXPORT __attribute__((dllexport)) +#define ROS2_CONTROL_PI3HAT_HARDWARE_IMPORT __attribute__((dllimport)) +#else +#define ROS2_CONTROL_PI3HAT_HARDWARE_EXPORT __declspec(dllexport) +#define ROS2_CONTROL_PI3HAT_HARDWARE_IMPORT __declspec(dllimport) +#endif +#ifdef ROS2_CONTROL_PI3HAT_HARDWARE_BUILDING_DLL +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC ROS2_CONTROL_PI3HAT_HARDWARE_EXPORT +#else +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC ROS2_CONTROL_PI3HAT_HARDWARE_IMPORT +#endif +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC_TYPE ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC +#define ROS2_CONTROL_PI3HAT_HARDWARE_LOCAL +#else +#define ROS2_CONTROL_PI3HAT_HARDWARE_EXPORT __attribute__((visibility("default"))) +#define ROS2_CONTROL_PI3HAT_HARDWARE_IMPORT +#if __GNUC__ >= 4 +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC __attribute__((visibility("default"))) +#define ROS2_CONTROL_PI3HAT_HARDWARE_LOCAL __attribute__((visibility("hidden"))) +#else +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC +#define ROS2_CONTROL_PI3HAT_HARDWARE_LOCAL +#endif +#define ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC_TYPE +#endif + +#endif // ROS2_CONTROL_PI3HAT_HARDWARE__VISIBILITY_CONTROL_H_ \ No newline at end of file From 92b3609961678d7b3eee434c5d6a875c0b8d7d00 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 8 Jul 2024 17:33:03 +0200 Subject: [PATCH 013/137] Changed names for wrappers and pi3hat --- .../MoteusWrapper.hpp | 4 +- .../MotorWrapperBase.hpp} | 10 ++-- .../pi3hat_hardware_interface.hpp | 59 +++++++++---------- .../MoteusWrapper.cpp | 12 ++-- 4 files changed, 41 insertions(+), 44 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/include/{actuator_wrappers => motor_wrappers}/MoteusWrapper.hpp (81%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{actuator_wrappers/ActuatorWrapperBase.hpp => motor_wrappers/MotorWrapperBase.hpp} (76%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{actuator_wrappers => motor_wrappers}/MoteusWrapper.cpp (60%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp similarity index 81% rename from src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp index af53934f..792dc4d2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp @@ -3,9 +3,9 @@ #include "moteus/moteus.h" -#include "ActuatorWrapperBase.hpp" +#include "MotorWrapperBase.hpp" -class MoteusWrapper: public ActuatorWrapperBase, protected mjbots::moteus::Controller +class MoteusWrapper: public MotorWrapperBase, protected mjbots::moteus::Controller { private: /* Command structure for moteus object*/ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp similarity index 76% rename from src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp index 805e6838..ca338908 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp @@ -1,17 +1,17 @@ -#ifndef _ACTUATOR_WRAPPER_BASE_ -#define _ACTUATOR_WRAPPER_BASE_ +#ifndef _MOTOR_WRAPPER_BASE_ +#define _MOTOR_WRAPPER_BASE_ #include "pi3hat/pi3hat.h" /* - Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface + Base Motor Wrapper class, used for wrapping actuator API with simple CRTP interface to create CAN frames for pi3hat Input structure. Note that it uses static polymorphism, so remember to change instance of your derived class in pi3hat hardware interface files (only for creation of an object). */ template -class ActuatorWrapperBase +class MotorWrapperBase { private: @@ -30,7 +30,7 @@ class ActuatorWrapperBase public: /* Constructor: takes CanFrame for later editing*/ - ActuatorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame): + MotorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame): tx_frame_(tx_frame), rx_frame_(rx_frame) {}; /* Static virtual method for preparing TX CAN frame */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 53b724a2..33dcbc7c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -17,8 +17,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "actuator_wrappers/MoteusWrapper.hpp" -#include "joint_actuator_transform/JointActuatorTransform.hpp" +#include "motor_wrappers/MoteusWrapper.hpp" #include "pi3hat/pi3hat.h" #include "pi3hat/realtime.h" @@ -76,37 +75,35 @@ namespace pi3hat_hardware_interface std::array hw_state_imu_linear_acceleration_; // x, y, z /* Actuator CAN config */ - std::vector hw_actuator_can_channels_; - std::vector hw_actuator_can_ids_; + std::vector hw_motor_can_channels_; + std::vector hw_motor_can_ids_; /* Actuator parameters */ - std::vector hw_actuator_position_scales_; - std::vector hw_actuator_velocity_scales_; - std::vector hw_actuator_effort_scales_; - std::vector hw_actuator_kp_scales_; - std::vector hw_actuator_kd_scales_; - std::vector hw_actuator_axis_directions_; - std::vector hw_actuator_position_offsets_; - - // Actuator limits - std::vector hw_actuator_position_mins_; - std::vector hw_actuator_position_maxs_; - std::vector hw_actuator_velocity_maxs_; - std::vector hw_actuator_effort_maxs_; - std::vector hw_actuator_kp_maxs_; - std::vector hw_actuator_kd_maxs_; - - using eigen_vector = Eigen::Vector; - - /* Joint states */ - eigen_vector hw_joint_positions_; - eigen_vector hw_joint_velocities_; - eigen_vector hw_joint_torques_; - - // Actuator commands - eigen_vector hw_command_positions_; - eigen_vector hw_command_velocities_; - eigen_vector hw_command_torques_; + std::vector hw_motor_position_scales_; + std::vector hw_motor_velocity_scales_; + std::vector hw_motor_effort_scales_; + std::vector hw_motor_kp_scales_; + std::vector hw_motor_kd_scales_; + std::vector hw_motor_axis_directions_; + std::vector hw_motor_position_offsets_; + + /* Motor limits */ + std::vector hw_motor_position_mins_; + std::vector hw_motor_position_maxs_; + std::vector hw_motor_velocity_maxs_; + std::vector hw_motor_effort_maxs_; + std::vector hw_motor_kp_maxs_; + std::vector hw_motor_kd_maxs_; + + /* Motor states */ + std::vector hw_motor_positions_; + std::vector hw_motor_velocities_; + std::vector hw_motor_torques_; + + // Motor commands + std::vector hw_command_positions_; + std::vector hw_command_velocities_; + std::vector hw_command_torques_; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp similarity index 60% rename from src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp index ab6990e1..b3060e24 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp @@ -1,21 +1,21 @@ -#include "../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../include/motor_wrappers/MoteusWrapper.hpp" /* - Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. + Moteus Motor Wrapper. It uses API from moteus repository to communicate with pi3hat interface. https://github.com/mjbots/moteus */ MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, mjbots::moteus::PositionMode::Command command): -ActuatorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controller(options), position_command_(command) +MotorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controller(options), position_command_(command) { /* Prepare CAN tx frame*/ - ActuatorWrapperBase::tx_frame_.id = options.id; - ActuatorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure + MotorWrapperBase::tx_frame_.id = options.id; + MotorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure - ActuatorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus + MotorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus } From 487d0094550cdac2422d55999a020030e7aa1c1f Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 8 Jul 2024 19:04:56 +0200 Subject: [PATCH 014/137] First iteration of pi3hat cpp file --- .../pi3hat_hardware_interface.hpp | 21 +++---- .../src/pi3hat_hardware_interface.cpp | 63 +++++++++++++++++++ 2 files changed, 71 insertions(+), 13 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 33dcbc7c..5e5d7448 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -24,6 +24,8 @@ #include "visibility_control.hpp" +#include + namespace pi3hat_hardware_interface { class Pi3HatHardwareInterface : public hardware_interface::SystemInterface @@ -63,28 +65,23 @@ namespace pi3hat_hardware_interface private: - mjbots::pi3hat::Pi3Hat& pi3hat_; + std::shared_ptr pi3hat_; mjbots::pi3hat::Pi3Hat::Input pi3hat_input_; mjbots::pi3hat::Attitude attitude_; - std::vector tx_can_frames_; //Pomysl jeszcze nad tym! - std::vector rx_can_frames_; + + std::shared_ptr tx_can_frames_; //Pomysl jeszcze nad tym! + std::shared_ptr rx_can_frames_; /* IMU states */ std::array hw_state_imu_orientation_; // x, y, z, w std::array hw_state_imu_angular_velocity_; // x, y, z std::array hw_state_imu_linear_acceleration_; // x, y, z - /* Actuator CAN config */ + /* Motor CAN config */ std::vector hw_motor_can_channels_; std::vector hw_motor_can_ids_; - /* Actuator parameters */ - std::vector hw_motor_position_scales_; - std::vector hw_motor_velocity_scales_; - std::vector hw_motor_effort_scales_; - std::vector hw_motor_kp_scales_; - std::vector hw_motor_kd_scales_; - std::vector hw_motor_axis_directions_; + /* Motor parameters */ std::vector hw_motor_position_offsets_; /* Motor limits */ @@ -92,8 +89,6 @@ namespace pi3hat_hardware_interface std::vector hw_motor_position_maxs_; std::vector hw_motor_velocity_maxs_; std::vector hw_motor_effort_maxs_; - std::vector hw_motor_kp_maxs_; - std::vector hw_motor_kd_maxs_; /* Motor states */ std::vector hw_motor_positions_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp new file mode 100644 index 00000000..aa5f8137 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -0,0 +1,63 @@ +#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" + +using namespace pi3hat_hardware_interface; + + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) +{ + if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) + { + return hardware_interface::CallbackReturn::ERROR; + } + hw_motor_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_motor_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_motor_torques_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + hw_command_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_command_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_command_torques_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + for (const hardware_interface::ComponentInfo &joint : info_.joints) + { + hw_motor_can_channels_.push_back(std::stoi(joint.parameters.at("can_channel"))); + hw_motor_can_ids_.push_back(std::stoi(joint.parameters.at("can_id"))); + hw_motor_position_mins_.push_back(std::stod(joint.parameters.at("position_min"))); + hw_motor_position_maxs_.push_back(std::stod(joint.parameters.at("position_max"))); + hw_motor_velocity_maxs_.push_back(std::stod(joint.parameters.at("velocity_max"))); + hw_motor_effort_maxs_.push_back(std::stod(joint.parameters.at("effort_max"))); + hw_motor_position_offsets_.push_back(std::stod(joint.parameters.at("position_offset"))); + } + + /* Standard CAN config */ + mjbots::pi3hat::Pi3Hat::CanConfiguration can_config; + + /* Configure the Pi3Hat for 1000hz IMU sampling */ + mjbots::pi3hat::Pi3Hat::Configuration config; + config.attitude_rate_hz = 1000; + /* Set the mounting orientation of the IMU */ + config.mounting_deg.yaw = std::stod(info_.hardware_parameters.at("imu_mounting_deg.yaw")); + config.mounting_deg.pitch = std::stod(info_.hardware_parameters.at("imu_mounting_deg.pitch")); + config.mounting_deg.roll = std::stod(info_.hardware_parameters.at("imu_mounting_deg.roll")); + /* Initialize the Pi3Hat input */ + pi3hat_input_ = mjbots::pi3hat::Pi3Hat::Input(); + pi3hat_input_.attitude = &attitude_; + + tx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[info_.joints.size()]); + rx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[info_.joints.size()]); + + mjbots::pi3hat::Span rx_can_frames_span_(rx_can_frames_.get(), info_.joints.size()); + pi3hat_input_.rx_can = rx_can_frames_span_; + mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), info_.joints.size()); + pi3hat_input_.tx_can = tx_can_frames_span_; + + // Set up the CAN configuration + for (size_t i = 0; i < info_.joints.size(); i++) + { + config.can[hw_motor_can_channels_[i] - 1] = can_config; + pi3hat_input_.tx_can[i].id = hw_motor_can_ids_[i]; + pi3hat_input_.tx_can[i].bus = hw_motor_can_channels_[i]; + pi3hat_input_.tx_can[i].expect_reply = true; + } + // Initialize the Pi3Hat + pi3hat_ = std::make_shared(config); +} \ No newline at end of file From d5fe51930f98b7f1e69b60840a646a2620e2c704 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 8 Jul 2024 20:07:53 +0200 Subject: [PATCH 015/137] Added MotorState and working on hardware interface --- .../include/motor_wrappers/MoteusWrapper.hpp | 4 +++- .../motor_wrappers/MotorWrapperBase.hpp | 17 +++++++++++++++-- .../pi3hat_hardware_interface.hpp | 14 ++++++-------- .../src/motor_wrappers/MoteusWrapper.cpp | 7 ++++--- .../src/pi3hat_hardware_interface.cpp | 18 +++++++++++++++--- 5 files changed, 43 insertions(+), 17 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp index 792dc4d2..49136a5e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp @@ -15,10 +15,12 @@ class MoteusWrapper: public MotorWrapperBase, protected mjbots::m /* Create Moteus Wrapper from existing frames */ MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, mjbots::pi3hat::CanFrame& tx_frame,mjbots::pi3hat::CanFrame& rx_frame, - mjbots::moteus::PositionMode::Command command); + MotorState& motor_command, MotorState& motor_state, + mjbots::moteus::PositionMode::Command command); /* Static override */ void make_position(double position, double velocity, double feedforward_torque); + void }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp index ca338908..85d20650 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp @@ -10,6 +10,14 @@ so remember to change instance of your derived class in pi3hat hardware interface files (only for creation of an object). */ + +struct MotorState +{ + double position_; + double velocity_; + double torque_; +}; + template class MotorWrapperBase { @@ -27,11 +35,16 @@ class MotorWrapperBase mjbots::pi3hat::CanFrame& tx_frame_; mjbots::pi3hat::CanFrame& rx_frame_; + /* Motor commands and states */ + MotorState& motor_command_; + MotorState& motor_state_; public: /* Constructor: takes CanFrame for later editing*/ - MotorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame): - tx_frame_(tx_frame), rx_frame_(rx_frame) {}; + MotorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, + MotorState& motor_command, MotorState& motor_state): + tx_frame_(tx_frame), rx_frame_(rx_frame), + motor_command_(motor_command), motor_state_(motor_state) {}; /* Static virtual method for preparing TX CAN frame */ void make_position(double position, double velocity, double feedforward_torque) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 5e5d7448..8bfac467 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -78,7 +78,7 @@ namespace pi3hat_hardware_interface std::array hw_state_imu_linear_acceleration_; // x, y, z /* Motor CAN config */ - std::vector hw_motor_can_channels_; + std::vector hw_motor_can_buses_; std::vector hw_motor_can_ids_; /* Motor parameters */ @@ -91,15 +91,13 @@ namespace pi3hat_hardware_interface std::vector hw_motor_effort_maxs_; /* Motor states */ - std::vector hw_motor_positions_; - std::vector hw_motor_velocities_; - std::vector hw_motor_torques_; + std::vector hw_states_; // Motor commands - std::vector hw_command_positions_; - std::vector hw_command_velocities_; - std::vector hw_command_torques_; - + std::vector hw_commands_; + + /* Motor Wrappers (here change to your own wrapper) */ + std::vector moteus_wrappers; }; } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp index b3060e24..39744756 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp @@ -7,9 +7,10 @@ */ MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, -mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, -mjbots::moteus::PositionMode::Command command): -MotorWrapperBase(tx_frame, rx_frame), mjbots::moteus::Controller(options), position_command_(command) + mjbots::pi3hat::CanFrame& tx_frame,mjbots::pi3hat::CanFrame& rx_frame, + MotorState& motor_command, MotorState& motor_state, + mjbots::moteus::PositionMode::Command command): +MotorWrapperBase(tx_frame, rx_frame, motor_command, motor_state), mjbots::moteus::Controller(options), position_command_(command) { /* Prepare CAN tx frame*/ MotorWrapperBase::tx_frame_.id = options.id; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index aa5f8137..f304e2c2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -9,6 +9,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa { return hardware_interface::CallbackReturn::ERROR; } + // POPRAW hw_motor_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_motor_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); hw_motor_torques_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); @@ -51,13 +52,24 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa pi3hat_input_.tx_can = tx_can_frames_span_; // Set up the CAN configuration - for (size_t i = 0; i < info_.joints.size(); i++) + for (size_t i = 0; i < info_.joints.size(); i++) // TUTAJ POMYSL O CO CMN { - config.can[hw_motor_can_channels_[i] - 1] = can_config; + config.can[hw_motor_can_buses_[i] - 1] = can_config; pi3hat_input_.tx_can[i].id = hw_motor_can_ids_[i]; - pi3hat_input_.tx_can[i].bus = hw_motor_can_channels_[i]; + pi3hat_input_.tx_can[i].bus = hw_motor_can_buses_[i]; pi3hat_input_.tx_can[i].expect_reply = true; } // Initialize the Pi3Hat pi3hat_ = std::make_shared(config); + + /* Create motor wrappers*/ + for(size_t i = 0; i < info_.joints.size(); i++) + { + auto options = mjbots::moteus::Controller::Options(); + options.id = hw_motor_can_ids_[i]; + options.bus = hw_motor_can_buses_[i]; + auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], + rx_can_frames_[i], mjbots::moteus::PositionMode::Command()); + moteus_wrappers.push_back(moteus_wrapper); + } } \ No newline at end of file From 0d5133994b6ba87da497b3a66d24d8b5cfa2a56e Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 8 Jul 2024 20:31:04 +0200 Subject: [PATCH 016/137] Making changes for MotorState struct --- .../include/motor_wrappers/MoteusWrapper.hpp | 9 ++++--- .../motor_wrappers/MotorWrapperBase.hpp | 9 +++++-- .../pi3hat_hardware_interface.hpp | 24 ++++++++++++++++--- .../src/motor_wrappers/MoteusWrapper.cpp | 9 ++++--- .../src/pi3hat_hardware_interface.cpp | 18 +++++++------- 5 files changed, 48 insertions(+), 21 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp index 49136a5e..964e7410 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp @@ -5,6 +5,8 @@ #include "moteus/moteus.h" #include "MotorWrapperBase.hpp" +namespace motor_wrappers +{ class MoteusWrapper: public MotorWrapperBase, protected mjbots::moteus::Controller { private: @@ -14,13 +16,14 @@ class MoteusWrapper: public MotorWrapperBase, protected mjbots::m public: /* Create Moteus Wrapper from existing frames */ MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, - mjbots::pi3hat::CanFrame& tx_frame,mjbots::pi3hat::CanFrame& rx_frame, + mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, MotorState& motor_command, MotorState& motor_state, mjbots::moteus::PositionMode::Command command); /* Static override */ - void make_position(double position, double velocity, double feedforward_torque); - void + void make_command(double position, double velocity, double feedforward_torque); + void get_state(); +}; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp index 85d20650..56da31a9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp @@ -10,7 +10,8 @@ so remember to change instance of your derived class in pi3hat hardware interface files (only for creation of an object). */ - +namespace motor_wrappers +{ struct MotorState { double position_; @@ -51,9 +52,13 @@ class MotorWrapperBase { derived().make_position(position, velocity, feedforward_torque); }; + void make_position(double position, double velocity, double feedforward_torque) + { + derived().make_position(position, velocity, feedforward_torque); //POPRAW + }; }; - +}; #endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 8bfac467..c0bab800 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -91,13 +91,31 @@ namespace pi3hat_hardware_interface std::vector hw_motor_effort_maxs_; /* Motor states */ - std::vector hw_states_; + std::vector hw_motor_states_; // Motor commands - std::vector hw_commands_; + std::vector hw_motor_commands_; /* Motor Wrappers (here change to your own wrapper) */ - std::vector moteus_wrappers; + std::vector moteus_wrappers; + + template + void make_commands(std::vector> motor_wrappers) + { + for(auto& motor_wrapper: motor_wrappers) + { + motor_wrapper.make_command(); + } + }; + + template + void get_states(std::vector> motor_wrappers) + { + for(auto& motor_wrapper: motor_wrappers) + { + motor_wrapper.get_state(); + } + } }; } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp index 39744756..677578c6 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp @@ -6,8 +6,11 @@ https://github.com/mjbots/moteus */ + +using namespace motor_wrappers; + MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, - mjbots::pi3hat::CanFrame& tx_frame,mjbots::pi3hat::CanFrame& rx_frame, + mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, MotorState& motor_command, MotorState& motor_state, mjbots::moteus::PositionMode::Command command): MotorWrapperBase(tx_frame, rx_frame, motor_command, motor_state), mjbots::moteus::Controller(options), position_command_(command) @@ -20,12 +23,12 @@ MotorWrapperBase(tx_frame, rx_frame, motor_command, motor_state), } -void MoteusWrapper::make_position(double position, double velocity, double feedforward_torque) +void MoteusWrapper::make_command(double position, double velocity, double feedforward_torque) { /* Change command values */ position_command_.position = position; position_command_.velocity = velocity; - position_command_.feedforward_torque = feedforward_torque; + position_command_.feedforward_torque = feedforward_torque; //POPRAW /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index f304e2c2..d7cf6737 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -9,18 +9,16 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa { return hardware_interface::CallbackReturn::ERROR; } - // POPRAW - hw_motor_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_motor_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_motor_torques_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + + hw_motor_commands_.resize(info_.joints.size(), MotorState{std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - hw_command_positions_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_command_velocities_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); - hw_command_torques_.resize(info_.joints.size(), std::numeric_limits::quiet_NaN()); + hw_motor_states_.resize(info_.joints.size(), MotorState{std::numeric_limits::quiet_NaN(), + std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); for (const hardware_interface::ComponentInfo &joint : info_.joints) { - hw_motor_can_channels_.push_back(std::stoi(joint.parameters.at("can_channel"))); + hw_motor_can_buses_.push_back(std::stoi(joint.parameters.at("can_channel"))); hw_motor_can_ids_.push_back(std::stoi(joint.parameters.at("can_id"))); hw_motor_position_mins_.push_back(std::stod(joint.parameters.at("position_min"))); hw_motor_position_maxs_.push_back(std::stod(joint.parameters.at("position_max"))); @@ -68,8 +66,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa auto options = mjbots::moteus::Controller::Options(); options.id = hw_motor_can_ids_[i]; options.bus = hw_motor_can_buses_[i]; - auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], - rx_can_frames_[i], mjbots::moteus::PositionMode::Command()); + auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], rx_can_frames_[i], + hw_motor_commands_[i], hw_motor_states_[i], mjbots::moteus::PositionMode::Command()); moteus_wrappers.push_back(moteus_wrapper); } } \ No newline at end of file From a4be754b3f89467dba5019b2c9785f836fd9be7f Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 9 Jul 2024 21:34:50 +0200 Subject: [PATCH 017/137] Name changes --- .../ActuatorWrapperBase.hpp} | 20 ++++---- .../MoteusWrapper.hpp | 8 ++-- .../JointActuatorTransform.hpp | 0 .../Transmissions.hpp | 0 .../pi3hat_hardware_interface.hpp | 46 +++++++++---------- .../MoteusWrapper.cpp | 16 +++---- .../JointActuatorTransform.cpp | 0 .../Transmissions.cpp | 0 .../src/pi3hat_hardware_interface.cpp | 36 +++++++-------- 9 files changed, 63 insertions(+), 63 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/include/{motor_wrappers/MotorWrapperBase.hpp => actuator_wrappers/ActuatorWrapperBase.hpp} (70%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{motor_wrappers => actuator_wrappers}/MoteusWrapper.hpp (73%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{joint_actuator_transform => old_code}/JointActuatorTransform.hpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{joint_actuator_transform => old_code}/Transmissions.hpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{motor_wrappers => actuator_wrappers}/MoteusWrapper.cpp (55%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{joint_actuator_transform => old_code}/JointActuatorTransform.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{joint_actuator_transform => old_code}/Transmissions.cpp (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp similarity index 70% rename from src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 56da31a9..a50ac06e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MotorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -5,14 +5,14 @@ /* - Base Motor Wrapper class, used for wrapping actuator API with simple CRTP interface + Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface to create CAN frames for pi3hat Input structure. Note that it uses static polymorphism, so remember to change instance of your derived class in pi3hat hardware interface files (only for creation of an object). */ -namespace motor_wrappers +namespace actuator_wrappers { -struct MotorState +struct ActuatorState { double position_; double velocity_; @@ -20,7 +20,7 @@ struct MotorState }; template -class MotorWrapperBase +class ActuatorWrapperBase { private: @@ -36,16 +36,16 @@ class MotorWrapperBase mjbots::pi3hat::CanFrame& tx_frame_; mjbots::pi3hat::CanFrame& rx_frame_; - /* Motor commands and states */ - MotorState& motor_command_; - MotorState& motor_state_; + /* Actuator commands and states */ + ActuatorState& actuator_command_; + ActuatorState& actuator_state_; public: /* Constructor: takes CanFrame for later editing*/ - MotorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - MotorState& motor_command, MotorState& motor_state): + ActuatorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, + ActuatorState& actuator_command, ActuatorState& actuator_state): tx_frame_(tx_frame), rx_frame_(rx_frame), - motor_command_(motor_command), motor_state_(motor_state) {}; + actuator_command_(actuator_command), actuator_state_(actuator_state) {}; /* Static virtual method for preparing TX CAN frame */ void make_position(double position, double velocity, double feedforward_torque) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp similarity index 73% rename from src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index 964e7410..15e29386 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/motor_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -3,11 +3,11 @@ #include "moteus/moteus.h" -#include "MotorWrapperBase.hpp" +#include "ActuatorWrapperBase.hpp" -namespace motor_wrappers +namespace actuator_wrappers { -class MoteusWrapper: public MotorWrapperBase, protected mjbots::moteus::Controller +class MoteusWrapper: public ActuatorWrapperBase, protected mjbots::moteus::Controller { private: /* Command structure for moteus object*/ @@ -17,7 +17,7 @@ class MoteusWrapper: public MotorWrapperBase, protected mjbots::m /* Create Moteus Wrapper from existing frames */ MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - MotorState& motor_command, MotorState& motor_state, + ActuatorState& motor_command, ActuatorState& motor_state, mjbots::moteus::PositionMode::Command command); /* Static override */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/JointActuatorTransform.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/joint_actuator_transform/Transmissions.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index c0bab800..da5d72eb 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -17,7 +17,7 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "motor_wrappers/MoteusWrapper.hpp" +#include "actuator_wrappers/MoteusWrapper.hpp" #include "pi3hat/pi3hat.h" #include "pi3hat/realtime.h" @@ -77,43 +77,43 @@ namespace pi3hat_hardware_interface std::array hw_state_imu_angular_velocity_; // x, y, z std::array hw_state_imu_linear_acceleration_; // x, y, z - /* Motor CAN config */ - std::vector hw_motor_can_buses_; - std::vector hw_motor_can_ids_; + /* Actuator CAN config */ + std::vector hw_actuator_can_buses_; + std::vector hw_actuator_can_ids_; - /* Motor parameters */ - std::vector hw_motor_position_offsets_; + /* Actuator parameters */ + std::vector hw_actuator_position_offsets_; - /* Motor limits */ - std::vector hw_motor_position_mins_; - std::vector hw_motor_position_maxs_; - std::vector hw_motor_velocity_maxs_; - std::vector hw_motor_effort_maxs_; + /* Actuator limits */ + std::vector hw_actuator_position_mins_; + std::vector hw_actuator_position_maxs_; + std::vector hw_actuator_velocity_maxs_; + std::vector hw_actuator_effort_maxs_; - /* Motor states */ - std::vector hw_motor_states_; + /* Actuator states */ + std::vector hw_actuator_states_; - // Motor commands - std::vector hw_motor_commands_; + // Actuator commands + std::vector hw_actuator_commands_; - /* Motor Wrappers (here change to your own wrapper) */ - std::vector moteus_wrappers; + /* Actuator Wrappers (here change to your own wrapper) */ + std::vector moteus_wrappers; template - void make_commands(std::vector> motor_wrappers) + void make_commands(std::vector> actuator_wrappers) { - for(auto& motor_wrapper: motor_wrappers) + for(auto& actuator_wrapper: actuator_wrappers) { - motor_wrapper.make_command(); + actuator_wrapper.make_command(); } }; template - void get_states(std::vector> motor_wrappers) + void get_states(std::vector> actuator_wrappers) { - for(auto& motor_wrapper: motor_wrappers) + for(auto& actuator_wrapper: actuator_wrappers) { - motor_wrapper.get_state(); + actuator_wrapper.get_state(); } } }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp similarity index 55% rename from src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 677578c6..4408e3a9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/motor_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -1,25 +1,25 @@ -#include "../include/motor_wrappers/MoteusWrapper.hpp" +#include "../include/actuator_wrappers/MoteusWrapper.hpp" /* - Moteus Motor Wrapper. It uses API from moteus repository to communicate with pi3hat interface. + Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. https://github.com/mjbots/moteus */ -using namespace motor_wrappers; +using namespace actuator_wrappers; MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - MotorState& motor_command, MotorState& motor_state, + ActuatorState& actuator_command, ActuatorState& actuator_state, mjbots::moteus::PositionMode::Command command): -MotorWrapperBase(tx_frame, rx_frame, motor_command, motor_state), mjbots::moteus::Controller(options), position_command_(command) +ActuatorWrapperBase(tx_frame, rx_frame, actuator_command, actuator_state), mjbots::moteus::Controller(options), position_command_(command) { /* Prepare CAN tx frame*/ - MotorWrapperBase::tx_frame_.id = options.id; - MotorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure + ActuatorWrapperBase::tx_frame_.id = options.id; + ActuatorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure - MotorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus + ActuatorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/JointActuatorTransform.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/Transmissions.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/src/joint_actuator_transform/Transmissions.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index d7cf6737..e2dbb56e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,7 +1,7 @@ #include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; - +using namespace actuator_wrappers; hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { @@ -9,22 +9,22 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa { return hardware_interface::CallbackReturn::ERROR; } - - hw_motor_commands_.resize(info_.joints.size(), MotorState{std::numeric_limits::quiet_NaN(), + + hw_actuator_commands_.resize(info_.joints.size(), ActuatorState{std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); - hw_motor_states_.resize(info_.joints.size(), MotorState{std::numeric_limits::quiet_NaN(), + hw_actuator_states_.resize(info_.joints.size(), ActuatorState{std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); for (const hardware_interface::ComponentInfo &joint : info_.joints) { - hw_motor_can_buses_.push_back(std::stoi(joint.parameters.at("can_channel"))); - hw_motor_can_ids_.push_back(std::stoi(joint.parameters.at("can_id"))); - hw_motor_position_mins_.push_back(std::stod(joint.parameters.at("position_min"))); - hw_motor_position_maxs_.push_back(std::stod(joint.parameters.at("position_max"))); - hw_motor_velocity_maxs_.push_back(std::stod(joint.parameters.at("velocity_max"))); - hw_motor_effort_maxs_.push_back(std::stod(joint.parameters.at("effort_max"))); - hw_motor_position_offsets_.push_back(std::stod(joint.parameters.at("position_offset"))); + hw_actuator_can_buses_.push_back(std::stoi(joint.parameters.at("can_channel"))); + hw_actuator_can_ids_.push_back(std::stoi(joint.parameters.at("can_id"))); + hw_actuator_position_mins_.push_back(std::stod(joint.parameters.at("position_min"))); + hw_actuator_position_maxs_.push_back(std::stod(joint.parameters.at("position_max"))); + hw_actuator_velocity_maxs_.push_back(std::stod(joint.parameters.at("velocity_max"))); + hw_actuator_effort_maxs_.push_back(std::stod(joint.parameters.at("effort_max"))); + hw_actuator_position_offsets_.push_back(std::stod(joint.parameters.at("position_offset"))); } /* Standard CAN config */ @@ -52,22 +52,22 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa // Set up the CAN configuration for (size_t i = 0; i < info_.joints.size(); i++) // TUTAJ POMYSL O CO CMN { - config.can[hw_motor_can_buses_[i] - 1] = can_config; - pi3hat_input_.tx_can[i].id = hw_motor_can_ids_[i]; - pi3hat_input_.tx_can[i].bus = hw_motor_can_buses_[i]; + config.can[hw_actuator_can_buses_[i] - 1] = can_config; + pi3hat_input_.tx_can[i].id = hw_actuator_can_ids_[i]; + pi3hat_input_.tx_can[i].bus = hw_actuator_can_buses_[i]; pi3hat_input_.tx_can[i].expect_reply = true; } // Initialize the Pi3Hat pi3hat_ = std::make_shared(config); - /* Create motor wrappers*/ + /* Create actuator wrappers*/ for(size_t i = 0; i < info_.joints.size(); i++) { auto options = mjbots::moteus::Controller::Options(); - options.id = hw_motor_can_ids_[i]; - options.bus = hw_motor_can_buses_[i]; + options.id = hw_actuator_can_ids_[i]; + options.bus = hw_actuator_can_buses_[i]; auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], rx_can_frames_[i], - hw_motor_commands_[i], hw_motor_states_[i], mjbots::moteus::PositionMode::Command()); + hw_actuator_commands_[i], hw_actuator_states_[i], mjbots::moteus::PositionMode::Command()); moteus_wrappers.push_back(moteus_wrapper); } } \ No newline at end of file From 3188ebc6a7ccac9de73f91b7c9c31b9d2d9e9cfd Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 10 Jul 2024 21:53:52 +0200 Subject: [PATCH 018/137] Changed interface for actuator wrappers --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 50 +++++++++++++------ .../actuator_wrappers/MoteusWrapper.hpp | 11 ++-- 2 files changed, 41 insertions(+), 20 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index a50ac06e..d7815812 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -2,7 +2,7 @@ #define _MOTOR_WRAPPER_BASE_ #include "pi3hat/pi3hat.h" - +#include /* Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface @@ -12,6 +12,14 @@ */ namespace actuator_wrappers { + +struct ActuatorCommand +{ + double position_; + double velocity_; + double torque_; +}; + struct ActuatorState { double position_; @@ -19,11 +27,22 @@ struct ActuatorState double torque_; }; +struct ActuatorParameters +{ + double position_max_; + double position_min_; + double velocity_max_; + double torque_max_; + int direction_; +}; + template class ActuatorWrapperBase { private: + ActuatorParameters params_; + /* Used for CRTP interface */ Derived& derived() { @@ -33,28 +52,29 @@ class ActuatorWrapperBase protected: /* pi3hat CAN frames */ - mjbots::pi3hat::CanFrame& tx_frame_; - mjbots::pi3hat::CanFrame& rx_frame_; + using CanFrame = mjbots::pi3hat::CanFrame; - /* Actuator commands and states */ - ActuatorState& actuator_command_; - ActuatorState& actuator_state_; public: /* Constructor: takes CanFrame for later editing*/ - ActuatorWrapperBase(mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - ActuatorState& actuator_command, ActuatorState& actuator_state): - tx_frame_(tx_frame), rx_frame_(rx_frame), - actuator_command_(actuator_command), actuator_state_(actuator_state) {}; + ActuatorWrapperBase(ActuatorParameters params): params_(params) {}; - /* Static virtual method for preparing TX CAN frame */ - void make_position(double position, double velocity, double feedforward_torque) + /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ + void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { - derived().make_position(position, velocity, feedforward_torque); + command.position_ = params_.direction_* (fmax(command.position_, params_.position_min), params_.position_max_); + command.velocity_ = params_.direction_* fmin(fmax(command.velocity_, -params_.velocity_max_), params_.velocity_max_); + command.torque_ = params_.direction_* fmin(fmax(command.torque_, -params_.torque_max_), params_.torque_max_); + + derived().make_position(tx_frame, command); }; - void make_position(double position, double velocity, double feedforward_torque) + /* Static virtual method for preparing ActuatorState form RX CAN frame */ + void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state) { - derived().make_position(position, velocity, feedforward_torque); //POPRAW + derived().make_position(rx_frame, state); + state.position_ = params_.direction_ * state.position_; + state.velocity_ = params_.direction_ * state.velocity_; + state.torque_ = params_.direction_ * state.torque_; }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index 15e29386..fc4afafe 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -12,17 +12,18 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots private: /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; + mjbots::moteus::PositionMode::Format potision_format_; public: /* Create Moteus Wrapper from existing frames */ - MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, - mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - ActuatorState& motor_command, ActuatorState& motor_state, + MoteusWrapper(ActuatorParameters params, + mjbots::moteus::Controller::Options& options, + mjbots::moteus::PositionMode::Format format, mjbots::moteus::PositionMode::Command command); /* Static override */ - void make_command(double position, double velocity, double feedforward_torque); - void get_state(); + void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command); + void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state); }; }; From 0ea879b51dfa8d0166208df1add985f033caa3c4 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 11 Jul 2024 19:53:06 +0200 Subject: [PATCH 019/137] Added reading state from RX CAN frame --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 2 +- .../actuator_wrappers/MoteusWrapper.hpp | 19 +++++--- .../pi3hat_hardware_interface.hpp | 2 +- .../src/actuator_wrappers/MoteusWrapper.cpp | 45 ++++++++++--------- 4 files changed, 40 insertions(+), 28 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index d7815812..0bba06d3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -57,7 +57,7 @@ class ActuatorWrapperBase public: /* Constructor: takes CanFrame for later editing*/ - ActuatorWrapperBase(ActuatorParameters params): params_(params) {}; + ActuatorWrapperBase(ActuatorParameters& params): params_(params) {}; /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index fc4afafe..4c15367d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -12,18 +12,25 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots private: /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; - mjbots::moteus::PositionMode::Format potision_format_; + mjbots::moteus::PositionMode::Format command_format_; + mjbots::moteus::Query::Format query_format_; + + protected: + // This is a member variable so we can avoid re-allocating it on + // every call. - From mjbots + std::vector single_command_replies_; public: /* Create Moteus Wrapper from existing frames */ - MoteusWrapper(ActuatorParameters params, + MoteusWrapper(ActuatorParameters& params, mjbots::moteus::Controller::Options& options, - mjbots::moteus::PositionMode::Format format, - mjbots::moteus::PositionMode::Command command); + mjbots::moteus::PositionMode::Format& command_format, + mjbots::moteus::Query::Format& query_format, + mjbots::moteus::PositionMode::Command& command); /* Static override */ - void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command); - void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state); + void command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command); + void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state); }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index da5d72eb..7391760d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -94,7 +94,7 @@ namespace pi3hat_hardware_interface std::vector hw_actuator_states_; // Actuator commands - std::vector hw_actuator_commands_; + std::vector hw_actuator_commands_; /* Actuator Wrappers (here change to your own wrapper) */ std::vector moteus_wrappers; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 4408e3a9..23d9f834 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -9,31 +9,36 @@ using namespace actuator_wrappers; -MoteusWrapper::MoteusWrapper(const mjbots::moteus::Controller::Options& options = {}, - mjbots::pi3hat::CanFrame& tx_frame, mjbots::pi3hat::CanFrame& rx_frame, - ActuatorState& actuator_command, ActuatorState& actuator_state, - mjbots::moteus::PositionMode::Command command): -ActuatorWrapperBase(tx_frame, rx_frame, actuator_command, actuator_state), mjbots::moteus::Controller(options), position_command_(command) -{ - /* Prepare CAN tx frame*/ - ActuatorWrapperBase::tx_frame_.id = options.id; - ActuatorWrapperBase::tx_frame_.bus = options.bus; // Copies values from options structure - - ActuatorWrapperBase::tx_frame_.expect_reply = true; // Expect reply from the same bus -} +MoteusWrapper::MoteusWrapper(ActuatorParameters& params, + mjbots::moteus::Controller::Options& options, + mjbots::moteus::PositionMode::Format& command_format, + mjbots::moteus::Query::Format& query_format, + mjbots::moteus::PositionMode::Command& command): +ActuatorWrapperBase(params), mjbots::moteus::Controller(options), +position_command_(command), command_format_(command_format), query_format_(query_format) { } -void MoteusWrapper::make_command(double position, double velocity, double feedforward_torque) +void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) { /* Change command values */ - position_command_.position = position; - position_command_.velocity = velocity; - position_command_.feedforward_torque = feedforward_torque; //POPRAW + position_command_.position = command.position_; + position_command_.velocity = command.velocity_; + position_command_.feedforward_torque = command.torque_; //POPRAW /* create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); + mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_, + &command_format_, &query_format_); /* Copy data from CANFD frame to CAN frame*/ - tx_frame_.size = can_fd_frame.size; - std::memcpy(tx_frame_.data, can_fd_frame.data, can_fd_frame.size); -} \ No newline at end of file + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} + + +void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) +{ + auto result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); + state.position_ = result.position; + state.velocity_ = result.velocity; + state.torque_ = result.torque; +} From 367376b7d77b61237b3be9443cfc0c27dce15c82 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 11 Jul 2024 22:09:45 +0200 Subject: [PATCH 020/137] Added init to actuator wrappers and edited pi3hat interface --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 7 ++++++ .../actuator_wrappers/MoteusWrapper.hpp | 10 +------- .../pi3hat_hardware_interface.hpp | 23 +++++++++++-------- .../src/actuator_wrappers/MoteusWrapper.cpp | 21 +++++++++++------ 4 files changed, 36 insertions(+), 25 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 0bba06d3..c2f7dad3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -59,6 +59,12 @@ class ActuatorWrapperBase /* Constructor: takes CanFrame for later editing*/ ActuatorWrapperBase(ActuatorParameters& params): params_(params) {}; + /* Static virtual method for starting actuators */ + void init(CanFrame& tx_frame) + { + derived().init(CanFrame& tx_frame); + }; + /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { @@ -68,6 +74,7 @@ class ActuatorWrapperBase derived().make_position(tx_frame, command); }; + /* Static virtual method for preparing ActuatorState form RX CAN frame */ void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state) { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index 4c15367d..d33ccd0e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -12,23 +12,15 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots private: /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; - mjbots::moteus::PositionMode::Format command_format_; - mjbots::moteus::Query::Format query_format_; - - protected: - // This is a member variable so we can avoid re-allocating it on - // every call. - From mjbots - std::vector single_command_replies_; public: /* Create Moteus Wrapper from existing frames */ MoteusWrapper(ActuatorParameters& params, mjbots::moteus::Controller::Options& options, - mjbots::moteus::PositionMode::Format& command_format, - mjbots::moteus::Query::Format& query_format, mjbots::moteus::PositionMode::Command& command); /* Static override */ + void init(CanFrame& tx_frame); void command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command); void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state); }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 7391760d..8a423ebe 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -65,6 +65,8 @@ namespace pi3hat_hardware_interface private: + size_t number_of_actuators; + std::shared_ptr pi3hat_; mjbots::pi3hat::Pi3Hat::Input pi3hat_input_; mjbots::pi3hat::Attitude attitude_; @@ -78,15 +80,15 @@ namespace pi3hat_hardware_interface std::array hw_state_imu_linear_acceleration_; // x, y, z /* Actuator CAN config */ - std::vector hw_actuator_can_buses_; + std::vector hw_actuator_can_buses_; std::vector hw_actuator_can_ids_; /* Actuator parameters */ - std::vector hw_actuator_position_offsets_; + std::vector hw_actuator_position_offsets_; // To wrzucimy do opcji /* Actuator limits */ std::vector hw_actuator_position_mins_; - std::vector hw_actuator_position_maxs_; + std::vector hw_actuator_position_maxs_; // To wrzucimy do opcji std::vector hw_actuator_velocity_maxs_; std::vector hw_actuator_effort_maxs_; @@ -102,18 +104,21 @@ namespace pi3hat_hardware_interface template void make_commands(std::vector> actuator_wrappers) { - for(auto& actuator_wrapper: actuator_wrappers) - { - actuator_wrapper.make_command(); + // TODO: Uporządkuj wcześniej silniki względem id + for(size_t i = 0; i < number_of_actuators; i++) + { + size_t actuator_id = tx_can_frames_[i].id; + actuator_wrappers[actuator_id].command_to_tx_frame(tx_can_frames_[i], hw_actuator_commands_[actuator_id]); } }; template void get_states(std::vector> actuator_wrappers) { - for(auto& actuator_wrapper: actuator_wrappers) - { - actuator_wrapper.get_state(); + for(size_t i = 0; i < number_of_actuators; i++) + { + size_t actuator_id = tx_can_frames_[i].id; + actuator_wrappers[actuator_id].rx_frame_to_state(rx_can_frames_[i], hw_actuator_states_[actuator_id]); } } }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 23d9f834..bb4c2f8a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -11,13 +11,21 @@ using namespace actuator_wrappers; MoteusWrapper::MoteusWrapper(ActuatorParameters& params, mjbots::moteus::Controller::Options& options, - mjbots::moteus::PositionMode::Format& command_format, - mjbots::moteus::Query::Format& query_format, mjbots::moteus::PositionMode::Command& command): ActuatorWrapperBase(params), mjbots::moteus::Controller(options), -position_command_(command), command_format_(command_format), query_format_(query_format) { } +position_command_(command){ } + +void MoteusWrapper::init(CanFrame& tx_frame) +{ + mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakeStop(); + + /* Copy data from CANFD frame to CAN frame*/ + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} + void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) { /* Change command values */ @@ -26,18 +34,17 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorComman position_command_.feedforward_torque = command.torque_; //POPRAW /* create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_, - &command_format_, &query_format_); + mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame*/ tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } - void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) { - auto result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); + /* Parse data from RX CAN frame to Result object */ + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position; state.velocity_ = result.velocity; state.torque_ = result.torque; From dc9cf42b802694e7293d5c7063fc5507bdb4a7b3 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 13:38:15 +0200 Subject: [PATCH 021/137] Added test for controlling 1 moteus actuator --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 6 +- .../actuator_wrappers/MoteusWrapper.hpp | 2 +- .../src/actuator_wrappers/MoteusWrapper.cpp | 2 +- .../src/main_test.cpp | 108 ++++++++++++++++++ 4 files changed, 113 insertions(+), 5 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index c2f7dad3..8baf3b86 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -1,7 +1,7 @@ #ifndef _MOTOR_WRAPPER_BASE_ #define _MOTOR_WRAPPER_BASE_ -#include "pi3hat/pi3hat.h" +#include "../pi3hat/pi3hat.h" #include /* @@ -62,13 +62,13 @@ class ActuatorWrapperBase /* Static virtual method for starting actuators */ void init(CanFrame& tx_frame) { - derived().init(CanFrame& tx_frame); + derived().init(tx_frame); }; /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { - command.position_ = params_.direction_* (fmax(command.position_, params_.position_min), params_.position_max_); + command.position_ = params_.direction_* (fmax(command.position_, params_.position_min_), params_.position_max_); command.velocity_ = params_.direction_* fmin(fmax(command.velocity_, -params_.velocity_max_), params_.velocity_max_); command.torque_ = params_.direction_* fmin(fmax(command.torque_, -params_.torque_max_), params_.torque_max_); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index d33ccd0e..23a79d7d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -2,7 +2,7 @@ #define _MOTEUS_ACTUATOR_WRAPPER_ -#include "moteus/moteus.h" +#include "../moteus/moteus.h" #include "ActuatorWrapperBase.hpp" namespace actuator_wrappers diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index bb4c2f8a..5c865110 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -1,4 +1,4 @@ -#include "../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../include/actuator_wrappers/MoteusWrapper.hpp" /* diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp new file mode 100644 index 00000000..3b9cd5e1 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -0,0 +1,108 @@ +#include "../include/actuator_wrappers/MoteusWrapper.hpp" + + + +#include "../include/pi3hat/pi3hat.h" +#include "../include/pi3hat/realtime.h" +#include +#include +#include + +static double GetNow() +{ + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; +}; + +int main(int argc, char argv[]) +{ + // moteus options + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = 0; + moteus_options.id = 1; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_command; + + + // pi3hat + mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; + pi3hat_configuration.attitude_rate_hz = 1000; + + mjbots::pi3hat::CanFrame tx_frame; + mjbots::pi3hat::CanFrame rx_frame; + + mjbots::pi3hat::Span tx_span(&tx_frame, 1); + mjbots::pi3hat::Span rx_span(&tx_frame, 1); + mjbots::pi3hat::Attitude attitude; + + mjbots::pi3hat::Pi3Hat::Input input; + + input.tx_can = tx_span; + input.rx_can = rx_span; + input.attitude = &attitude; + input.request_attitude = true; + + + mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + + // pi3hat output + mjbots::pi3hat::Pi3Hat::Output pi3hat_output; + + // moteus wrapper + actuator_wrappers::ActuatorParameters params; + params.direction_ = 1; + params.position_max_ = 6; + params.position_min_ = -6; + params.velocity_max_ = 5; + params.torque_max_ = 1; + + actuator_wrappers::MoteusWrapper moteus_wrapper(params, moteus_options, moteus_command); + actuator_wrappers::ActuatorCommand actuator_command; + actuator_command.velocity_ = 0; + actuator_command.torque_ = 0; + + actuator_wrappers::ActuatorState actuator_state; + + std::cout << "Options for controller succesfully initialized!" << std::endl; + + + mjbots::pi3hat::ConfigureRealtime(0); + std::cout << "Realtime control activated!" << std::endl; + + // set stop to moteus + moteus_wrapper.init(tx_frame); + pi3hat_output = pi3hat.Cycle(input); + std::cout << "Controller successfully started!" << std::endl; + + // Buffer for printing info + char buf[4096] = {}; + + auto prev = GetNow(); + double frequency; + while(true) + { + auto now = GetNow(); + actuator_command.position_ = 3 * sin(now - prev); + moteus_wrapper.command_to_tx_frame(tx_frame, actuator_command); + pi3hat_output = pi3hat.Cycle(input); + auto mesaure_time = GetNow() - now; + frequency = 1/mesaure_time; + moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); + ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f,%7.3f,%7.3f)", + frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); + ::printf("\r"); + ::fflush(::stdout); + } + + return 0; +} \ No newline at end of file From 7ad0254be837b4635d61be3d70d340a281e8e67a Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 15:48:15 +0200 Subject: [PATCH 022/137] Fixed small problems --- .../pi3hat_hardware_interface/src/main_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 3b9cd5e1..1c2cf93c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -16,7 +16,7 @@ static double GetNow() static_cast(ts.tv_nsec) / 1e9; }; -int main(int argc, char argv[]) +int main(int argc, char** argv) { // moteus options using mjbots::moteus::Controller; @@ -75,7 +75,7 @@ int main(int argc, char argv[]) std::cout << "Options for controller succesfully initialized!" << std::endl; - + mjbots::pi3hat::ConfigureRealtime(0); std::cout << "Realtime control activated!" << std::endl; @@ -98,7 +98,7 @@ int main(int argc, char argv[]) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); - ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f,%7.3f,%7.3f)", + ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); ::printf("\r"); ::fflush(::stdout); From 4f76197d461d184594affc955291cfea1870f525 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 16:07:21 +0200 Subject: [PATCH 023/137] Testing --- src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 1c2cf93c..1a9662d9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -52,7 +52,7 @@ int main(int argc, char** argv) input.attitude = &attitude; input.request_attitude = true; - + std::cout << "Options for controller succesfully initialized!" << std::endl; mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); // pi3hat output From d5339febc7d27bf9d6f1c359ce16ad315fabb33a Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 16:43:26 +0200 Subject: [PATCH 024/137] Changed bus --- src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 1a9662d9..bed06639 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -21,7 +21,7 @@ int main(int argc, char** argv) // moteus options using mjbots::moteus::Controller; Controller::Options moteus_options; - moteus_options.bus = 0; + moteus_options.bus = 1; moteus_options.id = 1; // moteus command format (it will be copied to wrapper) From 9b72ecfc62d962bc8204a29829a9edfdcb1ff85c Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 16:49:09 +0200 Subject: [PATCH 025/137] Testing --- src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index bed06639..8be9ee4f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -98,6 +98,7 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); + std::cout << "Jestem tutaj" << std::endl; ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); ::printf("\r"); From e40f57067af079dcfb0bfa9f72af35de891c19fc Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 17:25:52 +0200 Subject: [PATCH 026/137] Testing --- .../pi3hat_hardware_interface/src/main_test.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 8be9ee4f..6ef56172 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -40,7 +40,9 @@ int main(int argc, char** argv) mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; - + tx_frame.id = 1; // TO DODAJ DO WRAPPERA!! + tx_frame.bus = 1; + tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&tx_frame, 1); mjbots::pi3hat::Attitude attitude; From 21d7aded175a58bb2522e652de882815ce0c3a08 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 17:32:14 +0200 Subject: [PATCH 027/137] Testing --- .../src/actuator_wrappers/MoteusWrapper.cpp | 4 +++- .../pi3hat_hardware_interface/src/main_test.cpp | 14 ++++++++------ 2 files changed, 11 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 5c865110..3957e222 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -20,7 +20,9 @@ position_command_(command){ } void MoteusWrapper::init(CanFrame& tx_frame) { mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakeStop(); - + position_command_.position = 0; + position_command_.velocity = 1; // TEST + position_command_.feedforward_torque = 0; /* Copy data from CANFD frame to CAN frame*/ tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 6ef56172..94a7586c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -7,6 +7,7 @@ #include #include #include +#include static double GetNow() { @@ -63,9 +64,9 @@ int main(int argc, char** argv) // moteus wrapper actuator_wrappers::ActuatorParameters params; params.direction_ = 1; - params.position_max_ = 6; - params.position_min_ = -6; - params.velocity_max_ = 5; + params.position_max_ = 10; + params.position_min_ = -10; + params.velocity_max_ = 4; params.torque_max_ = 1; actuator_wrappers::MoteusWrapper moteus_wrapper(params, moteus_options, moteus_command); @@ -91,19 +92,20 @@ int main(int argc, char** argv) auto prev = GetNow(); double frequency; + std::string status_line; while(true) { auto now = GetNow(); - actuator_command.position_ = 3 * sin(now - prev); + actuator_command.position_ = 10 * sin(now - prev); moteus_wrapper.command_to_tx_frame(tx_frame, actuator_command); pi3hat_output = pi3hat.Cycle(input); auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); - std::cout << "Jestem tutaj" << std::endl; ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); - ::printf("\r"); + status_line += buf; + ::printf("%s \r", status_line.c_str()); ::fflush(::stdout); } From bdbf1ef838b4229f83cd3baf862411b74505e5f8 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 12 Jul 2024 17:36:25 +0200 Subject: [PATCH 028/137] Testing --- .../pi3hat_hardware_interface/src/main_test.cpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 94a7586c..17af1ec3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -45,7 +45,7 @@ int main(int argc, char** argv) tx_frame.bus = 1; tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); - mjbots::pi3hat::Span rx_span(&tx_frame, 1); + mjbots::pi3hat::Span rx_span(&rx_frame, 1); mjbots::pi3hat::Attitude attitude; mjbots::pi3hat::Pi3Hat::Input input; @@ -102,10 +102,10 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); - ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)", + ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f) ", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); status_line += buf; - ::printf("%s \r", status_line.c_str()); + ::printf("%s\n \r", status_line.c_str()); ::fflush(::stdout); } From c81c077e699dc51c7f624eb8eaad4dab73c460bd Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 13 Jul 2024 16:50:35 +0200 Subject: [PATCH 029/137] Testing --- .../pi3hat_hardware_interface/src/main_test.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 17af1ec3..be259791 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -38,12 +38,14 @@ int main(int argc, char** argv) // pi3hat mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; pi3hat_configuration.attitude_rate_hz = 1000; + pi3hat_configuration.can[0].automatic_retransmission = false; + pi3hat_configuration.can[0].fdcan_frame = false; + mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; tx_frame.id = 1; // TO DODAJ DO WRAPPERA!! tx_frame.bus = 1; - tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&rx_frame, 1); mjbots::pi3hat::Attitude attitude; @@ -96,7 +98,7 @@ int main(int argc, char** argv) while(true) { auto now = GetNow(); - actuator_command.position_ = 10 * sin(now - prev); + actuator_command.position_ = 5 * sin(now - prev); moteus_wrapper.command_to_tx_frame(tx_frame, actuator_command); pi3hat_output = pi3hat.Cycle(input); auto mesaure_time = GetNow() - now; From 4a03fa87a782e68286c497a0e4316569b3eca684 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 13 Jul 2024 18:49:39 +0200 Subject: [PATCH 030/137] Added new states for actuator wrapper and new function for creating wrappers --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 4 ++++ .../pi3hat_hardware_interface.hpp | 19 +++++++++++++++---- .../src/actuator_wrappers/MoteusWrapper.cpp | 7 ++++--- 3 files changed, 23 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 8baf3b86..d9764937 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -13,6 +13,7 @@ namespace actuator_wrappers { +/* Structure for basic actuator command */ struct ActuatorCommand { double position_; @@ -20,11 +21,14 @@ struct ActuatorCommand double torque_; }; +/* Structure for basic actuator state */ struct ActuatorState { double position_; double velocity_; double torque_; + int temperature_; + bool fault = false; }; struct ActuatorParameters diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 8a423ebe..10cb2453 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -95,14 +95,18 @@ namespace pi3hat_hardware_interface /* Actuator states */ std::vector hw_actuator_states_; - // Actuator commands + // Actuator commands std::vector hw_actuator_commands_; /* Actuator Wrappers (here change to your own wrapper) */ std::vector moteus_wrappers; + /* Function for creating moteus wrappers (here u can add your own wrapper) + Remember to change this function in source code */ + void add_actuator_wrapper(const hardware_interface::HardwareInfo &info, const WrapperType type); + template - void make_commands(std::vector> actuator_wrappers) + void commands_to_tx_frames(std::vector> actuator_wrappers) { // TODO: Uporządkuj wcześniej silniki względem id for(size_t i = 0; i < number_of_actuators; i++) @@ -113,7 +117,7 @@ namespace pi3hat_hardware_interface }; template - void get_states(std::vector> actuator_wrappers) + void rx_frames_to_states(std::vector> actuator_wrappers) { for(size_t i = 0; i < number_of_actuators; i++) { @@ -123,6 +127,13 @@ namespace pi3hat_hardware_interface } }; -} +/* Here add your actuator wrapper type */ +enum WrapperType +{ + Moteus = 0, + +}; + +}; #endif diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 3957e222..b37ea52e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -19,10 +19,9 @@ position_command_(command){ } void MoteusWrapper::init(CanFrame& tx_frame) { + /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakeStop(); - position_command_.position = 0; - position_command_.velocity = 1; // TEST - position_command_.feedforward_torque = 0; + /* Copy data from CANFD frame to CAN frame*/ tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); @@ -50,4 +49,6 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& s state.position_ = result.position; state.velocity_ = result.velocity; state.torque_ = result.torque; + state.temperature_ = result.temperature; + state.fault = result.fault; } From 60674b704f59fce909949ea63267d2ddad183ad1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 14 Jul 2024 18:05:39 +0200 Subject: [PATCH 031/137] Added IMUTransform object --- .../include/imu/IMUTransform.hpp | 21 +++++++++++ .../src/imu/IMUTransform.cpp | 36 +++++++++++++++++++ 2 files changed, 57 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp new file mode 100644 index 00000000..e9760231 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp @@ -0,0 +1,21 @@ +#ifndef _IMU_ +#define _IMU_ + +#include +#include "../pi3hat/pi3hat.h" +#include + +namespace IMU +{ + class IMUTransform + { + public: + + /* Function for transforming transforming IMU data */ + void transform_attitude(mjbots::pi3hat::Attitude& attitude); + }; + +}; + + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp new file mode 100644 index 00000000..a3ad08a5 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp @@ -0,0 +1,36 @@ +#include "../../include/imu/IMUTransform.hpp" + + +using namespace IMU; + +void IMUTransform::transform_attitude(mjbots::pi3hat::Attitude& attitude) +{ + /* Comment taken from: https://github.com/G-Levine/pi3hat_hardware_interface/blob/main/src/pi3hat_hardware_interface.cpp */ + /* + The quaternion returned by the pi3hat is the rotation from the gravity frame to the IMU frame. + Gravity frame: + +x and +y are parallel to the ground + +z points towards the ground + IMU frame: + +x points towards the side of the Pi with the USB-C port + +y points towards the side of the Pi with the USB-A ports + +z points up from the Pi + However, we want the rotation from the world frame to the IMU frame. + World frame: + +x and +y are parallel to the ground + +z points towards the sky + Therefore, we need to rotate the quaternion returned by the pi3hat by 180 degrees about the x-axis or y-axis. We choose to rotate about the x-axis. + Let the quaternion returned by the pi3hat be (x, y, z, w). + After applying a 180-degree rotation about the x-axis, the new quaternion is: + (w, -z, y, -x) + */ + + double temp_x = attitude.attitude.x; + double temp_y = attitude.attitude.y; + + attitude.attitude.x = attitude.attitude.w; + attitude.attitude.w = -temp_x; + + attitude.attitude.y = -attitude.attitude.z; + attitude.attitude.z = temp_y; +} \ No newline at end of file From 0cb51f63c3dba139535eb2c98e93a92371d7b203 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 14 Jul 2024 18:05:59 +0200 Subject: [PATCH 032/137] Modified wrappers --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 19 ++++++++++++------- .../actuator_wrappers/MoteusWrapper.hpp | 5 +++++ .../src/actuator_wrappers/MoteusWrapper.cpp | 13 ++++++------- 3 files changed, 23 insertions(+), 14 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index d9764937..96bb4643 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -16,23 +16,25 @@ namespace actuator_wrappers /* Structure for basic actuator command */ struct ActuatorCommand { - double position_; - double velocity_; - double torque_; + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ }; /* Structure for basic actuator state */ struct ActuatorState { - double position_; - double velocity_; - double torque_; - int temperature_; + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ + int temperature_; /* [Celcius] */ bool fault = false; }; struct ActuatorParameters { + int id; + int bus; double position_max_; double position_min_; double velocity_max_; @@ -72,6 +74,8 @@ class ActuatorWrapperBase /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { + tx_frame.id = params_.id; + tx_frame.bus = params_.bus; command.position_ = params_.direction_* (fmax(command.position_, params_.position_min_), params_.position_max_); command.velocity_ = params_.direction_* fmin(fmax(command.velocity_, -params_.velocity_max_), params_.velocity_max_); command.torque_ = params_.direction_* fmin(fmax(command.torque_, -params_.torque_max_), params_.torque_max_); @@ -82,6 +86,7 @@ class ActuatorWrapperBase /* Static virtual method for preparing ActuatorState form RX CAN frame */ void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state) { + if(rx_frame.id != params_.id) return; /* This should not happen! (map frame to wrapper first) */ derived().make_position(rx_frame, state); state.position_ = params_.direction_ * state.position_; state.velocity_ = params_.direction_ * state.velocity_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index 23a79d7d..c3808040 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -10,6 +10,11 @@ namespace actuator_wrappers class MoteusWrapper: public ActuatorWrapperBase, protected mjbots::moteus::Controller { private: + + /* Const coefficients for easy radians - rotations transform */ + const double rotation_to_radians = 2 * M_PI; + const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index b37ea52e..b82a8405 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -1,6 +1,5 @@ #include "../../include/actuator_wrappers/MoteusWrapper.hpp" - /* Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. https://github.com/mjbots/moteus @@ -30,11 +29,11 @@ void MoteusWrapper::init(CanFrame& tx_frame) void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) { /* Change command values */ - position_command_.position = command.position_; - position_command_.velocity = command.velocity_; - position_command_.feedforward_torque = command.torque_; //POPRAW + position_command_.position = command.position_ * radians_to_rotation; + position_command_.velocity = command.velocity_ * radians_to_rotation; + position_command_.feedforward_torque = command.torque_; - /* create CANFD frame*/ + /* Create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame*/ @@ -46,8 +45,8 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& s { /* Parse data from RX CAN frame to Result object */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); - state.position_ = result.position; - state.velocity_ = result.velocity; + state.position_ = result.position * rotation_to_radians; + state.velocity_ = result.velocity * rotation_to_radians; state.torque_ = result.torque; state.temperature_ = result.temperature; state.fault = result.fault; From 52ec0304377de1330d1b39e9f40d026da561223a Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 14 Jul 2024 18:06:27 +0200 Subject: [PATCH 033/137] Modified pi3hat hardware interface --- .../pi3hat_hardware_interface.hpp | 82 ++++++++++--------- 1 file changed, 45 insertions(+), 37 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 10cb2453..ca6473a0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -5,6 +5,7 @@ #include #include #include +#include #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -17,10 +18,10 @@ #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" -#include "actuator_wrappers/MoteusWrapper.hpp" - -#include "pi3hat/pi3hat.h" -#include "pi3hat/realtime.h" +#include "../actuator_wrappers/MoteusWrapper.hpp" +#include "../imu/IMUTransform.hpp" +#include "../pi3hat/pi3hat.h" +#include "../pi3hat/realtime.h" #include "visibility_control.hpp" @@ -64,75 +65,82 @@ namespace pi3hat_hardware_interface const rclcpp::Time &time, const rclcpp::Duration &period) override; private: + /* PART FOR COMMUNICATION WITH HARDWARE: */ + /* number of actuators */ size_t number_of_actuators; + /* Pi3hat */ std::shared_ptr pi3hat_; - mjbots::pi3hat::Pi3Hat::Input pi3hat_input_; - mjbots::pi3hat::Attitude attitude_; - std::shared_ptr tx_can_frames_; //Pomysl jeszcze nad tym! - std::shared_ptr rx_can_frames_; + /* Pi3hat input structure */ + mjbots::pi3hat::Pi3Hat::Input pi3hat_input_; /* IMU states */ - std::array hw_state_imu_orientation_; // x, y, z, w - std::array hw_state_imu_angular_velocity_; // x, y, z - std::array hw_state_imu_linear_acceleration_; // x, y, z + mjbots::pi3hat::Attitude attitude_; - /* Actuator CAN config */ - std::vector hw_actuator_can_buses_; - std::vector hw_actuator_can_ids_; + /* IMU transform */ + IMU::IMUTransform imu_transform_; - /* Actuator parameters */ - std::vector hw_actuator_position_offsets_; // To wrzucimy do opcji + /* TX CAN frames */ + std::shared_ptr tx_can_frames_; - /* Actuator limits */ - std::vector hw_actuator_position_mins_; - std::vector hw_actuator_position_maxs_; // To wrzucimy do opcji - std::vector hw_actuator_velocity_maxs_; - std::vector hw_actuator_effort_maxs_; + /* RX CAN frames */ + std::shared_ptr rx_can_frames_; - /* Actuator states */ - std::vector hw_actuator_states_; + /* Container for motor_id -> joint_index maping */ + std::map actuator_joint_map_; - // Actuator commands + /* Actuator states and commands */ + std::vector hw_actuator_states_; std::vector hw_actuator_commands_; - /* Actuator Wrappers (here change to your own wrapper) */ + /* Actuator Wrappers (HERE change to your own wrapper) */ std::vector moteus_wrappers; + + using JointState = actuator_wrappers::ActuatorState; + using JointCommand = actuator_wrappers::ActuatorCommand; + + /* Joint states and commands (for transmissions)*/ + std::vector hw_joint_states_; + std::vector hw_joint_commands_; + /* Function for creating moteus wrappers (here u can add your own wrapper) Remember to change this function in source code */ void add_actuator_wrapper(const hardware_interface::HardwareInfo &info, const WrapperType type); - + template void commands_to_tx_frames(std::vector> actuator_wrappers) { // TODO: Uporządkuj wcześniej silniki względem id - for(size_t i = 0; i < number_of_actuators; i++) + for(int i = 0; i < number_of_actuators; i++) { - size_t actuator_id = tx_can_frames_[i].id; - actuator_wrappers[actuator_id].command_to_tx_frame(tx_can_frames_[i], hw_actuator_commands_[actuator_id]); + actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], hw_actuator_commands_[i]); } }; template void rx_frames_to_states(std::vector> actuator_wrappers) { - for(size_t i = 0; i < number_of_actuators; i++) + for(int i = 0; i < number_of_actuators; i++) { - size_t actuator_id = tx_can_frames_[i].id; - actuator_wrappers[actuator_id].rx_frame_to_state(rx_can_frames_[i], hw_actuator_states_[actuator_id]); + int joint_index = actuator_joint_map_[rx_can_frames_[i].id]; + actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], hw_actuator_states_[joint_index]); } } + + /* PART FOR CREATING TRANSMISSION OBJECTS:*/ + }; -/* Here add your actuator wrapper type */ -enum WrapperType -{ - Moteus = 0, -}; + /* Here add your actuator wrapper type */ + enum WrapperType + { + Moteus = 0, + + }; }; From ebad6185d139c93be426a0856e897c8345e9964d Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 15 Jul 2024 22:17:04 +0200 Subject: [PATCH 034/137] Added interface for creating transmissions --- .../pi3hat_hardware_interface.hpp | 45 ++++++++++++++++--- 1 file changed, 38 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index ca6473a0..125f1671 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -6,18 +6,31 @@ #include #include #include +#include -#include "hardware_interface/handle.hpp" -#include "hardware_interface/hardware_info.hpp" -#include "hardware_interface/system_interface.hpp" -#include "hardware_interface/types/hardware_interface_return_values.hpp" #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" +#include "rclcpp/logger.hpp" + +#include "hardware_interface/handle.hpp" +#include "hardware_interface/hardware_info.hpp" +#include "hardware_interface/system_interface.hpp" +#include "hardware_interface/types/hardware_interface_return_values.hpp" + #include "rclcpp_lifecycle/node_interfaces/lifecycle_node_interface.hpp" #include "rclcpp_lifecycle/state.hpp" +#include "transmission_interface/transmission.hpp" +#include "transmission_interface/simple_transmission.hpp" +#include "transmission_interface/four_bar_linkage_transmission.hpp" +#include "transmission_interface/differential_transmission.hpp" +#include "transmission_interface/simple_transmission_loader.hpp" +#include "transmission_interface/four_bar_linkage_transmission_loader.hpp" +#include "transmission_interface/differential_transmission_loader.hpp" +#include "transmission_interface/transmission_interface_exception.hpp" + #include "../actuator_wrappers/MoteusWrapper.hpp" #include "../imu/IMUTransform.hpp" #include "../pi3hat/pi3hat.h" @@ -25,8 +38,6 @@ #include "visibility_control.hpp" -#include - namespace pi3hat_hardware_interface { class Pi3HatHardwareInterface : public hardware_interface::SystemInterface @@ -65,6 +76,9 @@ namespace pi3hat_hardware_interface const rclcpp::Time &time, const rclcpp::Duration &period) override; private: + /* TYPICAL ROS2 OBJECTS: */ + std::unique_ptr logger_; + /* PART FOR COMMUNICATION WITH HARDWARE: */ /* number of actuators */ @@ -131,7 +145,24 @@ namespace pi3hat_hardware_interface } /* PART FOR CREATING TRANSMISSION OBJECTS:*/ - + + /* Transmission interfaces*/ + std::vector> transmissions_; + + /* Function for creating all transmissions */ + void create_transmission_interface(const hardware_interface::HardwareInfo &info); + + /* Functions for creating simple transmission */ + void create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::SimpleTransmissionLoader& loader); + + /* Functions for creating four bar linkage transmission */ + void create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::FourBarLinkageTransmissionLoader& loader); + + /* Functions for creating differential transmission */ + void create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::DifferentialTransmissionLoader& loader); }; From 63ce53fc00e236977f008e6f9243c14a045acc3d Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 16 Jul 2024 19:08:05 +0200 Subject: [PATCH 035/137] Added simple example --- .../src/simple_test.cpp | 80 +++++++++++++++++++ 1 file changed, 80 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp new file mode 100644 index 00000000..4b44a63c --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -0,0 +1,80 @@ +#include "../include/pi3hat/pi3hat.h" +#include "../include/pi3hat/realtime.h" +#include "../include/moteus/moteus.h" + + +static double GetNow() +{ + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; +}; + +int main(int argc, char** argv) +{ + // moteus part + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = 1; + moteus_options.id = 1; + Controller moteus_controller(moteus_options); + + mjbots::moteus::PositionMode::Command moteus_command; + + //pi3hat part + + mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; + pi3hat_configuration.attitude_rate_hz = 1000; + pi3hat_configuration.can[0].automatic_retransmission = false; + pi3hat_configuration.can[0].fdcan_frame = false; + + mjbots::pi3hat::CanFrame tx_frame; + mjbots::pi3hat::CanFrame rx_frame; + tx_frame.id = 1; + tx_frame.bus = 1; + mjbots::pi3hat::Span tx_span(&tx_frame, 1); + mjbots::pi3hat::Span rx_span(&rx_frame, 1); + mjbots::pi3hat::Attitude attitude; + + mjbots::pi3hat::Pi3Hat::Input input; + mjbots::pi3hat::Pi3Hat::Output pi3hat_output; + input.tx_can = tx_span; + input.rx_can = rx_span; + input.attitude = &attitude; + input.request_attitude = true; + + mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + + // Buffer for printing info + char buf[4096] = {}; + + auto prev = GetNow(); + double frequency; + std::string status_line; + while(true) + { + auto now = GetNow(); + double position_ = 5 * sin(now - prev); + moteus_command.position = position_; + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakePosition(moteus_command); + + /* Copy data from CANFD frame to CAN frame*/ + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + + + auto mesaure_time = GetNow() - now; + frequency = 1/mesaure_time; + pi3hat_output = pi3hat.Cycle(input); + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); + double state_position = result.position; + ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f) ", + frequency, state_position); + status_line += buf; + ::printf("%s\n \r", status_line.c_str()); + ::fflush(::stdout); + + }; + return 0; +} From c027773740a84971431ad3a0d6dc4a33f5c1b82d Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 16 Jul 2024 19:14:27 +0200 Subject: [PATCH 036/137] Quick update --- .../pi3hat_hardware_interface/src/simple_test.cpp | 14 +++++--------- 1 file changed, 5 insertions(+), 9 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 4b44a63c..df6a84d3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -17,7 +17,7 @@ int main(int argc, char** argv) using mjbots::moteus::Controller; Controller::Options moteus_options; moteus_options.bus = 1; - moteus_options.id = 1; + moteus_options.id = 2; Controller moteus_controller(moteus_options); mjbots::moteus::PositionMode::Command moteus_command; @@ -26,13 +26,12 @@ int main(int argc, char** argv) mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; pi3hat_configuration.attitude_rate_hz = 1000; - pi3hat_configuration.can[0].automatic_retransmission = false; - pi3hat_configuration.can[0].fdcan_frame = false; mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; tx_frame.id = 1; - tx_frame.bus = 1; + tx_frame.bus = 2; + tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&rx_frame, 1); mjbots::pi3hat::Attitude attitude; @@ -69,11 +68,8 @@ int main(int argc, char** argv) pi3hat_output = pi3hat.Cycle(input); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); double state_position = result.position; - ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f) ", - frequency, state_position); - status_line += buf; - ::printf("%s\n \r", status_line.c_str()); - ::fflush(::stdout); + std::cout << "Attitude?: " << pi3hat_output.attitude_present << std::endl; + std::cout << "Data size?: " << pi3hat_output.rx_can_size << std::endl; }; return 0; From 1eee04049a9db5e96c271034dfb803f3d79c09f7 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 17 Jul 2024 18:18:06 +0200 Subject: [PATCH 037/137] Starting making transmission interface --- .../pi3hat_hardware_interface.hpp | 11 +++--- .../src/pi3hat_hardware_interface.cpp | 36 +++++++++++++++++++ 2 files changed, 42 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 125f1671..2684902a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -13,6 +13,7 @@ #include "rclcpp/macros.hpp" #include "rclcpp/time.hpp" #include "rclcpp/logger.hpp" +#include "rclcpp/logging.hpp" #include "hardware_interface/handle.hpp" #include "hardware_interface/hardware_info.hpp" @@ -103,7 +104,7 @@ namespace pi3hat_hardware_interface std::shared_ptr rx_can_frames_; /* Container for motor_id -> joint_index maping */ - std::map actuator_joint_map_; + std::vector actuator_joint_map_; /* Actuator states and commands */ std::vector hw_actuator_states_; @@ -150,18 +151,18 @@ namespace pi3hat_hardware_interface std::vector> transmissions_; /* Function for creating all transmissions */ - void create_transmission_interface(const hardware_interface::HardwareInfo &info); + hardware_interface::CallbackReturn create_transmission_interface(const hardware_interface::HardwareInfo &info); /* Functions for creating simple transmission */ - void create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::SimpleTransmissionLoader& loader); /* Functions for creating four bar linkage transmission */ - void create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_fbl_transmissions(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader); /* Functions for creating differential transmission */ - void create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_diff_transmissions(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::DifferentialTransmissionLoader& loader); }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index e2dbb56e..d49bb50d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -9,6 +9,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_unique( + rclcpp::get_logger("Pi3HatHardwareInterface")); hw_actuator_commands_.resize(info_.joints.size(), ActuatorState{std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); @@ -70,4 +72,38 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa hw_actuator_commands_[i], hw_actuator_states_[i], mjbots::moteus::PositionMode::Command()); moteus_wrappers.push_back(moteus_wrapper); } +} + + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::SimpleTransmissionLoader& loader) +{ + for (const auto & transmission_info : info_.transmissions) + { + if (transmission_info.type != "transmission_interface/SimpleTransmission") + { + continue; + } + std::shared_ptr transmission; + try + { + transmission = loader.load(transmission_info); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + std::vector joint_handles; + std::vector actuator_handles; + + if(transmission_info.joints.size() != 1) + { + RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; + } + transmission_interface::JointHandle joint_handle(transmission_info.joints[0].name, + hardware_interface::HW_IF_POSITION, ); //pomysl jak tu dodac position odpowiedniego jointa! + joint_handles.push_back() + } } \ No newline at end of file From 7f3ab3e6dcc9bba396d10a489ebc9536d58c46a3 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 18 Jul 2024 20:51:24 +0200 Subject: [PATCH 038/137] Define function for SimpleTransmission and utility transmission functions --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 12 +-- .../pi3hat_hardware_interface.hpp | 23 +++-- .../src/pi3hat_hardware_interface.cpp | 84 +++++++++++++++---- 3 files changed, 94 insertions(+), 25 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 96bb4643..c27429e5 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -2,13 +2,13 @@ #define _MOTOR_WRAPPER_BASE_ #include "../pi3hat/pi3hat.h" -#include - +#include /* Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface to create CAN frames for pi3hat Input structure. Note that it uses static polymorphism, so remember to change instance of your derived class in pi3hat hardware interface files - (only for creation of an object). + (only for creation of an object). Converting values (like rotations to radians) should + be done in your wrapper. */ namespace actuator_wrappers { @@ -76,9 +76,9 @@ class ActuatorWrapperBase { tx_frame.id = params_.id; tx_frame.bus = params_.bus; - command.position_ = params_.direction_* (fmax(command.position_, params_.position_min_), params_.position_max_); - command.velocity_ = params_.direction_* fmin(fmax(command.velocity_, -params_.velocity_max_), params_.velocity_max_); - command.torque_ = params_.direction_* fmin(fmax(command.torque_, -params_.torque_max_), params_.torque_max_); + command.position_ = params_.direction_* std::clamp(command.position_, params_.position_min_, params_.position_max_); + command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); + command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); derived().make_position(tx_frame, command); }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 2684902a..8080604c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -7,6 +7,7 @@ #include #include #include +#include #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -77,7 +78,7 @@ namespace pi3hat_hardware_interface const rclcpp::Time &time, const rclcpp::Duration &period) override; private: - /* TYPICAL ROS2 OBJECTS: */ + /* UTILITY ROS2 OBJECTS: */ std::unique_ptr logger_; /* PART FOR COMMUNICATION WITH HARDWARE: */ @@ -110,6 +111,9 @@ namespace pi3hat_hardware_interface std::vector hw_actuator_states_; std::vector hw_actuator_commands_; + /* For transmission interface */ + std::vector hw_actuator_transmission_passthrough_; + /* Actuator Wrappers (HERE change to your own wrapper) */ std::vector moteus_wrappers; @@ -120,6 +124,8 @@ namespace pi3hat_hardware_interface /* Joint states and commands (for transmissions)*/ std::vector hw_joint_states_; std::vector hw_joint_commands_; + /* For transmission interface */ + std::vector hw_joint_transmission_passthrough_; /* Function for creating moteus wrappers (here u can add your own wrapper) Remember to change this function in source code */ @@ -155,17 +161,24 @@ namespace pi3hat_hardware_interface /* Functions for creating simple transmission */ hardware_interface::CallbackReturn create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::SimpleTransmissionLoader& loader); + transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating four bar linkage transmission */ hardware_interface::CallbackReturn create_fbl_transmissions(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::FourBarLinkageTransmissionLoader& loader); + transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating differential transmission */ hardware_interface::CallbackReturn create_diff_transmissions(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::DifferentialTransmissionLoader& loader); - }; + transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names); + + /* Functions for checking, if data passed from urdf is correct */ + void load_transmission_data(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::TransmissionSharedPtr& transmission, + transmission_interface::TransmissionLoader& loader); + /* Functions for creating joint and actuator handels */ + void append_joint_handels(std::vector& joint_handles, std::string joint_name, int joint_index); + void append_actuator_handels(std::vector& actuator_handles, std::string actuator_name, int actuator_index); /* Here add your actuator wrapper type */ enum WrapperType diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index d49bb50d..507edb24 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -74,26 +74,71 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa } } +void Pi3HatHardwareInterface::append_joint_handels(std::vector& joint_handles, std::string joint_name, int joint_index) +{ + transmission_interface::JointHandle joint_handle_position(joint_name, hardware_interface::HW_IF_POSITION, + &hw_joint_transmission_passthrough_[joint_index].position_); + joint_handles.push_back(joint_handle_position); + + transmission_interface::JointHandle joint_handle_velocity(joint_name, hardware_interface::HW_IF_VELOCITY, + &hw_joint_transmission_passthrough_[joint_index].velocity_); + joint_handles.push_back(joint_handle_velocity); + + transmission_interface::JointHandle joint_handle_torque(joint_name, hardware_interface::HW_IF_EFFORT, + &hw_joint_transmission_passthrough_[joint_index].torque_); + joint_handles.push_back(joint_handle_torque); +} +void Pi3HatHardwareInterface::append_actuator_handels(std::vector& actuator_handles, std::string actuator_name, int actuator_index) +{ + transmission_interface::ActuatorHandle actuator_handle_position(actuator_name, hardware_interface::HW_IF_POSITION, + &hw_actuator_transmission_passthrough_[actuator_index].position_); + actuator_handles.push_back(actuator_handle_position); + + transmission_interface::ActuatorHandle actuator_handle_velocity(actuator_name, hardware_interface::HW_IF_VELOCITY, + &hw_actuator_transmission_passthrough_[actuator_index].velocity_); + actuator_handles.push_back(actuator_handle_velocity); + + transmission_interface::ActuatorHandle actuator_handle_torque(actuator_name, hardware_interface::HW_IF_EFFORT, + &hw_actuator_transmission_passthrough_[actuator_index].torque_); + actuator_handles.push_back(actuator_handle_torque); +} + + +void Pi3HatHardwareInterface::load_transmission_data(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::TransmissionSharedPtr& transmission, transmission_interface::TransmissionLoader& loader) +{ + try + { + transmission = loader.load(transmission_info); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return; + } +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) +{ + + return hardware_interface::CallbackReturn::SUCCESS; +} hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::SimpleTransmissionLoader& loader) + transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names) { + RCLCPP_INFO(*logger_, "Simple transmissions initialization starting!"); for (const auto & transmission_info : info_.transmissions) { + std::shared_ptr transmission; + if (transmission_info.type != "transmission_interface/SimpleTransmission") { continue; } - std::shared_ptr transmission; - try - { - transmission = loader.load(transmission_info); - } - catch (const transmission_interface::TransmissionInterfaceException & exc) - { - RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - return hardware_interface::CallbackReturn::ERROR; - } + + load_transmission_data(transmission_info, transmission, loader); + std::vector joint_handles; std::vector actuator_handles; @@ -102,8 +147,19 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); return hardware_interface::CallbackReturn::ERROR; } - transmission_interface::JointHandle joint_handle(transmission_info.joints[0].name, - hardware_interface::HW_IF_POSITION, ); //pomysl jak tu dodac position odpowiedniego jointa! - joint_handles.push_back() + + /* Find joint index for transmission handels by name*/ + std::vector::const_iterator joint_it = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[0].name); + int joint_index = std::distance(joint_names.begin(), joint_it); + + /* Joint handels */ + + /* Actuator handels */ + + transmission->configure(joint_handles, actuator_handles); } + RCLCPP_INFO(*logger_, "Simple transmissions initialized!"); + + return hardware_interface::CallbackReturn::SUCCESS; } \ No newline at end of file From 5698465200d088d516f751f4928aaea6c07c7ab8 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 14:54:16 +0200 Subject: [PATCH 039/137] Testing --- .../pi3hat_hardware_interface/src/simple_test.cpp | 15 ++++++++++----- 1 file changed, 10 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index df6a84d3..a194907e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -17,7 +17,7 @@ int main(int argc, char** argv) using mjbots::moteus::Controller; Controller::Options moteus_options; moteus_options.bus = 1; - moteus_options.id = 2; + moteus_options.id = 1; Controller moteus_controller(moteus_options); mjbots::moteus::PositionMode::Command moteus_command; @@ -28,12 +28,12 @@ int main(int argc, char** argv) pi3hat_configuration.attitude_rate_hz = 1000; mjbots::pi3hat::CanFrame tx_frame; - mjbots::pi3hat::CanFrame rx_frame; + mjbots::pi3hat::CanFrame rx_frame[10]; tx_frame.id = 1; - tx_frame.bus = 2; + tx_frame.bus = 1; tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); - mjbots::pi3hat::Span rx_span(&rx_frame, 1); + mjbots::pi3hat::Span rx_span(rx_frame, 10); mjbots::pi3hat::Attitude attitude; mjbots::pi3hat::Pi3Hat::Input input; @@ -45,6 +45,11 @@ int main(int argc, char** argv) mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + pi3hat_output = pi3hat.Cycle(input); + std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; // Buffer for printing info char buf[4096] = {}; @@ -66,7 +71,7 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; pi3hat_output = pi3hat.Cycle(input); - mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; std::cout << "Attitude?: " << pi3hat_output.attitude_present << std::endl; std::cout << "Data size?: " << pi3hat_output.rx_can_size << std::endl; From f5701311f6365cd5f9f64be8f6e0996a9f336d0e Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 15:04:21 +0200 Subject: [PATCH 040/137] Testing --- .../pi3hat_hardware_interface/src/simple_test.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index a194907e..d9c5b128 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -43,12 +43,13 @@ int main(int argc, char** argv) input.attitude = &attitude; input.request_attitude = true; - mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + std::unique_ptr pi3hat = std::make_unique(pi3hat_configuration); mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - pi3hat_output = pi3hat.Cycle(input); + + pi3hat_output = pi3hat->Cycle(input); std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; // Buffer for printing info char buf[4096] = {}; @@ -70,7 +71,7 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; - pi3hat_output = pi3hat.Cycle(input); + pi3hat_output = pi3hat->Cycle(input); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; std::cout << "Attitude?: " << pi3hat_output.attitude_present << std::endl; From cdc77d8e93386eb49b8169c34175b4e0ae9d8bfc Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 15:07:21 +0200 Subject: [PATCH 041/137] Testing --- .../pi3hat_hardware_interface/src/simple_test.cpp | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index d9c5b128..7f9a5f38 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -45,12 +45,12 @@ int main(int argc, char** argv) std::unique_ptr pi3hat = std::make_unique(pi3hat_configuration); - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + // mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); + // tx_frame.size = can_fd_frame.size; + // std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - pi3hat_output = pi3hat->Cycle(input); - std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; + // pi3hat_output = pi3hat->Cycle(input); + // std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; // Buffer for printing info char buf[4096] = {}; From 3b72fdfdeba3993b587551f47b11e036bf42059c Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 15:22:05 +0200 Subject: [PATCH 042/137] Testing --- .../pi3hat_hardware_interface/src/simple_test.cpp | 2 ++ 1 file changed, 2 insertions(+) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 7f9a5f38..94787023 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -65,6 +65,8 @@ int main(int argc, char** argv) mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakePosition(moteus_command); /* Copy data from CANFD frame to CAN frame*/ + tx_frame.bus = can_fd_frame.bus; + tx_frame.id = can_fd_frame.arbitration_id; tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); From d53d71ce9a18e58170344d9257a722b9901c8a02 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 15:49:00 +0200 Subject: [PATCH 043/137] Fixed wrappers --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 6 ++---- .../src/actuator_wrappers/MoteusWrapper.cpp | 6 ++++++ .../pi3hat_hardware_interface/src/main_test.cpp | 13 ++++--------- .../pi3hat_hardware_interface/src/simple_test.cpp | 2 +- 4 files changed, 13 insertions(+), 14 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index c27429e5..df23a7ab 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -47,8 +47,6 @@ class ActuatorWrapperBase { private: - ActuatorParameters params_; - /* Used for CRTP interface */ Derived& derived() { @@ -56,6 +54,7 @@ class ActuatorWrapperBase }; protected: + ActuatorParameters params_; /* pi3hat CAN frames */ using CanFrame = mjbots::pi3hat::CanFrame; @@ -86,8 +85,7 @@ class ActuatorWrapperBase /* Static virtual method for preparing ActuatorState form RX CAN frame */ void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state) { - if(rx_frame.id != params_.id) return; /* This should not happen! (map frame to wrapper first) */ - derived().make_position(rx_frame, state); + derived().rx_frame_to_state(rx_frame, state); state.position_ = params_.direction_ * state.position_; state.velocity_ = params_.direction_ * state.velocity_; state.torque_ = params_.direction_ * state.torque_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index b82a8405..f15e2a6e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -22,6 +22,8 @@ void MoteusWrapper::init(CanFrame& tx_frame) mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakeStop(); /* Copy data from CANFD frame to CAN frame*/ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } @@ -37,6 +39,8 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorComman mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame*/ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } @@ -44,6 +48,8 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorComman void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) { /* Parse data from RX CAN frame to Result object */ + if(((rx_frame.id >> 8) & 0x7f) != params_.id) return; /* This should not happen! (map frame to wrapper first) */ + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians; state.velocity_ = result.velocity * rotation_to_radians; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index be259791..b3a66f28 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -44,8 +44,7 @@ int main(int argc, char** argv) mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; - tx_frame.id = 1; // TO DODAJ DO WRAPPERA!! - tx_frame.bus = 1; + mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&rx_frame, 1); mjbots::pi3hat::Attitude attitude; @@ -70,6 +69,8 @@ int main(int argc, char** argv) params.position_min_ = -10; params.velocity_max_ = 4; params.torque_max_ = 1; + params.bus = 1; + params.id = 1; actuator_wrappers::MoteusWrapper moteus_wrapper(params, moteus_options, moteus_command); actuator_wrappers::ActuatorCommand actuator_command; @@ -89,12 +90,8 @@ int main(int argc, char** argv) pi3hat_output = pi3hat.Cycle(input); std::cout << "Controller successfully started!" << std::endl; - // Buffer for printing info - char buf[4096] = {}; - auto prev = GetNow(); double frequency; - std::string status_line; while(true) { auto now = GetNow(); @@ -104,10 +101,8 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); - ::snprintf(buf, sizeof(buf) -1, "f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f) ", + ::printf("f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)\r", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); - status_line += buf; - ::printf("%s\n \r", status_line.c_str()); ::fflush(::stdout); } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 94787023..37f32a88 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -66,7 +66,7 @@ int main(int argc, char** argv) /* Copy data from CANFD frame to CAN frame*/ tx_frame.bus = can_fd_frame.bus; - tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.id = can_fd_frame.arbitration_id; // DZIEKI TEMU DZIALAAA tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); From cf19da930a4f2e816809e9eb13a26b2b509e0388 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:08:08 +0200 Subject: [PATCH 044/137] Updated main_test --- .../src/main_test.cpp | 18 ++++++++++++++---- 1 file changed, 14 insertions(+), 4 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index b3a66f28..90486898 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -17,6 +17,16 @@ static double GetNow() static_cast(ts.tv_nsec) / 1e9; }; +template +void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, const actuator_wrappers::ActuatorCommand& command) +{ + wrapper.command_to_tx_frame(tx_frame, command); +}; +template +void state(actuator_wrappers::ActuatorWrapperBase& wrapper,const mjbots::pi3hat::CanFrame& rx_frame, actuator_wrappers::ActuatorState& state) +{ + wrapper.rx_frame_to_state(rx_frame, state); +} int main(int argc, char** argv) { // moteus options @@ -44,7 +54,7 @@ int main(int argc, char** argv) mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; - + mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&rx_frame, 1); mjbots::pi3hat::Attitude attitude; @@ -56,7 +66,6 @@ int main(int argc, char** argv) input.attitude = &attitude; input.request_attitude = true; - std::cout << "Options for controller succesfully initialized!" << std::endl; mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); // pi3hat output @@ -96,11 +105,12 @@ int main(int argc, char** argv) { auto now = GetNow(); actuator_command.position_ = 5 * sin(now - prev); - moteus_wrapper.command_to_tx_frame(tx_frame, actuator_command); + command(moteus_wrapper, tx_frame, actuator_command); pi3hat_output = pi3hat.Cycle(input); + ::usleep(100); auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; - moteus_wrapper.rx_frame_to_state(rx_frame, actuator_state); + state(moteus_wrapper, rx_frame, actuator_state); ::printf("f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)\r", frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); ::fflush(::stdout); From 5ce2bc9ea4c6b1d1bbc3bf011e3390e55a3abe6d Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:10:04 +0200 Subject: [PATCH 045/137] Updated main_test --- .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index df23a7ab..3a6bcf5c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -71,7 +71,7 @@ class ActuatorWrapperBase }; /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ - void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) + void command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) { tx_frame.id = params_.id; tx_frame.bus = params_.bus; @@ -83,7 +83,7 @@ class ActuatorWrapperBase }; /* Static virtual method for preparing ActuatorState form RX CAN frame */ - void rx_frame_to_state(CanFrame& rx_frame, ActuatorState& state) + void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) { derived().rx_frame_to_state(rx_frame, state); state.position_ = params_.direction_ * state.position_; From 1636e6dc27eb6eb0923fe5721b2b7d9748298717 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:11:00 +0200 Subject: [PATCH 046/137] Updated main_test --- .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 3a6bcf5c..8bdefd27 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -79,7 +79,7 @@ class ActuatorWrapperBase command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); - derived().make_position(tx_frame, command); + derived().command_to_tx_frame(tx_frame, command); }; /* Static virtual method for preparing ActuatorState form RX CAN frame */ From 757deb0d6faf996d02c9e25eaa7204d0383c5ebe Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:14:04 +0200 Subject: [PATCH 047/137] Changed command to tx_frame --- .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 2 +- .../include/actuator_wrappers/MoteusWrapper.hpp | 2 +- .../src/actuator_wrappers/MoteusWrapper.cpp | 4 ++-- .../pi3hat_hardware_interface/src/main_test.cpp | 2 +- 4 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 8bdefd27..7debd5da 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -71,7 +71,7 @@ class ActuatorWrapperBase }; /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ - void command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) + void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { tx_frame.id = params_.id; tx_frame.bus = params_.bus; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index c3808040..ec0383f6 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -26,7 +26,7 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots /* Static override */ void init(CanFrame& tx_frame); - void command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command); + void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command); void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state); }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index f15e2a6e..84972dd7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -28,7 +28,7 @@ void MoteusWrapper::init(CanFrame& tx_frame) std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } -void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ActuatorCommand& command) +void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { /* Change command values */ position_command_.position = command.position_ * radians_to_rotation; @@ -49,7 +49,7 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& s { /* Parse data from RX CAN frame to Result object */ if(((rx_frame.id >> 8) & 0x7f) != params_.id) return; /* This should not happen! (map frame to wrapper first) */ - + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians; state.velocity_ = result.velocity * rotation_to_radians; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 90486898..d1edc330 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -18,7 +18,7 @@ static double GetNow() }; template -void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, const actuator_wrappers::ActuatorCommand& command) +void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, actuator_wrappers::ActuatorCommand& command) { wrapper.command_to_tx_frame(tx_frame, command); }; From 4b3e0d8658d6c58bbfde7e2460e608f23005e1b2 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:17:21 +0200 Subject: [PATCH 048/137] Edited tests --- .../pi3hat_hardware_interface/src/main_test.cpp | 4 ++-- .../pi3hat_hardware_interface/src/simple_test.cpp | 10 +++++----- 2 files changed, 7 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index d1edc330..71811b11 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -111,8 +111,8 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; state(moteus_wrapper, rx_frame, actuator_state); - ::printf("f/p/v/t=(%7.3f, %7.3f, %7.3f, %7.3f)\r", - frequency, actuator_state.position_, actuator_state.velocity_, actuator_state.torque_); + ::printf("f, pos_c, pos_s=(%7.3f, %7.3f, %7.3f)\r", + frequency, actuator_command.position_, actuator_state.position_); ::fflush(::stdout); } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 37f32a88..6f9c3bc0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -45,12 +45,12 @@ int main(int argc, char** argv) std::unique_ptr pi3hat = std::make_unique(pi3hat_configuration); - // mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); - // tx_frame.size = can_fd_frame.size; - // std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller.MakeStop(); + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - // pi3hat_output = pi3hat->Cycle(input); - // std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; + pi3hat_output = pi3hat->Cycle(input); + std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; // Buffer for printing info char buf[4096] = {}; From c48d1f2c30efb9ec3609989c68ae351d43ef171d Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 16:44:25 +0200 Subject: [PATCH 049/137] Added sleep to test --- .../pi3hat_hardware_interface/src/simple_test.cpp | 1 + 1 file changed, 1 insertion(+) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 6f9c3bc0..d77fcced 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -74,6 +74,7 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; pi3hat_output = pi3hat->Cycle(input); + ::usleep(1000); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; std::cout << "Attitude?: " << pi3hat_output.attitude_present << std::endl; From f444c577c81f2ba2168513fc239b5a9d270e9ea7 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:06:05 +0200 Subject: [PATCH 050/137] Tests edited --- .../pi3hat_hardware_interface/src/main_test.cpp | 1 + .../pi3hat_hardware_interface/src/simple_test.cpp | 5 +++-- 2 files changed, 4 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 71811b11..6f75ce45 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -54,6 +54,7 @@ int main(int argc, char** argv) mjbots::pi3hat::CanFrame tx_frame; mjbots::pi3hat::CanFrame rx_frame; + tx_frame.expect_reply = true; mjbots::pi3hat::Span tx_span(&tx_frame, 1); mjbots::pi3hat::Span rx_span(&rx_frame, 1); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index d77fcced..32e2b8b1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -77,8 +77,9 @@ int main(int argc, char** argv) ::usleep(1000); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; - std::cout << "Attitude?: " << pi3hat_output.attitude_present << std::endl; - std::cout << "Data size?: " << pi3hat_output.rx_can_size << std::endl; + ::printf("f, pos_c, pos_s=(%7.3f, %7.3f, %7.3f)\r", + frequency, position_, state_position ); + ::fflush(::stdout); }; return 0; From 85d5b1931f31b9e0f4b5846eb398c480a6f3b88b Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:11:38 +0200 Subject: [PATCH 051/137] Tests edited --- src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp | 2 +- .../pi3hat_hardware_interface/src/simple_test.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 6f75ce45..5ba768e6 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -112,7 +112,7 @@ int main(int argc, char** argv) auto mesaure_time = GetNow() - now; frequency = 1/mesaure_time; state(moteus_wrapper, rx_frame, actuator_state); - ::printf("f, pos_c, pos_s=(%7.3f, %7.3f, %7.3f)\r", + ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", frequency, actuator_command.position_, actuator_state.position_); ::fflush(::stdout); } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 32e2b8b1..6ac5e7a3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -77,7 +77,7 @@ int main(int argc, char** argv) ::usleep(1000); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; - ::printf("f, pos_c, pos_s=(%7.3f, %7.3f, %7.3f)\r", + ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", frequency, position_, state_position ); ::fflush(::stdout); From 1761ed3da22fab6fa10d11f6378c2350784ef345 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:19:27 +0200 Subject: [PATCH 052/137] Tests edited --- .../pi3hat_hardware_interface/src/main_test.cpp | 6 ++---- .../pi3hat_hardware_interface/src/simple_test.cpp | 9 ++++----- 2 files changed, 6 insertions(+), 9 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp index 5ba768e6..9b75c478 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp @@ -48,8 +48,6 @@ int main(int argc, char** argv) // pi3hat mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; pi3hat_configuration.attitude_rate_hz = 1000; - pi3hat_configuration.can[0].automatic_retransmission = false; - pi3hat_configuration.can[0].fdcan_frame = false; mjbots::pi3hat::CanFrame tx_frame; @@ -101,7 +99,7 @@ int main(int argc, char** argv) std::cout << "Controller successfully started!" << std::endl; auto prev = GetNow(); - double frequency; + int frequency; while(true) { auto now = GetNow(); @@ -110,7 +108,7 @@ int main(int argc, char** argv) pi3hat_output = pi3hat.Cycle(input); ::usleep(100); auto mesaure_time = GetNow() - now; - frequency = 1/mesaure_time; + frequency = (int) 1/mesaure_time; state(moteus_wrapper, rx_frame, actuator_state); ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", frequency, actuator_command.position_, actuator_state.position_); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp index 6ac5e7a3..47a945e2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) char buf[4096] = {}; auto prev = GetNow(); - double frequency; + int frequency; std::string status_line; while(true) { @@ -69,12 +69,11 @@ int main(int argc, char** argv) tx_frame.id = can_fd_frame.arbitration_id; // DZIEKI TEMU DZIALAAA tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - - + + ::usleep(1000); auto mesaure_time = GetNow() - now; - frequency = 1/mesaure_time; + frequency = (int) 1/mesaure_time; pi3hat_output = pi3hat->Cycle(input); - ::usleep(1000); mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); double state_position = result.position; ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", From 30d09cc098aa893dcbae30af3d8e31ba86f24c16 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:24:51 +0200 Subject: [PATCH 053/137] Single wrapper tested --- .../src/{ => tests/hardware}/simple_test.cpp | 6 +++--- .../hardware/single_wrapper_test.cpp} | 2 +- 2 files changed, 4 insertions(+), 4 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/src/{ => tests/hardware}/simple_test.cpp (95%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{main_test.cpp => tests/hardware/single_wrapper_test.cpp} (99%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp similarity index 95% rename from src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp index 47a945e2..b787fcbf 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp @@ -1,6 +1,6 @@ -#include "../include/pi3hat/pi3hat.h" -#include "../include/pi3hat/realtime.h" -#include "../include/moteus/moteus.h" +#include "../../../include/pi3hat/pi3hat.h" +#include "../../../include/pi3hat/realtime.h" +#include "../../../include/moteus/moteus.h" static double GetNow() diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp similarity index 99% rename from src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp index 9b75c478..824cf43a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/main_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp @@ -106,7 +106,7 @@ int main(int argc, char** argv) actuator_command.position_ = 5 * sin(now - prev); command(moteus_wrapper, tx_frame, actuator_command); pi3hat_output = pi3hat.Cycle(input); - ::usleep(100); + ::usleep(1000); auto mesaure_time = GetNow() - now; frequency = (int) 1/mesaure_time; state(moteus_wrapper, rx_frame, actuator_state); From 08527e932534de0368821af482e75ac7b65e36bf Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:30:23 +0200 Subject: [PATCH 054/137] Quick fix --- .../src/tests/hardware/single_wrapper_test.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp index 824cf43a..cfdc8ef5 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp @@ -2,8 +2,8 @@ -#include "../include/pi3hat/pi3hat.h" -#include "../include/pi3hat/realtime.h" +#include "../../../include/pi3hat/pi3hat.h" +#include "../../../include/pi3hat/realtime.h" #include #include #include From f5a2b522847b53e27c21abfc887a8742cd1458e2 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 17:30:45 +0200 Subject: [PATCH 055/137] Quick fix --- .../src/tests/hardware/single_wrapper_test.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp index cfdc8ef5..e10835d3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp @@ -1,4 +1,4 @@ -#include "../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../../include/actuator_wrappers/MoteusWrapper.hpp" From caaf3977a808075d748e1245b066fea7291f77f8 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 19 Jul 2024 18:40:04 +0200 Subject: [PATCH 056/137] Removed useless imports --- .../meldog_leg_tests/meldog_leg_tests/circle_trajectory.py | 1 - .../meldog_leg_tests/meldog_leg_tests/demo_single_motor.py | 1 - .../meldog_leg_tests/meldog_leg_tests/forward_kinematics_2D.py | 1 - .../meldog_leg_tests/meldog_leg_tests/inverse_kinematics_2D.py | 1 - .../meldog_leg_tests/meldog_leg_tests/jump_control_node.py | 1 - .../meldog_leg_tests/meldog_leg_tests/linear_trajectory.py | 1 - .../meldog_leg_tests/meldog_leg_tests/moteus_ploter.py | 3 --- .../meldog_leg_tests/multi_moteus_controller_two_threads.py | 1 - 8 files changed, 10 deletions(-) diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/circle_trajectory.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/circle_trajectory.py index 332afecd..7bc03545 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/circle_trajectory.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/circle_trajectory.py @@ -3,7 +3,6 @@ from geometry_msgs.msg import Vector3 import math from rclpy.duration import Duration -from rclpy.time import Time class Circle_Trajectory(Node): def __init__(self,name): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/demo_single_motor.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/demo_single_motor.py index 86555ce8..6263b005 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/demo_single_motor.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/demo_single_motor.py @@ -5,7 +5,6 @@ from meldog_interfaces_tests.msg import MoteusControl from geometry_msgs.msg import Vector3 import math -import matplotlib.pyplot as plt from rclpy.duration import Duration class Demo_Single_Motor(Node): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/forward_kinematics_2D.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/forward_kinematics_2D.py index da13921c..c8030545 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/forward_kinematics_2D.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/forward_kinematics_2D.py @@ -3,7 +3,6 @@ from meldog_interfaces_tests.msg import MultiMoteusState from geometry_msgs.msg import Vector3 import math -import matplotlib.pyplot as plt class Leg_Forward_Kinematics_Solver(Node): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/inverse_kinematics_2D.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/inverse_kinematics_2D.py index fd732e21..3286ca70 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/inverse_kinematics_2D.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/inverse_kinematics_2D.py @@ -5,7 +5,6 @@ from meldog_interfaces_tests.msg import MoteusControl from geometry_msgs.msg import Vector3 import math -import matplotlib.pyplot as plt class Leg_Inverse_Kinematics_Solver(Node): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/jump_control_node.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/jump_control_node.py index b2e14252..7c9cec80 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/jump_control_node.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/jump_control_node.py @@ -8,7 +8,6 @@ import pandas import numpy import pathlib -import sys class Optimal_Jump_Controler_Node(Node): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/linear_trajectory.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/linear_trajectory.py index 0c006e5c..2a0cfe9f 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/linear_trajectory.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/linear_trajectory.py @@ -3,7 +3,6 @@ from geometry_msgs.msg import Vector3 import math from rclpy.duration import Duration -from rclpy.time import Time import rclpy.time class Linear_Trajectory(Node): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/moteus_ploter.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/moteus_ploter.py index 62fca8c4..23bfca17 100644 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/moteus_ploter.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/moteus_ploter.py @@ -2,12 +2,9 @@ from rclpy.node import Node from meldog_interfaces_tests.msg import MultiMoteusControl, MultiMoteusState from meldog_interfaces_tests.msg import MoteusControl, MoteusState -from geometry_msgs.msg import Vector3 -import math import matplotlib.pyplot as plt import matplotlib.animation as animation import threading -import sys class Ploter(Node): def __init__(self, name, plt: plt): diff --git a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/multi_moteus_controller_two_threads.py b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/multi_moteus_controller_two_threads.py index 8876c809..813b975b 100755 --- a/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/multi_moteus_controller_two_threads.py +++ b/src/meldog_tests/meldog_leg_tests/meldog_leg_tests/multi_moteus_controller_two_threads.py @@ -8,7 +8,6 @@ import math import threading import moteus.multiplex -import copy import queue # TODO: Dodaj serwer do wylaczenia moteusa (make_stop()) From bf6bc3e277ba468d2b66ba6da46dd4dc5c697699 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 20 Jul 2024 23:45:30 +0200 Subject: [PATCH 057/137] Edited transmissions loading --- .../src/pi3hat_hardware_interface.cpp | 48 ++++++++++++++++++- 1 file changed, 47 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 507edb24..73ed9738 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -148,18 +148,64 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm return hardware_interface::CallbackReturn::ERROR; } + if(transmission_info.actuators.size() != 1) + { + RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; + } + /* Find joint index for transmission handels by name*/ std::vector::const_iterator joint_it = std::find(joint_names.begin(), joint_names.end(), transmission_info.joints[0].name); int joint_index = std::distance(joint_names.begin(), joint_it); /* Joint handels */ - + std::vector joint_handels; + append_joint_handels(joint_handels, transmission_info.joints[0].name, joint_index); + /* Actuator handels */ + std::vector actuator_handels; + append_actuator_handels(actuator_handels, transmission_info.actuators[0].name, joint_index); transmission->configure(joint_handles, actuator_handles); + transmissions_.push_back(transmission); } RCLCPP_INFO(*logger_, "Simple transmissions initialized!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmissions(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names) +{ + RCLCPP_INFO(*logger_, "Simple transmissions initialization starting!"); + for (const auto & transmission_info : info_.transmissions) + { + std::shared_ptr transmission; + + if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") + { + continue; + } + + load_transmission_data(transmission_info, transmission, loader); + + std::vector joint_handles; + std::vector actuator_handles; + + if(transmission_info.joints.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; + } + + if(transmission_info.actuators.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; + } + + + } return hardware_interface::CallbackReturn::SUCCESS; } \ No newline at end of file From d2afc11eb1d720654756dd6f73a921bef0aa2e5b Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 21 Jul 2024 17:20:10 +0200 Subject: [PATCH 058/137] Added loader for fourbarlinkage transmission --- .../pi3hat_hardware_interface.hpp | 12 +- .../src/pi3hat_hardware_interface.cpp | 159 +++++++++++------- 2 files changed, 107 insertions(+), 64 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 8080604c..017c4cf4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -160,15 +160,15 @@ namespace pi3hat_hardware_interface hardware_interface::CallbackReturn create_transmission_interface(const hardware_interface::HardwareInfo &info); /* Functions for creating simple transmission */ - hardware_interface::CallbackReturn create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating four bar linkage transmission */ - hardware_interface::CallbackReturn create_fbl_transmissions(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating differential transmission */ - hardware_interface::CallbackReturn create_diff_transmissions(const hardware_interface::TransmissionInfo& transmission_info, + hardware_interface::CallbackReturn create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names); /* Functions for checking, if data passed from urdf is correct */ @@ -177,8 +177,10 @@ namespace pi3hat_hardware_interface transmission_interface::TransmissionLoader& loader); /* Functions for creating joint and actuator handels */ - void append_joint_handels(std::vector& joint_handles, std::string joint_name, int joint_index); - void append_actuator_handels(std::vector& actuator_handles, std::string actuator_name, int actuator_index); + void append_joint_handles(std::vector& joint_handles, + const std::string joint_name, const int joint_index); + void append_actuator_handles(std::vector& actuator_handles, + const std::string actuator_name, const int actuator_index); /* Here add your actuator wrapper type */ enum WrapperType diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 73ed9738..50326583 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -3,6 +3,7 @@ using namespace pi3hat_hardware_interface; using namespace actuator_wrappers; +/* MAIN FUNCTIONS */ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) @@ -74,7 +75,13 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa } } -void Pi3HatHardwareInterface::append_joint_handels(std::vector& joint_handles, std::string joint_name, int joint_index) + + + + + + /* UTILITY FUNCTIONS */ +void Pi3HatHardwareInterface::append_joint_handles(std::vector& joint_handles, const std::string joint_name, const int joint_index) { transmission_interface::JointHandle joint_handle_position(joint_name, hardware_interface::HW_IF_POSITION, &hw_joint_transmission_passthrough_[joint_index].position_); @@ -88,7 +95,8 @@ void Pi3HatHardwareInterface::append_joint_handels(std::vector& actuator_handles, std::string actuator_name, int actuator_index) + +void Pi3HatHardwareInterface::append_actuator_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index) { transmission_interface::ActuatorHandle actuator_handle_position(actuator_name, hardware_interface::HW_IF_POSITION, &hw_actuator_transmission_passthrough_[actuator_index].position_); @@ -124,88 +132,121 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_ return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transmissions(const hardware_interface::TransmissionInfo& transmission_info, +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names) { - RCLCPP_INFO(*logger_, "Simple transmissions initialization starting!"); - for (const auto & transmission_info : info_.transmissions) + RCLCPP_INFO(*logger_, "Simple transmission initialization starting!"); + + std::shared_ptr transmission; + if (transmission_info.type != "transmission_interface/SimpleTransmission") + { + RCLCPP_FATAL(*logger_, "This is not SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + + load_transmission_data(transmission_info, transmission, loader); + + if(transmission_info.joints.size() != 1) { - std::shared_ptr transmission; + RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } - if (transmission_info.type != "transmission_interface/SimpleTransmission") - { - continue; - } + if(transmission_info.actuators.size() != 1) + { + RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } - load_transmission_data(transmission_info, transmission, loader); + /* Find joint index for transmission handels by name*/ + std::vector::const_iterator joint_it = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[0].name); + int joint_index = std::distance(joint_names.begin(), joint_it); - std::vector joint_handles; - std::vector actuator_handles; + /* Joint handels */ + std::vector joint_handles; + append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index); - if(transmission_info.joints.size() != 1) - { - RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; - } - - if(transmission_info.actuators.size() != 1) - { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; - } - - /* Find joint index for transmission handels by name*/ - std::vector::const_iterator joint_it = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[0].name); - int joint_index = std::distance(joint_names.begin(), joint_it); - - /* Joint handels */ - std::vector joint_handels; - append_joint_handels(joint_handels, transmission_info.joints[0].name, joint_index); - - /* Actuator handels */ - std::vector actuator_handels; - append_actuator_handels(actuator_handels, transmission_info.actuators[0].name, joint_index); + /* Actuator handels */ + std::vector actuator_handles; + append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index); + try + { transmission->configure(joint_handles, actuator_handles); - transmissions_.push_back(transmission); } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + transmissions_.push_back(transmission); + RCLCPP_INFO(*logger_, "Simple transmissions initialized!"); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmissions(const hardware_interface::TransmissionInfo& transmission_info, +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names) { RCLCPP_INFO(*logger_, "Simple transmissions initialization starting!"); - for (const auto & transmission_info : info_.transmissions) + + std::shared_ptr transmission; + if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") { - std::shared_ptr transmission; + RCLCPP_FATAL(*logger_, "This is not FourBarLinkageTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + + load_transmission_data(transmission_info, transmission, loader); + std::vector joint_handles; + std::vector actuator_handles; - if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") - { - continue; - } + if(transmission_info.joints.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of joints in FourBarLinkageTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + if(transmission_info.actuators.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of actuators in FourBarLinkageTTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } - load_transmission_data(transmission_info, transmission, loader); + /* Find joints indexes for transmission handels by name*/ + std::vector::const_iterator joint_it_1 = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[0].name); + int joint_index_1 = std::distance(joint_names.begin(), joint_it_1); - std::vector joint_handles; - std::vector actuator_handles; + std::vector::const_iterator joint_it_2 = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[1].name); + int joint_index_2 = std::distance(joint_names.begin(), joint_it_2); - if(transmission_info.joints.size() != 2) - { - RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; - } + /* Joint handels */ + std::vector joint_handles; + append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index_1); + append_joint_handles(joint_handles, transmission_info.joints[1].name, joint_index_2); + + /* Actuator handels */ + std::vector actuator_handles; + append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index_1); + append_actuator_handles(actuator_handles, transmission_info.actuators[1].name, joint_index_2); - if(transmission_info.actuators.size() != 2) - { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; - } + try + { + transmission->configure(joint_handles, actuator_handles); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + transmissions_.push_back(transmission); + + RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialized!"); - } return hardware_interface::CallbackReturn::SUCCESS; } \ No newline at end of file From a28760a03a9de4145038574be2f6285519db0ca2 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 21 Jul 2024 17:29:56 +0200 Subject: [PATCH 059/137] Added differential transmision loader --- .../src/pi3hat_hardware_interface.cpp | 69 ++++++++++++++++++- 1 file changed, 66 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 50326583..de167e51 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -191,7 +191,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names) { - RCLCPP_INFO(*logger_, "Simple transmissions initialization starting!"); + RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialization starting!"); std::shared_ptr transmission; if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") @@ -199,7 +199,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss RCLCPP_FATAL(*logger_, "This is not FourBarLinkageTransmission!"); return hardware_interface::CallbackReturn::ERROR; // this should not happen! } - + load_transmission_data(transmission_info, transmission, loader); std::vector joint_handles; std::vector actuator_handles; @@ -211,7 +211,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss } if(transmission_info.actuators.size() != 2) { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in FourBarLinkageTTransmission!"); + RCLCPP_FATAL(*logger_, "Invalid number of actuators in FourBarLinkageTransmission!"); return hardware_interface::CallbackReturn::ERROR; // this should not happen! } @@ -248,5 +248,68 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialized!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, + transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names) +{ + RCLCPP_INFO(*logger_, "Differential transmissions initialization starting!"); + + std::shared_ptr transmission; + if (transmission_info.type != "transmission_interface/DifferentialTransmission") + { + RCLCPP_FATAL(*logger_, "This is not DifferentialTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + + load_transmission_data(transmission_info, transmission, loader); + std::vector joint_handles; + std::vector actuator_handles; + + if(transmission_info.joints.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of joints in DifferentialTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + if(transmission_info.actuators.size() != 2) + { + RCLCPP_FATAL(*logger_, "Invalid number of actuators in DifferentialTransmission!"); + return hardware_interface::CallbackReturn::ERROR; // this should not happen! + } + + /* Find joints indexes for transmission handels by name*/ + std::vector::const_iterator joint_it_1 = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[0].name); + int joint_index_1 = std::distance(joint_names.begin(), joint_it_1); + + std::vector::const_iterator joint_it_2 = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[1].name); + int joint_index_2 = std::distance(joint_names.begin(), joint_it_2); + + /* Joint handels */ + std::vector joint_handles; + append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index_1); + append_joint_handles(joint_handles, transmission_info.joints[1].name, joint_index_2); + + /* Actuator handels */ + std::vector actuator_handles; + append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index_1); + append_actuator_handles(actuator_handles, transmission_info.actuators[1].name, joint_index_2); + + try + { + transmission->configure(joint_handles, actuator_handles); + } + catch (const transmission_interface::TransmissionInterfaceException & exc) + { + RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); + return hardware_interface::CallbackReturn::ERROR; + } + + transmissions_.push_back(transmission); + + RCLCPP_INFO(*logger_, "Differential transmissions initialized!"); + return hardware_interface::CallbackReturn::SUCCESS; } \ No newline at end of file From 740d3fe8fd50b084b5519df221a38611949b88cc Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 21 Jul 2024 18:02:25 +0200 Subject: [PATCH 060/137] Defined transmissions loader funtions --- .../pi3hat_hardware_interface.hpp | 2 + .../src/pi3hat_hardware_interface.cpp | 50 ++++++++++++++++++- 2 files changed, 51 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 017c4cf4..e72fac9a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -182,6 +182,8 @@ namespace pi3hat_hardware_interface void append_actuator_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index); + + /* Here add your actuator wrapper type */ enum WrapperType { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index de167e51..23191ddf 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -80,7 +80,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa - /* UTILITY FUNCTIONS */ +/* TRANSMISSION FUNCTIONS */ void Pi3HatHardwareInterface::append_joint_handles(std::vector& joint_handles, const std::string joint_name, const int joint_index) { transmission_interface::JointHandle joint_handle_position(joint_name, hardware_interface::HW_IF_POSITION, @@ -128,7 +128,55 @@ void Pi3HatHardwareInterface::load_transmission_data(const hardware_interface::T hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) { + /* Prepare loaders */ + transmission_interface::SimpleTransmissionLoader simple_loader; + transmission_interface::FourBarLinkageTransmissionLoader fbl_loader; + transmission_interface::DifferentialTransmissionLoader diff_loader; + + /* Prepare joint names */ + std::vector joint_names; + for(const auto& joint: info.joints) + { + joint_names.push_back(joint.name); + } + + /* For fast computation, transmissions will be sorted by type */ + + /* Simple transmissions */ + for(const auto& transmission_info: info.transmissions) + { + if(transmission_info.type == "transmission_interface/SimpleTransmission") + { + if(create_simple_transmission(transmission_info, simple_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + { + return hardware_interface::CallbackReturn::ERROR; + } + } + } + /* FourBarLinkage transmissions */ + for(const auto& transmission_info: info.transmissions) + { + if(transmission_info.type == "transmission_interface/FourBarLinkageTransmission") + { + if(create_fbl_transmission(transmission_info, fbl_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + { + return hardware_interface::CallbackReturn::ERROR;; + } + } + } + + /* Differential transmissions */ + for(const auto& transmission_info: info.transmissions) + { + if(transmission_info.type == "transmission_interface/DifferentialTransmission") + { + if(create_diff_transmission(transmission_info, diff_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + { + return hardware_interface::CallbackReturn::ERROR; + } + } + } return hardware_interface::CallbackReturn::SUCCESS; } From d887b214f3cebb30aa7b6a3ef06c09d58e01be71 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 22 Jul 2024 17:02:39 +0200 Subject: [PATCH 061/137] Added hrdware test for two motors --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 6 +- .../src/actuator_wrappers/MoteusWrapper.cpp | 2 +- .../tests/hardware/double_wrapper_test.cpp | 161 ++++++++++++++++++ 3 files changed, 164 insertions(+), 5 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 7debd5da..b4fc1371 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -33,8 +33,8 @@ struct ActuatorState struct ActuatorParameters { - int id; - int bus; + int id_; /* Usage in your wrapper (check moteus wrapper)*/ + int bus_; /* Usage in your wrapper (check moteus wrapper)*/ double position_max_; double position_min_; double velocity_max_; @@ -73,8 +73,6 @@ class ActuatorWrapperBase /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { - tx_frame.id = params_.id; - tx_frame.bus = params_.bus; command.position_ = params_.direction_* std::clamp(command.position_, params_.position_min_, params_.position_max_); command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 84972dd7..307099ec 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -48,7 +48,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& com void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) { /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != params_.id) return; /* This should not happen! (map frame to wrapper first) */ + if(((rx_frame.id >> 8) & 0x7f) != params_.id_) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp new file mode 100644 index 00000000..d68acf6d --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp @@ -0,0 +1,161 @@ +#include "../../../include/actuator_wrappers/MoteusWrapper.hpp" + + + +#include "../../../include/pi3hat/pi3hat.h" +#include "../../../include/pi3hat/realtime.h" +#include +#include +#include +#include + + + +static double GetNow() +{ + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; +}; + +template +void init(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame) +{ + wrapper.init(tx_frame); +}; + +template +void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, actuator_wrappers::ActuatorCommand& command) +{ + wrapper.command_to_tx_frame(tx_frame, command); +}; +template +void state(actuator_wrappers::ActuatorWrapperBase& wrapper,const mjbots::pi3hat::CanFrame& rx_frame, actuator_wrappers::ActuatorState& state) +{ + wrapper.rx_frame_to_state(rx_frame, state); +} + + +int main(int argc, char** argv) +{ + // moteusues options + using mjbots::moteus::Controller; + Controller::Options moteus_1_options; + moteus_1_options.bus = 1; + moteus_1_options.id = 1; + + Controller::Options moteus_2_options; + moteus_2_options.bus = 2; + moteus_2_options.id = 2; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_1_options.position_format = format; + moteus_2_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_command; + + + // pi3hat + mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; + pi3hat_configuration.attitude_rate_hz = 1000; + + + std::vector tx_frame; + std::vector rx_frame; + tx_frame[0].expect_reply = true; + tx_frame[1].expect_reply = true; + + mjbots::pi3hat::Span tx_span(tx_frame.data(), 2); + mjbots::pi3hat::Span rx_span(rx_frame.data(), 2); + mjbots::pi3hat::Attitude attitude; + + mjbots::pi3hat::Pi3Hat::Input input; + + input.tx_can = tx_span; + input.rx_can = rx_span; + input.attitude = &attitude; + input.request_attitude = true; + + mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + + // pi3hat output + mjbots::pi3hat::Pi3Hat::Output pi3hat_output; + + // moteus wrapper + actuator_wrappers::ActuatorParameters params_1; + params_1.direction_ = 1; + params_1.position_max_ = 30; + params_1.position_min_ = -30; + params_1.velocity_max_ = 10; + params_1.torque_max_ = 1; + params_1.bus_ = 1; + params_1.id_ = 1; + + actuator_wrappers::ActuatorParameters params_2; + params_2.direction_ = 1; + params_2.position_max_ = 10; + params_2.position_min_ = -10; + params_2.velocity_max_ = 10; + params_2.torque_max_ = 1; + params_2.bus_ = 2; + params_2.id_ = 2; + + std::vector actuator_wrappers; + std::vector actuator_commands; + std::vector actuator_states; + + actuator_wrappers.push_back(actuator_wrappers::MoteusWrapper(params_1, moteus_1_options, moteus_command)); + actuator_wrappers.push_back(actuator_wrappers::MoteusWrapper(params_2, moteus_2_options, moteus_command)); + + actuator_commands.push_back(actuator_wrappers::ActuatorCommand()); + actuator_commands.push_back(actuator_wrappers::ActuatorCommand()); + + actuator_states.push_back(actuator_wrappers::ActuatorState()); + actuator_states.push_back(actuator_wrappers::ActuatorState()); + + + std::cout << "Options for controllers succesfully initialized!" << std::endl; + + + mjbots::pi3hat::ConfigureRealtime(0); + std::cout << "Realtime control activated!" << std::endl; + + // set stop to moteus + for(int i = 0; i < actuator_wrappers.size(); i++) + { + init(actuator_wrappers[i],tx_frame[i]); + } + pi3hat_output = pi3hat.Cycle(input); + std::cout << "Controllers successfully started!" << std::endl; + + auto prev = GetNow(); + int frequency; + while(true) + { + auto now = GetNow(); + actuator_commands[0].position_ = 20 * cos(now - prev); + actuator_commands[1].position_ = 10 * sin(now - prev); + for(int i = 0; i < actuator_wrappers.size(); i++) + { + command(actuator_wrappers[i],tx_frame[i],actuator_commands[i]); + } + pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000); + auto mesaure_time = GetNow() - now; + frequency = (int) 1/mesaure_time; + for(int i = 0; i < actuator_wrappers.size(); i++) + { + state(actuator_wrappers[i],rx_frame[i],actuator_states[i]); + } + ::printf("f = %d\n pos_1_command = %7.3f, pos_1_state = %7.3f)\n pos_2_command = %7.3f, pos_2_state = %7.3f)\r", + frequency, actuator_commands[0].position_, actuator_states[0].position_, actuator_commands[1].position_, actuator_states[1].position_); + ::fflush(::stdout); + } + + return 0; +} \ No newline at end of file From 26ed345979be2987e46d82814642b1af2b20c5d4 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 22 Jul 2024 17:16:05 +0200 Subject: [PATCH 062/137] Fixed segfault --- .../src/tests/hardware/double_wrapper_test.cpp | 4 ++-- .../src/tests/hardware/single_wrapper_test.cpp | 4 ++-- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp index d68acf6d..0828aaef 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp @@ -65,8 +65,8 @@ int main(int argc, char** argv) pi3hat_configuration.attitude_rate_hz = 1000; - std::vector tx_frame; - std::vector rx_frame; + std::vector tx_frame(2); + std::vector rx_frame(2); tx_frame[0].expect_reply = true; tx_frame[1].expect_reply = true; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp index e10835d3..c599bc9e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp @@ -77,8 +77,8 @@ int main(int argc, char** argv) params.position_min_ = -10; params.velocity_max_ = 4; params.torque_max_ = 1; - params.bus = 1; - params.id = 1; + params.bus_ = 1; + params.id_ = 1; actuator_wrappers::MoteusWrapper moteus_wrapper(params, moteus_options, moteus_command); actuator_wrappers::ActuatorCommand actuator_command; From 606ecb72f2cc180b59810a58145f2ddfbac14903 Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 23 Jul 2024 18:46:33 +0200 Subject: [PATCH 063/137] Moved tests directory --- .../{src => }/tests/hardware/double_wrapper_test.cpp | 6 +++--- .../{src => }/tests/hardware/simple_test.cpp | 6 +++--- .../{src => }/tests/hardware/single_wrapper_test.cpp | 6 +++--- 3 files changed, 9 insertions(+), 9 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/{src => }/tests/hardware/double_wrapper_test.cpp (97%) rename src/meldog_hardware/pi3hat_hardware_interface/{src => }/tests/hardware/simple_test.cpp (95%) rename src/meldog_hardware/pi3hat_hardware_interface/{src => }/tests/hardware/single_wrapper_test.cpp (95%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp similarity index 97% rename from src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp index 0828aaef..d2f9449b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/double_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp @@ -1,9 +1,9 @@ -#include "../../../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../include/actuator_wrappers/MoteusWrapper.hpp" -#include "../../../include/pi3hat/pi3hat.h" -#include "../../../include/pi3hat/realtime.h" +#include "../../include/pi3hat/pi3hat.h" +#include "../../include/pi3hat/realtime.h" #include #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp similarity index 95% rename from src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp index b787fcbf..99cf0678 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp @@ -1,6 +1,6 @@ -#include "../../../include/pi3hat/pi3hat.h" -#include "../../../include/pi3hat/realtime.h" -#include "../../../include/moteus/moteus.h" +#include "../../include/pi3hat/pi3hat.h" +#include "../../include/pi3hat/realtime.h" +#include "../../include/moteus/moteus.h" static double GetNow() diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp similarity index 95% rename from src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp index c599bc9e..3eb2bba1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/tests/hardware/single_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp @@ -1,9 +1,9 @@ -#include "../../../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../include/actuator_wrappers/MoteusWrapper.hpp" -#include "../../../include/pi3hat/pi3hat.h" -#include "../../../include/pi3hat/realtime.h" +#include "../../include/pi3hat/pi3hat.h" +#include "../../include/pi3hat/realtime.h" #include #include #include From beaca58e6100c92e249a684647f9751be5f65d26 Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 23 Jul 2024 18:47:28 +0200 Subject: [PATCH 064/137] Edited actuator wrappers --- .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 8 +++++--- .../include/actuator_wrappers/MoteusWrapper.hpp | 2 +- .../src/actuator_wrappers/MoteusWrapper.cpp | 2 +- 3 files changed, 7 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index b4fc1371..538b1a77 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -37,9 +37,10 @@ struct ActuatorParameters int bus_; /* Usage in your wrapper (check moteus wrapper)*/ double position_max_; double position_min_; + double position_offset_; double velocity_max_; double torque_max_; - int direction_; + int direction_ = 1; }; template @@ -62,7 +63,7 @@ class ActuatorWrapperBase public: /* Constructor: takes CanFrame for later editing*/ - ActuatorWrapperBase(ActuatorParameters& params): params_(params) {}; + ActuatorWrapperBase(const ActuatorParameters& params): params_(params) {}; /* Static virtual method for starting actuators */ void init(CanFrame& tx_frame) @@ -73,7 +74,8 @@ class ActuatorWrapperBase /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) { - command.position_ = params_.direction_* std::clamp(command.position_, params_.position_min_, params_.position_max_); + command.position_ = params_.direction_* std::clamp(command.position_, + params_.position_min_, params_.position_max_) + params_.position_offset_; command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index ec0383f6..e616dc98 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -20,7 +20,7 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots public: /* Create Moteus Wrapper from existing frames */ - MoteusWrapper(ActuatorParameters& params, + MoteusWrapper(const ActuatorParameters& params, mjbots::moteus::Controller::Options& options, mjbots::moteus::PositionMode::Command& command); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 307099ec..6ec9a638 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -8,7 +8,7 @@ using namespace actuator_wrappers; -MoteusWrapper::MoteusWrapper(ActuatorParameters& params, +MoteusWrapper::MoteusWrapper(const ActuatorParameters& params, mjbots::moteus::Controller::Options& options, mjbots::moteus::PositionMode::Command& command): ActuatorWrapperBase(params), mjbots::moteus::Controller(options), From 6f188fac4222daf6e3051be842dc8ca3e87c0567 Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 23 Jul 2024 18:48:11 +0200 Subject: [PATCH 065/137] Added actuator creation in init method --- .../pi3hat_hardware_interface.hpp | 29 +++---- .../src/pi3hat_hardware_interface.cpp | 82 ++++++++++++++++--- 2 files changed, 86 insertions(+), 25 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index e72fac9a..fefd4ad7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -78,6 +78,13 @@ namespace pi3hat_hardware_interface const rclcpp::Time &time, const rclcpp::Duration &period) override; private: + + /* Here add your actuator wrapper type */ + enum WrapperType + { + Moteus = 0, + }; + /* UTILITY ROS2 OBJECTS: */ std::unique_ptr logger_; @@ -127,9 +134,12 @@ namespace pi3hat_hardware_interface /* For transmission interface */ std::vector hw_joint_transmission_passthrough_; - /* Function for creating moteus wrappers (here u can add your own wrapper) - Remember to change this function in source code */ - void add_actuator_wrapper(const hardware_interface::HardwareInfo &info, const WrapperType type); + /* Function for choosing wrappers (here u can add your own wrapper) + Remember to change this function in source code */ + WrapperType choose_actuator_wrapper(const std::string& type); + + /* Function for creating moteus wrappers (here u can add your own wrapper) */ + void add_actuator_wrapper(const actuator_wrappers::ActuatorParameters& params, const WrapperType type); template void commands_to_tx_frames(std::vector> actuator_wrappers) @@ -181,16 +191,7 @@ namespace pi3hat_hardware_interface const std::string joint_name, const int joint_index); void append_actuator_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index); - - - - /* Here add your actuator wrapper type */ - enum WrapperType - { - Moteus = 0, - - }; - -}; + }; +}; #endif diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 23191ddf..5bc29016 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -4,30 +4,56 @@ using namespace pi3hat_hardware_interface; using namespace actuator_wrappers; /* MAIN FUNCTIONS */ + hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardware_interface::HardwareInfo &info) { if (hardware_interface::SystemInterface::on_init(info) != hardware_interface::CallbackReturn::SUCCESS) { return hardware_interface::CallbackReturn::ERROR; } + logger_ = std::make_unique( rclcpp::get_logger("Pi3HatHardwareInterface")); - hw_actuator_commands_.resize(info_.joints.size(), ActuatorState{std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + hw_actuator_commands_.resize(info_.joints.size()); + hw_actuator_states_.resize(info_.joints.size()); + hw_actuator_transmission_passthrough_.resize(info_.joints.size()); + - hw_actuator_states_.resize(info_.joints.size(), ActuatorState{std::numeric_limits::quiet_NaN(), - std::numeric_limits::quiet_NaN(), std::numeric_limits::quiet_NaN()}); + hw_joint_commands_.resize(info_.joints.size()); + hw_joint_states_.resize(info_.joints.size()); + hw_joint_transmission_passthrough_.resize(info_.joints.size()); for (const hardware_interface::ComponentInfo &joint : info_.joints) { - hw_actuator_can_buses_.push_back(std::stoi(joint.parameters.at("can_channel"))); - hw_actuator_can_ids_.push_back(std::stoi(joint.parameters.at("can_id"))); - hw_actuator_position_mins_.push_back(std::stod(joint.parameters.at("position_min"))); - hw_actuator_position_maxs_.push_back(std::stod(joint.parameters.at("position_max"))); - hw_actuator_velocity_maxs_.push_back(std::stod(joint.parameters.at("velocity_max"))); - hw_actuator_effort_maxs_.push_back(std::stod(joint.parameters.at("effort_max"))); - hw_actuator_position_offsets_.push_back(std::stod(joint.parameters.at("position_offset"))); + actuator_wrappers::ActuatorParameters params; + params.bus_ = std::stoi(joint.parameters.at("can_bus")); + params.id_ = std::stoi(joint.parameters.at("can_id")); + params.direction_ = std::stoi(joint.parameters.at("direction")); + params.position_offset_ = std::stod(joint.parameters.at("position_offset")); + std::string type_string = joint.parameters.at("motor_type"); + + WrapperType type = choose_actuator_wrapper(type_string); + + for(const auto& command_interface: joint.command_interfaces) + { + if(command_interface.name == hardware_interface::HW_IF_POSITION) + { + params.position_max_ = std::stod(command_interface.max); + params.position_min_ = std::stod(command_interface.min); + + } + else if(command_interface.name == hardware_interface::HW_IF_VELOCITY) + { + params.velocity_max_ = std::stod(command_interface.max); + } + else if(command_interface.name == hardware_interface::HW_IF_EFFORT) + { + params.torque_max_ = std::stod(command_interface.max); + } + } + add_actuator_wrapper(params, type); + } /* Standard CAN config */ @@ -360,4 +386,38 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmis RCLCPP_INFO(*logger_, "Differential transmissions initialized!"); return hardware_interface::CallbackReturn::SUCCESS; +} + +void Pi3HatHardwareInterface::add_actuator_wrapper(const ActuatorParameters& params, const WrapperType type) +{ + switch(type) + { + case Moteus: + /* moteus options */ + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_command; + + moteus_wrappers.push_back(MoteusWrapper(params, moteus_options, moteus_command)); + break; + } +} + +Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_actuator_wrapper(const std::string& type) +{ + if(type == "moteus") + { + return WrapperType::Moteus; + }; } \ No newline at end of file From eaccd06fb7610007068ec8f1b961c81546dd1164 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 24 Jul 2024 17:39:08 +0200 Subject: [PATCH 066/137] Small edits --- .../actuator_wrappers/MoteusWrapper.hpp | 4 +- .../include/imu/IMUTransform.hpp | 4 + .../pi3hat_hardware_interface.hpp | 16 ++-- .../src/imu/IMUTransform.cpp | 5 ++ .../src/pi3hat_hardware_interface.cpp | 79 +++++++++---------- 5 files changed, 57 insertions(+), 51 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index e616dc98..c97f855e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -12,8 +12,8 @@ class MoteusWrapper: public ActuatorWrapperBase, protected mjbots private: /* Const coefficients for easy radians - rotations transform */ - const double rotation_to_radians = 2 * M_PI; - const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + static const double rotation_to_radians = 2 * M_PI; + static const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp index e9760231..50913fa3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp @@ -4,11 +4,15 @@ #include #include "../pi3hat/pi3hat.h" #include +#include namespace IMU { class IMUTransform { + private: + static const double degrees_to_radians = 2 * M_PI / 360.0; + public: /* Function for transforming transforming IMU data */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index fefd4ad7..12fa3e3a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -115,11 +115,11 @@ namespace pi3hat_hardware_interface std::vector actuator_joint_map_; /* Actuator states and commands */ - std::vector hw_actuator_states_; - std::vector hw_actuator_commands_; + std::vector actuator_states_; + std::vector actuator_commands_; /* For transmission interface */ - std::vector hw_actuator_transmission_passthrough_; + std::vector actuator_transmission_passthrough_; /* Actuator Wrappers (HERE change to your own wrapper) */ std::vector moteus_wrappers; @@ -129,10 +129,10 @@ namespace pi3hat_hardware_interface using JointCommand = actuator_wrappers::ActuatorCommand; /* Joint states and commands (for transmissions)*/ - std::vector hw_joint_states_; - std::vector hw_joint_commands_; + std::vector joint_states_; + std::vector joint_commands_; /* For transmission interface */ - std::vector hw_joint_transmission_passthrough_; + std::vector joint_transmission_passthrough_; /* Function for choosing wrappers (here u can add your own wrapper) Remember to change this function in source code */ @@ -147,7 +147,7 @@ namespace pi3hat_hardware_interface // TODO: Uporządkuj wcześniej silniki względem id for(int i = 0; i < number_of_actuators; i++) { - actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], hw_actuator_commands_[i]); + actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], actuator_commands_[i]); } }; @@ -157,7 +157,7 @@ namespace pi3hat_hardware_interface for(int i = 0; i < number_of_actuators; i++) { int joint_index = actuator_joint_map_[rx_can_frames_[i].id]; - actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], hw_actuator_states_[joint_index]); + actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], actuator_states_[joint_index]); } } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp index a3ad08a5..8a51b1cb 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp @@ -33,4 +33,9 @@ void IMUTransform::transform_attitude(mjbots::pi3hat::Attitude& attitude) attitude.attitude.y = -attitude.attitude.z; attitude.attitude.z = temp_y; + + /* degrees/s to radians/s conversion */ + attitude.rate_dps.x *= degrees_to_radians; + attitude.rate_dps.y *= degrees_to_radians; + attitude.rate_dps.z *= degrees_to_radians; } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 5bc29016..a5e4e36f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -15,47 +15,44 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa logger_ = std::make_unique( rclcpp::get_logger("Pi3HatHardwareInterface")); - hw_actuator_commands_.resize(info_.joints.size()); - hw_actuator_states_.resize(info_.joints.size()); - hw_actuator_transmission_passthrough_.resize(info_.joints.size()); + actuator_commands_.resize(info_.joints.size()); + actuator_states_.resize(info_.joints.size()); + actuator_transmission_passthrough_.resize(info_.joints.size()); - hw_joint_commands_.resize(info_.joints.size()); - hw_joint_states_.resize(info_.joints.size()); - hw_joint_transmission_passthrough_.resize(info_.joints.size()); + joint_commands_.resize(info_.joints.size()); + joint_states_.resize(info_.joints.size()); + joint_transmission_passthrough_.resize(info_.joints.size()); + + actuator_joint_map_.resize(info_.joints.size()); + /* Prepare wrappers */ for (const hardware_interface::ComponentInfo &joint : info_.joints) { actuator_wrappers::ActuatorParameters params; - params.bus_ = std::stoi(joint.parameters.at("can_bus")); - params.id_ = std::stoi(joint.parameters.at("can_id")); - params.direction_ = std::stoi(joint.parameters.at("direction")); - params.position_offset_ = std::stod(joint.parameters.at("position_offset")); - std::string type_string = joint.parameters.at("motor_type"); - WrapperType type = choose_actuator_wrapper(type_string); + params.bus_ = std::stoi(joint.parameters.at("motor_can_bus")); + params.id_ = std::stoi(joint.parameters.at("motor_can_id")); - for(const auto& command_interface: joint.command_interfaces) - { - if(command_interface.name == hardware_interface::HW_IF_POSITION) - { - params.position_max_ = std::stod(command_interface.max); - params.position_min_ = std::stod(command_interface.min); + params.direction_ = std::stoi(joint.parameters.at("motor_direction")); + params.position_offset_ = std::stod(joint.parameters.at("motor_position_offset")); + params.position_max_ = std::stod(joint.parameters.at("motor_position_max")); + params.position_min_ = std::stod(joint.parameters.at("motor_position_min")); + params.velocity_max_ = std::stod(joint.parameters.at("motor_velocity_max")); + params.torque_max_ = std::stod(joint.parameters.at("motor_torque_max")); - } - else if(command_interface.name == hardware_interface::HW_IF_VELOCITY) - { - params.velocity_max_ = std::stod(command_interface.max); - } - else if(command_interface.name == hardware_interface::HW_IF_EFFORT) - { - params.torque_max_ = std::stod(command_interface.max); - } - } + actuator_joint_map_.insert(actuator_joint_map_.begin() + params.id_, + moteus_wrappers.size()); + + std::string type_string = joint.parameters.at("motor_type"); + WrapperType type = choose_actuator_wrapper(type_string); add_actuator_wrapper(params, type); } - + + /* Prepare transmissions */ + create_transmission_interface(info_); + /* Standard CAN config */ mjbots::pi3hat::Pi3Hat::CanConfiguration can_config; @@ -81,9 +78,9 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa // Set up the CAN configuration for (size_t i = 0; i < info_.joints.size(); i++) // TUTAJ POMYSL O CO CMN { - config.can[hw_actuator_can_buses_[i] - 1] = can_config; - pi3hat_input_.tx_can[i].id = hw_actuator_can_ids_[i]; - pi3hat_input_.tx_can[i].bus = hw_actuator_can_buses_[i]; + config.can[actuator_can_buses_[i] - 1] = can_config; + pi3hat_input_.tx_can[i].id = actuator_can_ids_[i]; + pi3hat_input_.tx_can[i].bus = actuator_can_buses_[i]; pi3hat_input_.tx_can[i].expect_reply = true; } // Initialize the Pi3Hat @@ -93,10 +90,10 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa for(size_t i = 0; i < info_.joints.size(); i++) { auto options = mjbots::moteus::Controller::Options(); - options.id = hw_actuator_can_ids_[i]; - options.bus = hw_actuator_can_buses_[i]; + options.id = actuator_can_ids_[i]; + options.bus = actuator_can_buses_[i]; auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], rx_can_frames_[i], - hw_actuator_commands_[i], hw_actuator_states_[i], mjbots::moteus::PositionMode::Command()); + actuator_commands_[i], actuator_states_[i], mjbots::moteus::PositionMode::Command()); moteus_wrappers.push_back(moteus_wrapper); } } @@ -110,30 +107,30 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa void Pi3HatHardwareInterface::append_joint_handles(std::vector& joint_handles, const std::string joint_name, const int joint_index) { transmission_interface::JointHandle joint_handle_position(joint_name, hardware_interface::HW_IF_POSITION, - &hw_joint_transmission_passthrough_[joint_index].position_); + &joint_transmission_passthrough_[joint_index].position_); joint_handles.push_back(joint_handle_position); transmission_interface::JointHandle joint_handle_velocity(joint_name, hardware_interface::HW_IF_VELOCITY, - &hw_joint_transmission_passthrough_[joint_index].velocity_); + &joint_transmission_passthrough_[joint_index].velocity_); joint_handles.push_back(joint_handle_velocity); transmission_interface::JointHandle joint_handle_torque(joint_name, hardware_interface::HW_IF_EFFORT, - &hw_joint_transmission_passthrough_[joint_index].torque_); + &joint_transmission_passthrough_[joint_index].torque_); joint_handles.push_back(joint_handle_torque); } void Pi3HatHardwareInterface::append_actuator_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index) { transmission_interface::ActuatorHandle actuator_handle_position(actuator_name, hardware_interface::HW_IF_POSITION, - &hw_actuator_transmission_passthrough_[actuator_index].position_); + &actuator_transmission_passthrough_[actuator_index].position_); actuator_handles.push_back(actuator_handle_position); transmission_interface::ActuatorHandle actuator_handle_velocity(actuator_name, hardware_interface::HW_IF_VELOCITY, - &hw_actuator_transmission_passthrough_[actuator_index].velocity_); + &actuator_transmission_passthrough_[actuator_index].velocity_); actuator_handles.push_back(actuator_handle_velocity); transmission_interface::ActuatorHandle actuator_handle_torque(actuator_name, hardware_interface::HW_IF_EFFORT, - &hw_actuator_transmission_passthrough_[actuator_index].torque_); + &actuator_transmission_passthrough_[actuator_index].torque_); actuator_handles.push_back(actuator_handle_torque); } From 012d0c3a1c6d62a8419655dc1754246894f930dd Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 13:40:07 +0200 Subject: [PATCH 067/137] Refactoring controller pi3hat interface --- .../include/controllers/Controller.hpp | 63 +++++++++++++++++++ .../controllers/bridges/ControllerBridge.hpp | 28 +++++++++ .../controllers/bridges/MoteusBridge.hpp | 38 +++++++++++ .../src/controllers/Controller.cpp | 33 ++++++++++ .../controllers/bridges/ControllerBridge.cpp | 7 +++ .../src/controllers/bridges/MoteusBridge.cpp | 55 ++++++++++++++++ 6 files changed, 224 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp new file mode 100644 index 00000000..453d8dff --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp @@ -0,0 +1,63 @@ +#ifndef _CONTROLLER_H_ +#define _CONTROLLER_H_ + +#include "../pi3hat/pi3hat.h" +#include "bridges/ControllerBridge.hpp" +#include +#include + +namespace controller_interface +{ + +/* Structure for basic actuator command */ +struct ControllerCommand +{ + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ +}; + +/* Structure for basic actuator state */ +struct ControllerState +{ + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ + int temperature_; /* [Celcius] */ + bool fault = false; +}; + +struct ControllerParameters +{ + double position_max_; + double position_min_; + double position_offset_; + double velocity_max_; + double torque_max_; + int direction_ = 1; + int id_; /* Usage in your bridge (check moteus bridge)*/ + int bus_; /* Usage in your bridge (check moteus bridge)*/ +}; + +class Controller +{ + private: + + using CanFrame = mjbots::pi3hat::CanFrame; + + const std::unique_ptr implementation_; + ControllerParameters params_; + + + public: + Controller(std::unique_ptr implementation, + const ControllerParameters& params); + + void make_command(CanFrame& tx_frame, ControllerCommand& command) const; + void get_state(const CanFrame& rx_frame, ControllerState& state) const; + void make_stop(CanFrame& tx_frame) const; + +}; + +}; +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp new file mode 100644 index 00000000..49d6b4c5 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp @@ -0,0 +1,28 @@ +#ifndef _CONTROLLER_BRIDGE_H_ +#define _CONTROLLER_BRIDGE_H_ + +#include "../Controller.hpp" + + +namespace controller_interface +{ + +class ControllerBridge +{ + protected: + ControllerParameters params_; + + public: + using CanFrame = mjbots::pi3hat::CanFrame; + + ControllerBridge(const ControllerParameters& params); + virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; + virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; + virtual void stop_to_tx_frame(CanFrame& tx_frame) = 0; + virtual ~ControllerBridge() = 0; + +}; + +}; + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp new file mode 100644 index 00000000..2b81c76c --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp @@ -0,0 +1,38 @@ +#ifndef _MOTEUS_BRIDGE_H_ +#define _MOTEUS_BRIDGE_H_ + +#include "ControllerBridge.hpp" +#include "../../moteus/moteus.h" + +namespace controller_interface +{ + +class MoteusBridge: public ControllerBridge +{ + private: + + /* Const coefficients for easy radians - rotations transform */ + static const double rotation_to_radians = 2 * M_PI; + static const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + + /* Command structure for moteus object*/ + mjbots::moteus::PositionMode::Command position_command_; + mjbots::moteus::Controller moteus_controller_; + + public: + using CanFrame = mjbots::pi3hat::CanFrame; + + MoteusBridge( + const ControllerParameters& params, + const mjbots::moteus::Controller::Options& options, + const mjbots::moteus::PositionMode::Command& command); + void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; + void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; + void stop_to_tx_frame(CanFrame& tx_frame) override; + ~MoteusBridge() override = default; + +}; + +}; + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp new file mode 100644 index 00000000..fefc76d4 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp @@ -0,0 +1,33 @@ +#include "../../include/controllers/Controller.hpp" + +using namespace controller_interface; +using mjbots::pi3hat::CanFrame; + + +Controller::Controller(std::unique_ptr implementation, + const ControllerParameters& params): + implementation_(std::move(implementation)), params_(params) {} + + +void Controller::make_command(CanFrame& tx_frame, ControllerCommand& command) const +{ + command.position_ = params_.direction_* std::clamp(command.position_, + params_.position_min_, params_.position_max_) + params_.position_offset_; + command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); + command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); + implementation_->command_to_tx_frame(tx_frame, command); + +} + +void Controller::get_state(const CanFrame& rx_frame, ControllerState& state) const +{ + implementation_->rx_frame_to_state(rx_frame, state); + state.position_ = params_.direction_ * state.position_; + state.velocity_ = params_.direction_ * state.velocity_; + state.torque_ = params_.direction_ * state.torque_; +} + +void Controller::make_stop(CanFrame& tx_frame) const +{ + implementation_->stop_to_tx_frame(tx_frame); +} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp new file mode 100644 index 00000000..147faa99 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp @@ -0,0 +1,7 @@ +#include "../../../include/controllers/bridges/ControllerBridge.hpp" + + +using namespace controller_interface; + + +ControllerBridge::ControllerBridge(const ControllerParameters& params): params_(params) {} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp new file mode 100644 index 00000000..82b03c61 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp @@ -0,0 +1,55 @@ +#include "../../../include/controllers/bridges/MoteusBridge.hpp" + +using namespace controller_interface; + +MoteusBridge::MoteusBridge( + const ControllerParameters& params, + const mjbots::moteus::Controller::Options& options, + const mjbots::moteus::PositionMode::Command& command): + + ControllerBridge(params), + position_command_(command), + moteus_controller_(mjbots::moteus::Controller(options)) {} + + +void MoteusBridge::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) +{ + /* Change command values */ + position_command_.position = command.position_ * radians_to_rotation; + position_command_.velocity = command.velocity_ * radians_to_rotation; + position_command_.feedforward_torque = command.torque_; + + /* Create CANFD frame*/ + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(position_command_); + + /* Copy data from CANFD frame to CAN frame*/ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} + +void MoteusBridge::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) +{ + /* Parse data from RX CAN frame to Result object */ + if(((rx_frame.id >> 8) & 0x7f) != params_.id_) return; /* This should not happen! (map frame to wrapper first) */ + + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); + state.position_ = result.position * rotation_to_radians; + state.velocity_ = result.velocity * rotation_to_radians; + state.torque_ = result.torque; + state.temperature_ = result.temperature; + state.fault = result.fault; +} + +void MoteusBridge::stop_to_tx_frame(CanFrame& tx_frame) +{ + /* create CANFD frame*/ + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); + + /* Copy data from CANFD frame to CAN frame*/ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} From d79a94d4e4478ba8b44d2d68d8044acfc8330be5 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 14:13:11 +0200 Subject: [PATCH 068/137] Renaming --- .../{Controller.hpp => ControllerBridge.hpp} | 8 ++++---- .../ControllerWrapper.hpp} | 8 ++++---- .../MoteusWrapper.hpp} | 8 ++++---- .../{Controller.cpp => ControllerBridge.cpp} | 18 +++++++++--------- .../controllers/bridges/ControllerBridge.cpp | 7 ------- .../controllers/wrappers/ControllerWrapper.cpp | 7 +++++++ .../MoteusWrapper.cpp} | 12 ++++++------ 7 files changed, 34 insertions(+), 34 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/include/controllers/{Controller.hpp => ControllerBridge.hpp} (87%) rename src/meldog_hardware/pi3hat_hardware_interface/include/controllers/{bridges/ControllerBridge.hpp => wrappers/ControllerWrapper.hpp} (75%) rename src/meldog_hardware/pi3hat_hardware_interface/include/controllers/{bridges/MoteusBridge.hpp => wrappers/MoteusWrapper.hpp} (87%) rename src/meldog_hardware/pi3hat_hardware_interface/src/controllers/{Controller.cpp => ControllerBridge.cpp} (54%) delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp rename src/meldog_hardware/pi3hat_hardware_interface/src/controllers/{bridges/MoteusBridge.cpp => wrappers/MoteusWrapper.cpp} (82%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp similarity index 87% rename from src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 453d8dff..934cc0e0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controller.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -2,7 +2,7 @@ #define _CONTROLLER_H_ #include "../pi3hat/pi3hat.h" -#include "bridges/ControllerBridge.hpp" +#include "wrappers/ControllerWrapper.hpp" #include #include @@ -39,18 +39,18 @@ struct ControllerParameters int bus_; /* Usage in your bridge (check moteus bridge)*/ }; -class Controller +class ControllerBridge { private: using CanFrame = mjbots::pi3hat::CanFrame; - const std::unique_ptr implementation_; + const std::unique_ptr wrapper_; ControllerParameters params_; public: - Controller(std::unique_ptr implementation, + ControllerBridge(std::unique_ptr wrapper, const ControllerParameters& params); void make_command(CanFrame& tx_frame, ControllerCommand& command) const; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp similarity index 75% rename from src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 49d6b4c5..6f2e08ef 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -1,13 +1,13 @@ #ifndef _CONTROLLER_BRIDGE_H_ #define _CONTROLLER_BRIDGE_H_ -#include "../Controller.hpp" +#include "../ControllerBridge.hpp" namespace controller_interface { -class ControllerBridge +class ControllerWrapper { protected: ControllerParameters params_; @@ -15,11 +15,11 @@ class ControllerBridge public: using CanFrame = mjbots::pi3hat::CanFrame; - ControllerBridge(const ControllerParameters& params); + ControllerWrapper(const ControllerParameters& params); virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void stop_to_tx_frame(CanFrame& tx_frame) = 0; - virtual ~ControllerBridge() = 0; + virtual ~ControllerWrapper() = 0; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp similarity index 87% rename from src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 2b81c76c..f0732372 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/bridges/MoteusBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -1,13 +1,13 @@ #ifndef _MOTEUS_BRIDGE_H_ #define _MOTEUS_BRIDGE_H_ -#include "ControllerBridge.hpp" +#include "ControllerWrapper.hpp" #include "../../moteus/moteus.h" namespace controller_interface { -class MoteusBridge: public ControllerBridge +class MoteusWrapper: public ControllerWrapper { private: @@ -22,14 +22,14 @@ class MoteusBridge: public ControllerBridge public: using CanFrame = mjbots::pi3hat::CanFrame; - MoteusBridge( + MoteusWrapper( const ControllerParameters& params, const mjbots::moteus::Controller::Options& options, const mjbots::moteus::PositionMode::Command& command); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void stop_to_tx_frame(CanFrame& tx_frame) override; - ~MoteusBridge() override = default; + ~MoteusWrapper() override = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp similarity index 54% rename from src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index fefc76d4..fd3e7df0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/Controller.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -1,33 +1,33 @@ -#include "../../include/controllers/Controller.hpp" +#include "../../include/controllers/ControllerBridge.hpp" using namespace controller_interface; using mjbots::pi3hat::CanFrame; -Controller::Controller(std::unique_ptr implementation, +ControllerBridge::ControllerBridge(std::unique_ptr wrapper, const ControllerParameters& params): - implementation_(std::move(implementation)), params_(params) {} + wrapper_(std::move(wrapper)), params_(params) {} -void Controller::make_command(CanFrame& tx_frame, ControllerCommand& command) const +void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& command) const { command.position_ = params_.direction_* std::clamp(command.position_, params_.position_min_, params_.position_max_) + params_.position_offset_; command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); - implementation_->command_to_tx_frame(tx_frame, command); + wrapper_->command_to_tx_frame(tx_frame, command); } -void Controller::get_state(const CanFrame& rx_frame, ControllerState& state) const +void ControllerBridge::get_state(const CanFrame& rx_frame, ControllerState& state) const { - implementation_->rx_frame_to_state(rx_frame, state); + wrapper_->rx_frame_to_state(rx_frame, state); state.position_ = params_.direction_ * state.position_; state.velocity_ = params_.direction_ * state.velocity_; state.torque_ = params_.direction_ * state.torque_; } -void Controller::make_stop(CanFrame& tx_frame) const +void ControllerBridge::make_stop(CanFrame& tx_frame) const { - implementation_->stop_to_tx_frame(tx_frame); + wrapper_->stop_to_tx_frame(tx_frame); } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp deleted file mode 100644 index 147faa99..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/ControllerBridge.cpp +++ /dev/null @@ -1,7 +0,0 @@ -#include "../../../include/controllers/bridges/ControllerBridge.hpp" - - -using namespace controller_interface; - - -ControllerBridge::ControllerBridge(const ControllerParameters& params): params_(params) {} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp new file mode 100644 index 00000000..764c5f15 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp @@ -0,0 +1,7 @@ +#include "../../../include/controllers/wrappers/ControllerWrapper.hpp" + + +using namespace controller_interface; + + +ControllerWrapper::ControllerWrapper(const ControllerParameters& params): params_(params) {} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp similarity index 82% rename from src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 82b03c61..bdb8f172 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/bridges/MoteusBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -1,18 +1,18 @@ -#include "../../../include/controllers/bridges/MoteusBridge.hpp" +#include "../../../include/controllers/wrappers/MoteusWrapper.hpp" using namespace controller_interface; -MoteusBridge::MoteusBridge( +MoteusWrapper::MoteusWrapper( const ControllerParameters& params, const mjbots::moteus::Controller::Options& options, const mjbots::moteus::PositionMode::Command& command): - ControllerBridge(params), + ControllerWrapper(params), position_command_(command), moteus_controller_(mjbots::moteus::Controller(options)) {} -void MoteusBridge::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) +void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) { /* Change command values */ position_command_.position = command.position_ * radians_to_rotation; @@ -29,7 +29,7 @@ void MoteusBridge::command_to_tx_frame(CanFrame& tx_frame, const ControllerComma std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } -void MoteusBridge::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) +void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ if(((rx_frame.id >> 8) & 0x7f) != params_.id_) return; /* This should not happen! (map frame to wrapper first) */ @@ -42,7 +42,7 @@ void MoteusBridge::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state.fault = result.fault; } -void MoteusBridge::stop_to_tx_frame(CanFrame& tx_frame) +void MoteusWrapper::stop_to_tx_frame(CanFrame& tx_frame) { /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); From 95bd94509e7ef328a02acc68ef7bebb5c6bbac69 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 14:38:51 +0200 Subject: [PATCH 069/137] Updated tests --- .../include/controllers/ControllerBridge.hpp | 4 +- .../include/controllers/Controllers.hpp | 8 +++ .../wrappers/ControllerWrapper.hpp | 7 +- .../controllers/wrappers/MoteusWrapper.hpp | 7 +- .../wrappers/ControllerWrapper.cpp | 3 - .../controllers/wrappers/MoteusWrapper.cpp | 5 +- ...rapper_test.cpp => double_bridge_test.cpp} | 68 ++++++++----------- ...rapper_test.cpp => single_bridge_test.cpp} | 37 ++++------ 8 files changed, 63 insertions(+), 76 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp rename src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/{double_wrapper_test.cpp => double_bridge_test.cpp} (59%) rename src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/{single_wrapper_test.cpp => single_bridge_test.cpp} (69%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 934cc0e0..62cbe441 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -1,5 +1,5 @@ -#ifndef _CONTROLLER_H_ -#define _CONTROLLER_H_ +#ifndef _CONTROLLER_BRIDGE_HPP_ +#define _CONTROLLER_BRIDGE_HPP_ #include "../pi3hat/pi3hat.h" #include "wrappers/ControllerWrapper.hpp" diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp new file mode 100644 index 00000000..98892c04 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp @@ -0,0 +1,8 @@ +#ifndef _CONTROLLERS_HPP_ +#define _CONTROLLERS_HPP_ + +#include "ControllerBridge.hpp" +#include "wrappers/ControllerWrapper.hpp" +#include "wrappers/MoteusWrapper.hpp" + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 6f2e08ef..0f2d9fd2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -1,5 +1,5 @@ -#ifndef _CONTROLLER_BRIDGE_H_ -#define _CONTROLLER_BRIDGE_H_ +#ifndef _CONTROLLER_WRAPPER_HPP_ +#define _CONTROLLER_WRAPPER_HPP_ #include "../ControllerBridge.hpp" @@ -10,12 +10,11 @@ namespace controller_interface class ControllerWrapper { protected: - ControllerParameters params_; public: using CanFrame = mjbots::pi3hat::CanFrame; - ControllerWrapper(const ControllerParameters& params); + ControllerWrapper() = default; virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void stop_to_tx_frame(CanFrame& tx_frame) = 0; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index f0732372..8011aa35 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -1,5 +1,5 @@ -#ifndef _MOTEUS_BRIDGE_H_ -#define _MOTEUS_BRIDGE_H_ +#ifndef _MOTEUS_WRAPPER_HPP_ +#define _MOTEUS_WRAPPER_HPP_ #include "ControllerWrapper.hpp" #include "../../moteus/moteus.h" @@ -22,8 +22,7 @@ class MoteusWrapper: public ControllerWrapper public: using CanFrame = mjbots::pi3hat::CanFrame; - MoteusWrapper( - const ControllerParameters& params, + MoteusWrapper( const mjbots::moteus::Controller::Options& options, const mjbots::moteus::PositionMode::Command& command); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp index 764c5f15..05c9a19e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp @@ -2,6 +2,3 @@ using namespace controller_interface; - - -ControllerWrapper::ControllerWrapper(const ControllerParameters& params): params_(params) {} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index bdb8f172..74018e86 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -3,11 +3,10 @@ using namespace controller_interface; MoteusWrapper::MoteusWrapper( - const ControllerParameters& params, const mjbots::moteus::Controller::Options& options, const mjbots::moteus::PositionMode::Command& command): - ControllerWrapper(params), + ControllerWrapper(), position_command_(command), moteus_controller_(mjbots::moteus::Controller(options)) {} @@ -32,7 +31,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != params_.id_) return; /* This should not happen! (map frame to wrapper first) */ + if(((rx_frame.id >> 8) & 0x7f) != moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp similarity index 59% rename from src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp index d2f9449b..e63cd426 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp @@ -1,4 +1,4 @@ -#include "../../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../include/controllers/Controllers.hpp" @@ -19,23 +19,6 @@ static double GetNow() static_cast(ts.tv_nsec) / 1e9; }; -template -void init(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame) -{ - wrapper.init(tx_frame); -}; - -template -void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, actuator_wrappers::ActuatorCommand& command) -{ - wrapper.command_to_tx_frame(tx_frame, command); -}; -template -void state(actuator_wrappers::ActuatorWrapperBase& wrapper,const mjbots::pi3hat::CanFrame& rx_frame, actuator_wrappers::ActuatorState& state) -{ - wrapper.rx_frame_to_state(rx_frame, state); -} - int main(int argc, char** argv) { @@ -87,7 +70,7 @@ int main(int argc, char** argv) mjbots::pi3hat::Pi3Hat::Output pi3hat_output; // moteus wrapper - actuator_wrappers::ActuatorParameters params_1; + controller_interface::ControllerParameters params_1; params_1.direction_ = 1; params_1.position_max_ = 30; params_1.position_min_ = -30; @@ -96,7 +79,7 @@ int main(int argc, char** argv) params_1.bus_ = 1; params_1.id_ = 1; - actuator_wrappers::ActuatorParameters params_2; + controller_interface::ControllerParameters params_2; params_2.direction_ = 1; params_2.position_max_ = 10; params_2.position_min_ = -10; @@ -105,18 +88,27 @@ int main(int argc, char** argv) params_2.bus_ = 2; params_2.id_ = 2; - std::vector actuator_wrappers; - std::vector actuator_commands; - std::vector actuator_states; - actuator_wrappers.push_back(actuator_wrappers::MoteusWrapper(params_1, moteus_1_options, moteus_command)); - actuator_wrappers.push_back(actuator_wrappers::MoteusWrapper(params_2, moteus_2_options, moteus_command)); + std::vector controllers; + std::vector controller_commands; + std::vector controller_states; + + controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_command); + std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); + controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); + + controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_command); + std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); + controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); + + controllers.push_back(std::move(controller_1)); + controllers.push_back(std::move(controller_2)); - actuator_commands.push_back(actuator_wrappers::ActuatorCommand()); - actuator_commands.push_back(actuator_wrappers::ActuatorCommand()); + controller_commands.push_back(controller_interface::ControllerCommand()); + controller_commands.push_back(controller_interface::ControllerCommand()); - actuator_states.push_back(actuator_wrappers::ActuatorState()); - actuator_states.push_back(actuator_wrappers::ActuatorState()); + controller_states.push_back(controller_interface::ControllerState()); + controller_states.push_back(controller_interface::ControllerState()); std::cout << "Options for controllers succesfully initialized!" << std::endl; @@ -126,9 +118,9 @@ int main(int argc, char** argv) std::cout << "Realtime control activated!" << std::endl; // set stop to moteus - for(int i = 0; i < actuator_wrappers.size(); i++) + for(int i = 0; i < controllers.size(); ++i) { - init(actuator_wrappers[i],tx_frame[i]); + controllers[i].make_stop(tx_frame[i]); } pi3hat_output = pi3hat.Cycle(input); std::cout << "Controllers successfully started!" << std::endl; @@ -138,22 +130,22 @@ int main(int argc, char** argv) while(true) { auto now = GetNow(); - actuator_commands[0].position_ = 20 * cos(now - prev); - actuator_commands[1].position_ = 10 * sin(now - prev); - for(int i = 0; i < actuator_wrappers.size(); i++) + controller_commands[0].position_ = 20 * cos(now - prev); + controller_commands[1].position_ = 10 * sin(now - prev); + for(int i = 0; i < controllers.size(); ++i) { - command(actuator_wrappers[i],tx_frame[i],actuator_commands[i]); + controllers[i].make_command(tx_frame[i], controller_commands[i]); } pi3hat_output = pi3hat.Cycle(input); ::usleep(1000); auto mesaure_time = GetNow() - now; frequency = (int) 1/mesaure_time; - for(int i = 0; i < actuator_wrappers.size(); i++) + for(int i = 0; i < controllers.size(); ++i) { - state(actuator_wrappers[i],rx_frame[i],actuator_states[i]); + controllers[i].get_state(rx_frame[i], controller_states[i]); } ::printf("f = %d\n pos_1_command = %7.3f, pos_1_state = %7.3f)\n pos_2_command = %7.3f, pos_2_state = %7.3f)\r", - frequency, actuator_commands[0].position_, actuator_states[0].position_, actuator_commands[1].position_, actuator_states[1].position_); + frequency, controller_commands[0].position_, controller_states[0].position_, controller_commands[1].position_, controller_states[1].position_); ::fflush(::stdout); } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp similarity index 69% rename from src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp index 3eb2bba1..3dc97ea0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_wrapper_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp @@ -1,4 +1,4 @@ -#include "../../include/actuator_wrappers/MoteusWrapper.hpp" +#include "../../include/controllers/Controllers.hpp" @@ -17,16 +17,6 @@ static double GetNow() static_cast(ts.tv_nsec) / 1e9; }; -template -void command(actuator_wrappers::ActuatorWrapperBase& wrapper, mjbots::pi3hat::CanFrame& tx_frame, actuator_wrappers::ActuatorCommand& command) -{ - wrapper.command_to_tx_frame(tx_frame, command); -}; -template -void state(actuator_wrappers::ActuatorWrapperBase& wrapper,const mjbots::pi3hat::CanFrame& rx_frame, actuator_wrappers::ActuatorState& state) -{ - wrapper.rx_frame_to_state(rx_frame, state); -} int main(int argc, char** argv) { // moteus options @@ -71,7 +61,7 @@ int main(int argc, char** argv) mjbots::pi3hat::Pi3Hat::Output pi3hat_output; // moteus wrapper - actuator_wrappers::ActuatorParameters params; + controller_interface::ControllerParameters params; params.direction_ = 1; params.position_max_ = 10; params.position_min_ = -10; @@ -80,12 +70,15 @@ int main(int argc, char** argv) params.bus_ = 1; params.id_ = 1; - actuator_wrappers::MoteusWrapper moteus_wrapper(params, moteus_options, moteus_command); - actuator_wrappers::ActuatorCommand actuator_command; - actuator_command.velocity_ = 0; - actuator_command.torque_ = 0; + controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); + + controller_interface::ControllerCommand controller_command; + controller_command.velocity_ = 0; + controller_command.torque_ = 0; - actuator_wrappers::ActuatorState actuator_state; + controller_interface::ControllerState controller_state; std::cout << "Options for controller succesfully initialized!" << std::endl; @@ -94,7 +87,7 @@ int main(int argc, char** argv) std::cout << "Realtime control activated!" << std::endl; // set stop to moteus - moteus_wrapper.init(tx_frame); + controller.make_stop(tx_frame); pi3hat_output = pi3hat.Cycle(input); std::cout << "Controller successfully started!" << std::endl; @@ -103,15 +96,15 @@ int main(int argc, char** argv) while(true) { auto now = GetNow(); - actuator_command.position_ = 5 * sin(now - prev); - command(moteus_wrapper, tx_frame, actuator_command); + controller_command.position_ = 5 * sin(now - prev); + controller.make_command(tx_frame, controller_command); pi3hat_output = pi3hat.Cycle(input); ::usleep(1000); auto mesaure_time = GetNow() - now; frequency = (int) 1/mesaure_time; - state(moteus_wrapper, rx_frame, actuator_state); + controller.get_state(rx_frame, controller_state); ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", - frequency, actuator_command.position_, actuator_state.position_); + frequency, controller_command.position_, controller_state.position_); ::fflush(::stdout); } From 5629a975c8b4ad811a18f31d1f9c0dcb4810abbd Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 14:44:39 +0200 Subject: [PATCH 070/137] Name changing and added directory for 3rd party libraries --- .../include/{ => 3rd_libs}/moteus/moteus.h | 0 .../include/{ => 3rd_libs}/moteus/moteus_multiplex.h | 0 .../include/{ => 3rd_libs}/moteus/moteus_optional.h | 0 .../include/{ => 3rd_libs}/moteus/moteus_protocol.h | 0 .../include/{ => 3rd_libs}/moteus/moteus_tokenizer.h | 0 .../include/{ => 3rd_libs}/moteus/moteus_transport.h | 0 .../include/{ => 3rd_libs}/pi3hat/pi3hat.cc | 0 .../include/{ => 3rd_libs}/pi3hat/pi3hat.h | 0 .../include/{ => 3rd_libs}/pi3hat/pi3hat_moteus_transport.h | 0 .../include/{ => 3rd_libs}/pi3hat/pi3hat_tool.cc | 0 .../include/{ => 3rd_libs}/pi3hat/realtime.h | 0 .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 2 +- .../include/controllers/ControllerBridge.hpp | 2 +- .../include/controllers/wrappers/MoteusWrapper.hpp | 2 +- .../include/{imu => imu_transform}/IMUTransform.hpp | 2 +- .../src/{imu => imu_transform}/IMUTransform.cpp | 2 +- .../tests/hardware/simple_test.cpp | 6 +++--- 17 files changed, 8 insertions(+), 8 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus_multiplex.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus_optional.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus_protocol.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus_tokenizer.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/moteus/moteus_transport.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/pi3hat/pi3hat.cc (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/pi3hat/pi3hat.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/pi3hat/pi3hat_moteus_transport.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/pi3hat/pi3hat_tool.cc (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{ => 3rd_libs}/pi3hat/realtime.h (100%) rename src/meldog_hardware/pi3hat_hardware_interface/include/{imu => imu_transform}/IMUTransform.hpp (90%) rename src/meldog_hardware/pi3hat_hardware_interface/src/{imu => imu_transform}/IMUTransform.cpp (96%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_multiplex.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_multiplex.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_multiplex.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_optional.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_optional.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_optional.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_protocol.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_protocol.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_protocol.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_tokenizer.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_tokenizer.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_tokenizer.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_transport.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/moteus/moteus_transport.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/moteus/moteus_transport.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat.cc similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.cc rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat.cc diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat_moteus_transport.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_moteus_transport.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat_moteus_transport.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat_tool.cc similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/pi3hat_tool.cc rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/pi3hat_tool.cc diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h b/src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/realtime.h similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat/realtime.h rename to src/meldog_hardware/pi3hat_hardware_interface/include/3rd_libs/pi3hat/realtime.h diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 538b1a77..4b5621a7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -1,7 +1,7 @@ #ifndef _MOTOR_WRAPPER_BASE_ #define _MOTOR_WRAPPER_BASE_ -#include "../pi3hat/pi3hat.h" +#include "../3rd_libs/pi3hat/pi3hat.h" #include /* Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 62cbe441..2bac7532 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -1,7 +1,7 @@ #ifndef _CONTROLLER_BRIDGE_HPP_ #define _CONTROLLER_BRIDGE_HPP_ -#include "../pi3hat/pi3hat.h" +#include "../3rd_libs/pi3hat/pi3hat.h" #include "wrappers/ControllerWrapper.hpp" #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 8011aa35..235404b4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -2,7 +2,7 @@ #define _MOTEUS_WRAPPER_HPP_ #include "ControllerWrapper.hpp" -#include "../../moteus/moteus.h" +#include "../../3rd_libs/moteus/moteus.h" namespace controller_interface { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp similarity index 90% rename from src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp rename to src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp index 50913fa3..4f1c0184 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/imu/IMUTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp @@ -2,7 +2,7 @@ #define _IMU_ #include -#include "../pi3hat/pi3hat.h" +#include "../3rd_libs/pi3hat/pi3hat.h" #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp similarity index 96% rename from src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp index 8a51b1cb..cb3ec9a6 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/imu/IMUTransform.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp @@ -1,4 +1,4 @@ -#include "../../include/imu/IMUTransform.hpp" +#include "../../include/imu_transform/IMUTransform.hpp" using namespace IMU; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp index 99cf0678..cd92f5bf 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp @@ -1,6 +1,6 @@ -#include "../../include/pi3hat/pi3hat.h" -#include "../../include/pi3hat/realtime.h" -#include "../../include/moteus/moteus.h" +#include "../../include/3rd_libs/pi3hat/pi3hat.h" +#include "../../include/3rd_libs/pi3hat/realtime.h" +#include "../../include/3rd_libs/moteus/moteus.h" static double GetNow() From 79db0e7d15d1e618853d013fe719b2e2a0433c6b Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 17:04:13 +0200 Subject: [PATCH 071/137] Few fixes, refactoring init function --- .../pi3hat_hardware_interface.hpp | 67 +++++---- .../src/pi3hat_hardware_interface.cpp | 132 +++++++++++------- .../tests/hardware/double_bridge_test.cpp | 8 +- .../tests/hardware/single_bridge_test.cpp | 6 +- 4 files changed, 119 insertions(+), 94 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 12fa3e3a..474587c8 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -33,8 +33,8 @@ #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_interface_exception.hpp" -#include "../actuator_wrappers/MoteusWrapper.hpp" -#include "../imu/IMUTransform.hpp" +#include "../controllers/Controllers.hpp" +#include "../imu_transform/IMUTransform.hpp" #include "../pi3hat/pi3hat.h" #include "../pi3hat/realtime.h" @@ -79,7 +79,7 @@ namespace pi3hat_hardware_interface private: - /* Here add your actuator wrapper type */ + /* Here add your controller wrapper type */ enum WrapperType { Moteus = 0, @@ -90,8 +90,8 @@ namespace pi3hat_hardware_interface /* PART FOR COMMUNICATION WITH HARDWARE: */ - /* number of actuators */ - size_t number_of_actuators; + /* number of controllers */ + size_t number_of_controllers; /* Pi3hat */ std::shared_ptr pi3hat_; @@ -112,21 +112,21 @@ namespace pi3hat_hardware_interface std::shared_ptr rx_can_frames_; /* Container for motor_id -> joint_index maping */ - std::vector actuator_joint_map_; + std::vector controller_joint_map_; - /* Actuator states and commands */ - std::vector actuator_states_; - std::vector actuator_commands_; + /* Controller states and commands */ + std::vector controller_states_; + std::vector controller_commands_; /* For transmission interface */ - std::vector actuator_transmission_passthrough_; + std::vector controller_transmission_passthrough_; - /* Actuator Wrappers (HERE change to your own wrapper) */ - std::vector moteus_wrappers; + /* Controller Wrappers (HERE change to your own wrapper) */ + std::vector controller_bridges; - using JointState = actuator_wrappers::ActuatorState; - using JointCommand = actuator_wrappers::ActuatorCommand; + using JointState = controller_interface::ControllerState; + using JointCommand = controller_interface::ControllerCommand; /* Joint states and commands (for transmissions)*/ std::vector joint_states_; @@ -134,34 +134,26 @@ namespace pi3hat_hardware_interface /* For transmission interface */ std::vector joint_transmission_passthrough_; + + /* FUNCTION FOR INITIALIZATION */ + /* Function for choosing wrappers (here u can add your own wrapper) Remember to change this function in source code */ - WrapperType choose_actuator_wrapper(const std::string& type); + WrapperType choose_wrapper_type(const std::string& type); /* Function for creating moteus wrappers (here u can add your own wrapper) */ - void add_actuator_wrapper(const actuator_wrappers::ActuatorParameters& params, const WrapperType type); + void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); - template - void commands_to_tx_frames(std::vector> actuator_wrappers) - { - // TODO: Uporządkuj wcześniej silniki względem id - for(int i = 0; i < number_of_actuators; i++) - { - actuator_wrappers[i].command_to_tx_frame(tx_can_frames_[i], actuator_commands_[i]); - } - }; + ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); - template - void rx_frames_to_states(std::vector> actuator_wrappers) - { - for(int i = 0; i < number_of_actuators; i++) - { - int joint_index = actuator_joint_map_[rx_can_frames_[i].id]; - actuator_wrappers[joint_index].rx_frame_to_state(rx_can_frames_[i], actuator_states_[joint_index]); - } - } - /* PART FOR CREATING TRANSMISSION OBJECTS:*/ + /* FUNCTION FOR MAKING COMMANDS/READING STATES */ + + void prepare_commands(); + + void get_states(); + + /* FUNCTIONS FOR CREATING TRANSMISSION OBJECTS:*/ /* Transmission interfaces*/ std::vector> transmissions_; @@ -189,8 +181,13 @@ namespace pi3hat_hardware_interface /* Functions for creating joint and actuator handels */ void append_joint_handles(std::vector& joint_handles, const std::string joint_name, const int joint_index); + void append_actuator_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index); + + + /* FUNCTIONS FOR INITIALIZING PI3HAT/CAN INTERFACE */ + }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index a5e4e36f..84229f20 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,7 +1,7 @@ #include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; -using namespace actuator_wrappers; +using namespace controller_interface; /* MAIN FUNCTIONS */ @@ -15,39 +15,38 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa logger_ = std::make_unique( rclcpp::get_logger("Pi3HatHardwareInterface")); - actuator_commands_.resize(info_.joints.size()); - actuator_states_.resize(info_.joints.size()); - actuator_transmission_passthrough_.resize(info_.joints.size()); + controller_commands_.resize(info_.joints.size()); + controller_states_.resize(info_.joints.size()); + controller_transmission_passthrough_.resize(info_.joints.size()); joint_commands_.resize(info_.joints.size()); joint_states_.resize(info_.joints.size()); joint_transmission_passthrough_.resize(info_.joints.size()); - actuator_joint_map_.resize(info_.joints.size()); + controller_joint_map_.resize(info_.joints.size()); - /* Prepare wrappers */ + /* Prepare controller bridges */ for (const hardware_interface::ComponentInfo &joint : info_.joints) { - actuator_wrappers::ActuatorParameters params; - - params.bus_ = std::stoi(joint.parameters.at("motor_can_bus")); - params.id_ = std::stoi(joint.parameters.at("motor_can_id")); - - params.direction_ = std::stoi(joint.parameters.at("motor_direction")); - params.position_offset_ = std::stod(joint.parameters.at("motor_position_offset")); - params.position_max_ = std::stod(joint.parameters.at("motor_position_max")); - params.position_min_ = std::stod(joint.parameters.at("motor_position_min")); - params.velocity_max_ = std::stod(joint.parameters.at("motor_velocity_max")); - params.torque_max_ = std::stod(joint.parameters.at("motor_torque_max")); - - actuator_joint_map_.insert(actuator_joint_map_.begin() + params.id_, - moteus_wrappers.size()); - - std::string type_string = joint.parameters.at("motor_type"); - WrapperType type = choose_actuator_wrapper(type_string); - add_actuator_wrapper(params, type); + controller_interface::ControllerParameters params; + WrapperType type; + try + { + params = get_controller_parameters(joint); + controller_joint_map_.insert(controller_joint_map_.begin() + params.id_, + controller_bridges.size()); + std::string type_string = joint.parameters.at("motor_type"); + type = choose_wrapper_type(type_string); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "Error reading motor/controller parameters!"); + return hardware_interface::CallbackReturn::ERROR; + } + + add_controller_bridge(params, type); } /* Prepare transmissions */ @@ -60,9 +59,18 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa mjbots::pi3hat::Pi3Hat::Configuration config; config.attitude_rate_hz = 1000; /* Set the mounting orientation of the IMU */ - config.mounting_deg.yaw = std::stod(info_.hardware_parameters.at("imu_mounting_deg.yaw")); - config.mounting_deg.pitch = std::stod(info_.hardware_parameters.at("imu_mounting_deg.pitch")); - config.mounting_deg.roll = std::stod(info_.hardware_parameters.at("imu_mounting_deg.roll")); + try + { + config.mounting_deg.yaw = std::stod(info_.hardware_parameters.at("imu_mounting_deg.yaw")); + config.mounting_deg.pitch = std::stod(info_.hardware_parameters.at("imu_mounting_deg.pitch")); + config.mounting_deg.roll = std::stod(info_.hardware_parameters.at("imu_mounting_deg.roll")); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "Error reading IMU parameters!"); + return hardware_interface::CallbackReturn::ERROR; + } + /* Initialize the Pi3Hat input */ pi3hat_input_ = mjbots::pi3hat::Pi3Hat::Input(); pi3hat_input_.attitude = &attitude_; @@ -76,26 +84,19 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa pi3hat_input_.tx_can = tx_can_frames_span_; // Set up the CAN configuration - for (size_t i = 0; i < info_.joints.size(); i++) // TUTAJ POMYSL O CO CMN + for (size_t i = 0; i < info_.joints.size(); i++) { - config.can[actuator_can_buses_[i] - 1] = can_config; - pi3hat_input_.tx_can[i].id = actuator_can_ids_[i]; - pi3hat_input_.tx_can[i].bus = actuator_can_buses_[i]; - pi3hat_input_.tx_can[i].expect_reply = true; + pi3hat_input_.tx_can[i].expect_reply = true; } + + config.can[0] = can_config; + config.can[1] = can_config; + config.can[2] = can_config; + config.can[3] = can_config; + config.can[4] = can_config; + // Initialize the Pi3Hat pi3hat_ = std::make_shared(config); - - /* Create actuator wrappers*/ - for(size_t i = 0; i < info_.joints.size(); i++) - { - auto options = mjbots::moteus::Controller::Options(); - options.id = actuator_can_ids_[i]; - options.bus = actuator_can_buses_[i]; - auto moteus_wrapper = MoteusWrapper(options, tx_can_frames_[i], rx_can_frames_[i], - actuator_commands_[i], actuator_states_[i], mjbots::moteus::PositionMode::Command()); - moteus_wrappers.push_back(moteus_wrapper); - } } @@ -122,15 +123,15 @@ void Pi3HatHardwareInterface::append_joint_handles(std::vector& actuator_handles, const std::string actuator_name, const int actuator_index) { transmission_interface::ActuatorHandle actuator_handle_position(actuator_name, hardware_interface::HW_IF_POSITION, - &actuator_transmission_passthrough_[actuator_index].position_); + &controller_transmission_passthrough_[actuator_index].position_); actuator_handles.push_back(actuator_handle_position); transmission_interface::ActuatorHandle actuator_handle_velocity(actuator_name, hardware_interface::HW_IF_VELOCITY, - &actuator_transmission_passthrough_[actuator_index].velocity_); + &controller_transmission_passthrough_[actuator_index].velocity_); actuator_handles.push_back(actuator_handle_velocity); transmission_interface::ActuatorHandle actuator_handle_torque(actuator_name, hardware_interface::HW_IF_EFFORT, - &actuator_transmission_passthrough_[actuator_index].torque_); + &controller_transmission_passthrough_[actuator_index].torque_); actuator_handles.push_back(actuator_handle_torque); } @@ -385,36 +386,63 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmis return hardware_interface::CallbackReturn::SUCCESS; } -void Pi3HatHardwareInterface::add_actuator_wrapper(const ActuatorParameters& params, const WrapperType type) +void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& params, const WrapperType type) { + std::unique_ptr wrapper_ptr; switch(type) { case Moteus: /* moteus options */ using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; Controller::Options moteus_options; moteus_options.bus = params.bus_; moteus_options.id = params.id_; - // moteus command format (it will be copied to wrapper) + /* moteus command format (it will be copied to wrapper) */ mjbots::moteus::PositionMode::Format format; format.feedforward_torque = mjbots::moteus::kFloat; format.maximum_torque = mjbots::moteus::kFloat; format.velocity_limit= mjbots::moteus::kFloat; moteus_options.position_format = format; - //moteus command (it will be copied to wrapper) + /* moteus command (it will be copied to wrapper) */ mjbots::moteus::PositionMode::Command moteus_command; + + wrapper_ptr = std::make_unique(moteus_options, moteus_command); - moteus_wrappers.push_back(MoteusWrapper(params, moteus_options, moteus_command)); break; - } + } + + controller_bridges.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); } -Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_actuator_wrapper(const std::string& type) +Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) { if(type == "moteus") { return WrapperType::Moteus; }; +} + +ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const hardware_interface::ComponentInfo& joint_info) +{ + ControllerParameters params; + try + { + params.bus_ = std::stoi(joint_info.parameters.at("controller_can_bus")); + params.id_ = std::stoi(joint_info.parameters.at("controller_can_id")); + params.direction_ = std::stoi(joint_info.parameters.at("motor_direction")); + params.position_offset_ = std::stod(joint_info.parameters.at("motor_position_offset")); + params.position_max_ = std::stod(joint_info.parameters.at("motor_position_max")); + params.position_min_ = std::stod(joint_info.parameters.at("motor_position_min")); + params.velocity_max_ = std::stod(joint_info.parameters.at("motor_velocity_max")); + params.torque_max_ = std::stod(joint_info.parameters.at("motor_torque_max")); + } + catch(const std::exception& e) + { + throw; + } + + return params; } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp index e63cd426..c7ba729d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp @@ -2,8 +2,8 @@ -#include "../../include/pi3hat/pi3hat.h" -#include "../../include/pi3hat/realtime.h" +#include "../../include/3rd_libs/pi3hat/pi3hat.h" +#include "../../include/3rd_libs/pi3hat/realtime.h" #include #include #include @@ -94,11 +94,11 @@ int main(int argc, char** argv) std::vector controller_states; controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_command); - std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); + std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_command); - std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); + std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); controllers.push_back(std::move(controller_1)); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp index 3dc97ea0..747d9e3b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp @@ -2,8 +2,8 @@ -#include "../../include/pi3hat/pi3hat.h" -#include "../../include/pi3hat/realtime.h" +#include "../../include/3rd_libs/pi3hat/pi3hat.h" +#include "../../include/3rd_libs/pi3hat/realtime.h" #include #include #include @@ -71,7 +71,7 @@ int main(int argc, char** argv) params.id_ = 1; controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); - std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); controller_interface::ControllerCommand controller_command; From 2362b9140a6cd0a7a0bf73d288ea027ed8d6e0d6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 26 Jul 2024 20:47:50 +0200 Subject: [PATCH 072/137] Removed changes --- .../include/controllers/ControllerBridge.hpp | 2 +- .../include/controllers/wrappers/ControllerWrapper.hpp | 1 + .../src/controllers/ControllerBridge.cpp | 5 +++-- .../src/controllers/wrappers/MoteusWrapper.cpp | 2 +- .../src/pi3hat_hardware_interface.cpp | 4 +++- .../tests/hardware/single_bridge_test.cpp | 3 ++- 6 files changed, 11 insertions(+), 6 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 2bac7532..90d19d4c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -53,7 +53,7 @@ class ControllerBridge ControllerBridge(std::unique_ptr wrapper, const ControllerParameters& params); - void make_command(CanFrame& tx_frame, ControllerCommand& command) const; + void make_command(CanFrame& tx_frame, ControllerCommand& command) const; //POMYSL JESZCZE O TYM void get_state(const CanFrame& rx_frame, ControllerState& state) const; void make_stop(CanFrame& tx_frame) const; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 0f2d9fd2..4b5c5998 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -18,6 +18,7 @@ class ControllerWrapper virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void stop_to_tx_frame(CanFrame& tx_frame) = 0; + virtual ~ControllerWrapper() = 0; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index fd3e7df0..877f8705 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -4,9 +4,10 @@ using namespace controller_interface; using mjbots::pi3hat::CanFrame; -ControllerBridge::ControllerBridge(std::unique_ptr wrapper, +ControllerBridge::ControllerBridge( + std::unique_ptr wrapper, const ControllerParameters& params): - wrapper_(std::move(wrapper)), params_(params) {} + wrapper_(std::move(wrapper)), params_(params){} void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& command) const diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 74018e86..f80bbb47 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -21,7 +21,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm /* Create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(position_command_); - /* Copy data from CANFD frame to CAN frame*/ + /* Copy data from CANFD frame to CAN frame */ tx_frame.id = can_fd_frame.arbitration_id; tx_frame.bus = can_fd_frame.bus; tx_frame.size = can_fd_frame.size; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 84229f20..b10c3cf0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -83,9 +83,11 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), info_.joints.size()); pi3hat_input_.tx_can = tx_can_frames_span_; - // Set up the CAN configuration + /* Set up CAN TX frames */ for (size_t i = 0; i < info_.joints.size(); i++) { + pi3hat_input_.tx_can[i].id = controller_bridges[i].get_can_id(); + pi3hat_input_.tx_can[i].bus = controller_bridges[i].get_can_bus(); pi3hat_input_.tx_can[i].expect_reply = true; } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp index 747d9e3b..fb8f0152 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp @@ -72,7 +72,8 @@ int main(int argc, char** argv) controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); - controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); + controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); + controller_interface::ControllerCommand controller_command; controller_command.velocity_ = 0; From 5241294cdeb47a154c783f346cbc7b9444da6ee0 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 27 Jul 2024 14:05:25 +0200 Subject: [PATCH 073/137] Changed controller bridge --- .../include/controllers/ControllerBridge.hpp | 4 +-- .../wrappers/ControllerWrapper.hpp | 3 +- .../controllers/wrappers/MoteusWrapper.hpp | 9 ++++-- .../src/controllers/ControllerBridge.cpp | 19 ++++++++++-- .../controllers/wrappers/MoteusWrapper.cpp | 31 ++++++++++++++++--- 5 files changed, 53 insertions(+), 13 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 90d19d4c..c259eebf 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -55,8 +55,8 @@ class ControllerBridge void make_command(CanFrame& tx_frame, ControllerCommand& command) const; //POMYSL JESZCZE O TYM void get_state(const CanFrame& rx_frame, ControllerState& state) const; - void make_stop(CanFrame& tx_frame) const; - + void initialize(CanFrame& tx_frame) const; + void start_up(CanFrame& tx_frame, ControllerCommand& command) const; }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 4b5c5998..691631c9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -17,7 +17,8 @@ class ControllerWrapper ControllerWrapper() = default; virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; - virtual void stop_to_tx_frame(CanFrame& tx_frame) = 0; + virtual void init_to_tx_frame(CanFrame& tx_frame) = 0; + virtual void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual ~ControllerWrapper() = 0; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 235404b4..8107aaf7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -12,8 +12,9 @@ class MoteusWrapper: public ControllerWrapper private: /* Const coefficients for easy radians - rotations transform */ - static const double rotation_to_radians = 2 * M_PI; - static const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + static const double rotation_to_radians_ = 2 * M_PI; + static const double radians_to_rotation_ = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + static const double startup_coefficient_ = 0.05; /* For slow start-up */ /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; @@ -27,7 +28,9 @@ class MoteusWrapper: public ControllerWrapper const mjbots::moteus::PositionMode::Command& command); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; - void stop_to_tx_frame(CanFrame& tx_frame) override; + void init_to_tx_frame(CanFrame& tx_frame) override; + void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; + ~MoteusWrapper() override = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 877f8705..57b7fb5a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -16,6 +16,9 @@ void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& comma params_.position_min_, params_.position_max_) + params_.position_offset_; command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); + + tx_frame.expect_reply = true; + wrapper_->command_to_tx_frame(tx_frame, command); } @@ -28,7 +31,19 @@ void ControllerBridge::get_state(const CanFrame& rx_frame, ControllerState& stat state.torque_ = params_.direction_ * state.torque_; } -void ControllerBridge::make_stop(CanFrame& tx_frame) const +void ControllerBridge::initialize(CanFrame& tx_frame) const +{ + tx_frame.expect_reply = true; + wrapper_->init_to_tx_frame(tx_frame); +} + +void ControllerBridge::start_up(CanFrame& tx_frame, ControllerCommand& command) const { - wrapper_->stop_to_tx_frame(tx_frame); + command.position_ = params_.direction_* std::clamp(command.position_, + params_.position_min_, params_.position_max_) + params_.position_offset_; + command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); + command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); + + tx_frame.expect_reply = true; + wrapper_->start_pos_to_tx_frame(tx_frame, command); } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index f80bbb47..1db7222d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -14,8 +14,8 @@ MoteusWrapper::MoteusWrapper( void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) { /* Change command values */ - position_command_.position = command.position_ * radians_to_rotation; - position_command_.velocity = command.velocity_ * radians_to_rotation; + position_command_.position = command.position_ * radians_to_rotation_; + position_command_.velocity = command.velocity_ * radians_to_rotation_; position_command_.feedforward_torque = command.torque_; /* Create CANFD frame*/ @@ -34,14 +34,14 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& if(((rx_frame.id >> 8) & 0x7f) != moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); - state.position_ = result.position * rotation_to_radians; - state.velocity_ = result.velocity * rotation_to_radians; + state.position_ = result.position * rotation_to_radians_; + state.velocity_ = result.velocity * rotation_to_radians_; state.torque_ = result.torque; state.temperature_ = result.temperature; state.fault = result.fault; } -void MoteusWrapper::stop_to_tx_frame(CanFrame& tx_frame) +void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) { /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); @@ -52,3 +52,24 @@ void MoteusWrapper::stop_to_tx_frame(CanFrame& tx_frame) tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } + +void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) +{ + /* Change command values for start-up (crate seperate command struct) */ + mjbots::moteus::PositionMode::Command start_command; + + start_command.position = command.position_ * radians_to_rotation_; + start_command.velocity = command.velocity_ * radians_to_rotation_; + start_command.feedforward_torque = command.torque_; + start_command.maximum_torque = startup_coefficient_ * position_command_.maximum_torque; + start_command.velocity_limit = startup_coefficient_ * position_command_.velocity_limit * radians_to_rotation_; + + /* Create CANFD frame*/ + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(start_command); + + /* Copy data from CANFD frame to CAN frame */ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} From ac3d56638d89627dfed1faefe376e7b16ed76728 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 27 Jul 2024 14:12:28 +0200 Subject: [PATCH 074/137] Implemented on_configure() and added new helper functions --- .../pi3hat_hardware_interface.hpp | 34 ++- .../src/pi3hat_hardware_interface.cpp | 257 +++++++++++++----- 2 files changed, 215 insertions(+), 76 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 474587c8..bf064448 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -88,10 +88,10 @@ namespace pi3hat_hardware_interface /* UTILITY ROS2 OBJECTS: */ std::unique_ptr logger_; - /* PART FOR COMMUNICATION WITH HARDWARE: */ + /* Number of controllers/joints */ + int joint_controller_number_; - /* number of controllers */ - size_t number_of_controllers; + /* PART FOR COMMUNICATION WITH HARDWARE: */ /* Pi3hat */ std::shared_ptr pi3hat_; @@ -111,8 +111,8 @@ namespace pi3hat_hardware_interface /* RX CAN frames */ std::shared_ptr rx_can_frames_; - /* Container for motor_id -> joint_index maping */ - std::vector controller_joint_map_; + /* Container for rx_frame_id (diffrent for diffrent controller type) to joint_index maping */ + std::unordered_map controller_joint_map_; /* Controller states and commands */ std::vector controller_states_; @@ -121,7 +121,7 @@ namespace pi3hat_hardware_interface /* For transmission interface */ std::vector controller_transmission_passthrough_; - /* Controller Wrappers (HERE change to your own wrapper) */ + /* Controller Bridges */ std::vector controller_bridges; @@ -131,6 +131,7 @@ namespace pi3hat_hardware_interface /* Joint states and commands (for transmissions)*/ std::vector joint_states_; std::vector joint_commands_; + /* For transmission interface */ std::vector joint_transmission_passthrough_; @@ -141,17 +142,24 @@ namespace pi3hat_hardware_interface Remember to change this function in source code */ WrapperType choose_wrapper_type(const std::string& type); + std::unique_ptr create_moteus_wrapper(const ControllerParameters& params); + /* Function for creating moteus wrappers (here u can add your own wrapper) */ void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); - /* FUNCTION FOR MAKING COMMANDS/READING STATES */ + /* FUNCTION FOR CONTROLLERS */ + void controllers_init(); + + void controllers_start_up(); + + void controllers_make_commands(); - void prepare_commands(); + void controllers_get_states(); - void get_states(); + void create_controller_joint_map(); /* FUNCTIONS FOR CREATING TRANSMISSION OBJECTS:*/ @@ -159,18 +167,18 @@ namespace pi3hat_hardware_interface std::vector> transmissions_; /* Function for creating all transmissions */ - hardware_interface::CallbackReturn create_transmission_interface(const hardware_interface::HardwareInfo &info); + void create_transmission_interface(const hardware_interface::HardwareInfo &info); /* Functions for creating simple transmission */ - hardware_interface::CallbackReturn create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, + void create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating four bar linkage transmission */ - hardware_interface::CallbackReturn create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, + void create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names); /* Functions for creating differential transmission */ - hardware_interface::CallbackReturn create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, + void create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names); /* Functions for checking, if data passed from urdf is correct */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index b10c3cf0..7de7720e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -24,8 +24,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa joint_states_.resize(info_.joints.size()); joint_transmission_passthrough_.resize(info_.joints.size()); - controller_joint_map_.resize(info_.joints.size()); - + joint_controller_number_ = info_.joints.size(); + /* Prepare controller bridges */ for (const hardware_interface::ComponentInfo &joint : info_.joints) { @@ -34,8 +34,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa try { params = get_controller_parameters(joint); - controller_joint_map_.insert(controller_joint_map_.begin() + params.id_, - controller_bridges.size()); std::string type_string = joint.parameters.at("motor_type"); type = choose_wrapper_type(type_string); @@ -50,7 +48,15 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa } /* Prepare transmissions */ - create_transmission_interface(info_); + try + { + create_transmission_interface(info_); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "%s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } /* Standard CAN config */ mjbots::pi3hat::Pi3Hat::CanConfiguration can_config; @@ -58,6 +64,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa /* Configure the Pi3Hat for 1000hz IMU sampling */ mjbots::pi3hat::Pi3Hat::Configuration config; config.attitude_rate_hz = 1000; + /* Set the mounting orientation of the IMU */ try { @@ -83,24 +90,77 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), info_.joints.size()); pi3hat_input_.tx_can = tx_can_frames_span_; - /* Set up CAN TX frames */ - for (size_t i = 0; i < info_.joints.size(); i++) - { - pi3hat_input_.tx_can[i].id = controller_bridges[i].get_can_id(); - pi3hat_input_.tx_can[i].bus = controller_bridges[i].get_can_bus(); - pi3hat_input_.tx_can[i].expect_reply = true; - } - config.can[0] = can_config; config.can[1] = can_config; config.can[2] = can_config; config.can[3] = can_config; config.can[4] = can_config; - // Initialize the Pi3Hat + /* Initialize the Pi3Hat and realtime options */ pi3hat_ = std::make_shared(config); + mjbots::pi3hat::ConfigureRealtime(0); + + /* Initialize all motors/remove all flags */ + controllers_init(); + pi3hat_->Cycle(pi3hat_input_); + + /* Create rx_frame.id -> joint map */ + try + { + create_controller_joint_map(); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "%s", e.what()); + return hardware_interface::CallbackReturn::ERROR; + } + /* Get states with prepared controller -> joint map */ + controllers_get_states(); + + return hardware_interface::CallbackReturn::SUCCESS; } +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const rclcpp_lifecycle::State &previous_state) +{ + /* Set all commands, states and transmission passthrough to 0 */ + for(int i = 0; i < controller_bridges.size(); ++i) + { + controller_commands_[i].position_ = 0; + controller_commands_[i].velocity_ = 0; + controller_commands_[i].torque_ = 0; + + controller_states_[i].position_ = 0; + controller_states_[i].velocity_ = 0; + controller_states_[i].torque_ = 0; + controller_states_[i].fault = 0; + controller_states_[i].temperature_ = 0; + + controller_transmission_passthrough_[i].position_ = 0; + controller_transmission_passthrough_[i].velocity_ = 0; + controller_transmission_passthrough_[i].torque_ = 0; + + + joint_commands_[i].position_; + joint_commands_[i].velocity_ = 0; + joint_commands_[i].torque_ = 0; + + joint_states_[i].position_ = 0; + joint_states_[i].velocity_ = 0; + joint_states_[i].torque_ = 0; + joint_states_[i].fault = 0; + joint_states_[i].temperature_ = 0; + + joint_transmission_passthrough_[i].position_ = 0; + joint_transmission_passthrough_[i].velocity_ = 0; + joint_transmission_passthrough_[i].torque_ = 0; + } + + /* Start motors */ + controllers_start_up(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000); + controllers_get_states(); +} @@ -152,7 +212,7 @@ void Pi3HatHardwareInterface::load_transmission_data(const hardware_interface::T } } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) +void Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) { /* Prepare loaders */ transmission_interface::SimpleTransmissionLoader simple_loader; @@ -173,10 +233,15 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_ { if(transmission_info.type == "transmission_interface/SimpleTransmission") { - if(create_simple_transmission(transmission_info, simple_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + try + { + create_simple_transmission(transmission_info, simple_loader, joint_names); + } + catch(const transmission_interface::TransmissionInterfaceException& e) { - return hardware_interface::CallbackReturn::ERROR; + throw; } + } } @@ -185,10 +250,15 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_ { if(transmission_info.type == "transmission_interface/FourBarLinkageTransmission") { - if(create_fbl_transmission(transmission_info, fbl_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + try + { + create_fbl_transmission(transmission_info, fbl_loader, joint_names); + } + catch(const transmission_interface::TransmissionInterfaceException& e) { - return hardware_interface::CallbackReturn::ERROR;; + throw; } + } } @@ -197,16 +267,19 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_transmission_ { if(transmission_info.type == "transmission_interface/DifferentialTransmission") { - if(create_diff_transmission(transmission_info, diff_loader, joint_names) == hardware_interface::CallbackReturn::ERROR) + try { - return hardware_interface::CallbackReturn::ERROR; + create_diff_transmission(transmission_info, diff_loader, joint_names); + } + catch(const transmission_interface::TransmissionInterfaceException& e) + { + throw; } } } - return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, +void Pi3HatHardwareInterface::create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names) { RCLCPP_INFO(*logger_, "Simple transmission initialization starting!"); @@ -215,7 +288,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm if (transmission_info.type != "transmission_interface/SimpleTransmission") { RCLCPP_FATAL(*logger_, "This is not SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("This is not SimpleTransmission!"); // this should not happen! } load_transmission_data(transmission_info, transmission, loader); @@ -223,13 +296,13 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm if(transmission_info.joints.size() != 1) { RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in SimpleTransmission!"); // this should not happen! } if(transmission_info.actuators.size() != 1) { RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in SimpleTransmission!"); // this should not happen! } /* Find joint index for transmission handels by name*/ @@ -252,17 +325,15 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_simple_transm catch (const transmission_interface::TransmissionInterfaceException & exc) { RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - return hardware_interface::CallbackReturn::ERROR; + throw; } transmissions_.push_back(transmission); RCLCPP_INFO(*logger_, "Simple transmissions initialized!"); - - return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, +void Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names) { RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialization starting!"); @@ -271,7 +342,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") { RCLCPP_FATAL(*logger_, "This is not FourBarLinkageTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("This is not FourBarLinkageTransmission!"); // this should not happen! } load_transmission_data(transmission_info, transmission, loader); @@ -281,12 +352,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss if(transmission_info.joints.size() != 2) { RCLCPP_FATAL(*logger_, "Invalid number of joints in FourBarLinkageTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in FourBarLinkageTransmission!"); // this should not happen! } if(transmission_info.actuators.size() != 2) { RCLCPP_FATAL(*logger_, "Invalid number of actuators in FourBarLinkageTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in FourBarLinkageTransmission!"); // this should not happen! } /* Find joints indexes for transmission handels by name*/ @@ -315,17 +386,15 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_fbl_transmiss catch (const transmission_interface::TransmissionInterfaceException & exc) { RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - return hardware_interface::CallbackReturn::ERROR; + throw; } transmissions_.push_back(transmission); RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialized!"); - - return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, +void Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names) { RCLCPP_INFO(*logger_, "Differential transmissions initialization starting!"); @@ -334,7 +403,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmis if (transmission_info.type != "transmission_interface/DifferentialTransmission") { RCLCPP_FATAL(*logger_, "This is not DifferentialTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("This is not DifferentialTransmission!"); // this should not happen! } load_transmission_data(transmission_info, transmission, loader); @@ -344,12 +413,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmis if(transmission_info.joints.size() != 2) { RCLCPP_FATAL(*logger_, "Invalid number of joints in DifferentialTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in DifferentialTransmission!");; // this should not happen! } if(transmission_info.actuators.size() != 2) { RCLCPP_FATAL(*logger_, "Invalid number of actuators in DifferentialTransmission!"); - return hardware_interface::CallbackReturn::ERROR; // this should not happen! + throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in DifferentialTransmission!"); // this should not happen! } /* Find joints indexes for transmission handels by name*/ @@ -378,14 +447,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::create_diff_transmis catch (const transmission_interface::TransmissionInterfaceException & exc) { RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - return hardware_interface::CallbackReturn::ERROR; + throw; } transmissions_.push_back(transmission); RCLCPP_INFO(*logger_, "Differential transmissions initialized!"); - - return hardware_interface::CallbackReturn::SUCCESS; } void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& params, const WrapperType type) @@ -394,31 +461,37 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& switch(type) { case Moteus: - /* moteus options */ - using mjbots::moteus::Controller; - using controller_interface::MoteusWrapper; - Controller::Options moteus_options; - moteus_options.bus = params.bus_; - moteus_options.id = params.id_; - - /* moteus command format (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - format.velocity_limit= mjbots::moteus::kFloat; - moteus_options.position_format = format; - - /* moteus command (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Command moteus_command; - - wrapper_ptr = std::make_unique(moteus_options, moteus_command); - - break; + wrapper_ptr = create_moteus_wrapper(params); + break; } controller_bridges.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); } +std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) +{ + /* moteus options */ + using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + /* moteus command format (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + + /* moteus command (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Command moteus_command; + moteus_command.maximum_torque = params.torque_max_; + moteus_command.velocity_limit = params.velocity_max_; + + return std::make_unique(moteus_options, moteus_command); +} + Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) { if(type == "moteus") @@ -447,4 +520,62 @@ ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const ha } return params; +} + +void Pi3HatHardwareInterface::controllers_init() +{ + int size = controller_bridges.size(); + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_bridges[i].initialize(tx_can_frames_[i]); + } +} + +void Pi3HatHardwareInterface::controllers_start_up() +{ + int size = controller_bridges.size(); + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_bridges[i].start_up(tx_can_frames_[i],controller_commands_[i]); + } +} + +void Pi3HatHardwareInterface::controllers_make_commands() +{ + int size = controller_bridges.size(); + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_bridges[i].make_command(tx_can_frames_[i], controller_commands_[i]); + } +} + +void Pi3HatHardwareInterface::controllers_get_states() +{ + int size = controller_bridges.size(); + for(int i = 0; i < joint_controller_number_; ++i) + { + int joint_id = controller_joint_map_.at(rx_can_frames_[i].id); + controller_bridges[joint_id].get_state(rx_can_frames_[i], controller_states_[joint_id]); + } +} + +void Pi3HatHardwareInterface::create_controller_joint_map() +{ + for(int i = 0; i < joint_controller_number_; ++i) + { + int joint_id = i; + int controller_id = tx_can_frames_[i].id; + for(int j = 0; j < joint_controller_number_; ++j) + { + if(controller_id == rx_can_frames_[j].id) + { + std::pair controller_joint_pair(controller_id, joint_id); + controller_joint_map_.emplace(controller_joint_pair); + } + } + } + if(controller_joint_map_.size() != joint_controller_number_) + { + throw std::logic_error("Controller -> Joint map has diffrent length!"); + } } \ No newline at end of file From 6132fe40a548694b7e6be109bee128e8597965c6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 28 Jul 2024 12:17:05 +0200 Subject: [PATCH 075/137] Updated tests --- .../tests/hardware/double_bridge_test.cpp | 62 ++++++++++++------- .../tests/hardware/single_bridge_test.cpp | 42 ++++++++----- 2 files changed, 63 insertions(+), 41 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp index c7ba729d..b460e5d3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp @@ -22,27 +22,6 @@ static double GetNow() int main(int argc, char** argv) { - // moteusues options - using mjbots::moteus::Controller; - Controller::Options moteus_1_options; - moteus_1_options.bus = 1; - moteus_1_options.id = 1; - - Controller::Options moteus_2_options; - moteus_2_options.bus = 2; - moteus_2_options.id = 2; - - // moteus command format (it will be copied to wrapper) - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - moteus_1_options.position_format = format; - moteus_2_options.position_format = format; - - //moteus command (it will be copied to wrapper) - mjbots::moteus::PositionMode::Command moteus_command; - - // pi3hat mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; pi3hat_configuration.attitude_rate_hz = 1000; @@ -88,16 +67,42 @@ int main(int argc, char** argv) params_2.bus_ = 2; params_2.id_ = 2; + // moteusues options + using mjbots::moteus::Controller; + Controller::Options moteus_1_options; + moteus_1_options.bus = 1; + moteus_1_options.id = 1; + + Controller::Options moteus_2_options; + moteus_2_options.bus = 2; + moteus_2_options.id = 2; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_1_options.position_format = format; + moteus_2_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_1_command; + moteus_1_command.maximum_torque = params_1.torque_max_; + moteus_1_command.velocity_limit = params_1.velocity_max_; + + mjbots::moteus::PositionMode::Command moteus_2_command; + moteus_2_command.maximum_torque = params_2.torque_max_; + moteus_2_command.velocity_limit = params_2.velocity_max_; + std::vector controllers; std::vector controller_commands; std::vector controller_states; - controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_command); + controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_1_command); std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); - controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_command); + controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_1_command); std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); @@ -120,9 +125,18 @@ int main(int argc, char** argv) // set stop to moteus for(int i = 0; i < controllers.size(); ++i) { - controllers[i].make_stop(tx_frame[i]); + controllers[i].initialize(tx_frame[i]); + } + pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000); + + for(int i = 0; i < controllers.size(); ++i) + { + controller_commands[i].position_ = 0; + controllers[i].start_up(tx_frame[i], controller_commands[i]); } pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000000); std::cout << "Controllers successfully started!" << std::endl; auto prev = GetNow(); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp index fb8f0152..58d0bc8e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp @@ -19,22 +19,6 @@ static double GetNow() int main(int argc, char** argv) { - // moteus options - using mjbots::moteus::Controller; - Controller::Options moteus_options; - moteus_options.bus = 1; - moteus_options.id = 1; - - // moteus command format (it will be copied to wrapper) - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - moteus_options.position_format = format; - - //moteus command (it will be copied to wrapper) - mjbots::moteus::PositionMode::Command moteus_command; - - // pi3hat mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; pi3hat_configuration.attitude_rate_hz = 1000; @@ -70,6 +54,23 @@ int main(int argc, char** argv) params.bus_ = 1; params.id_ = 1; + // moteus options + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = 1; + moteus_options.id = 1; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_command; + moteus_command.maximum_torque = params.torque_max_; + moteus_command.velocity_limit = params.velocity_max_; + controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); @@ -88,8 +89,15 @@ int main(int argc, char** argv) std::cout << "Realtime control activated!" << std::endl; // set stop to moteus - controller.make_stop(tx_frame); + controller.initialize(tx_frame); + pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000); + + controller_command.position_ = 0; + controller.start_up(tx_frame, controller_command); pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000000); + std::cout << "Controller successfully started!" << std::endl; auto prev = GetNow(); From ae5724d1ac7e049a67d3e680bad6759c267aca0e Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 28 Jul 2024 13:42:54 +0200 Subject: [PATCH 076/137] Finished implementation of pi3hat interface, time for tests --- .../include/controllers/ControllerBridge.hpp | 2 +- .../pi3hat_hardware_interface.hpp | 13 +- .../src/pi3hat_hardware_interface.cpp | 250 ++++++++++++++++-- 3 files changed, 248 insertions(+), 17 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index c259eebf..5f68f918 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -23,7 +23,7 @@ struct ControllerState double position_; /* [radians] */ double velocity_; /* [radians/s] */ double torque_; /* [Nm] */ - int temperature_; /* [Celcius] */ + double temperature_; /* [Celcius] */ bool fault = false; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index bf064448..50866d37 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -55,6 +55,10 @@ namespace pi3hat_hardware_interface hardware_interface::CallbackReturn on_configure( const rclcpp_lifecycle::State &previous_state) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC + hardware_interface::CallbackReturn on_cleanup( + const rclcpp_lifecycle::State &previous_state) override; + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC std::vector export_state_interfaces() override; @@ -118,11 +122,14 @@ namespace pi3hat_hardware_interface std::vector controller_states_; std::vector controller_commands_; + /* Controller start positions after initialization */ + std::vector controller_start_positions_; + /* For transmission interface */ std::vector controller_transmission_passthrough_; /* Controller Bridges */ - std::vector controller_bridges; + std::vector controller_bridges_; using JointState = controller_interface::ControllerState; @@ -166,6 +173,10 @@ namespace pi3hat_hardware_interface /* Transmission interfaces*/ std::vector> transmissions_; + void joint_to_controller_transform(); + + void controller_to_joint_transform(); + /* Function for creating all transmissions */ void create_transmission_interface(const hardware_interface::HardwareInfo &info); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 7de7720e..78470669 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -36,6 +36,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa params = get_controller_parameters(joint); std::string type_string = joint.parameters.at("motor_type"); + controller_start_positions_.push_back(std::stod(joint.parameters.at("motor_start_position"))); type = choose_wrapper_type(type_string); } catch(const std::exception& e) @@ -80,6 +81,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa /* Initialize the Pi3Hat input */ pi3hat_input_ = mjbots::pi3hat::Pi3Hat::Input(); + pi3hat_input_.request_attitude = true; + pi3hat_input_.wait_for_attitude = true; pi3hat_input_.attitude = &attitude_; tx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[info_.joints.size()]); @@ -99,10 +102,18 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa /* Initialize the Pi3Hat and realtime options */ pi3hat_ = std::make_shared(config); mjbots::pi3hat::ConfigureRealtime(0); + + RCLCPP_INFO(*logger_, "Hardware Interface successfully initialized!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const rclcpp_lifecycle::State &previous_state) +{ /* Initialize all motors/remove all flags */ controllers_init(); pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000); /* Create rx_frame.id -> joint map */ try @@ -114,18 +125,66 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa RCLCPP_FATAL(*logger_, "%s", e.what()); return hardware_interface::CallbackReturn::ERROR; } + /* Get states with prepared controller -> joint map */ controllers_get_states(); + + RCLCPP_INFO(*logger_, "Hardware Interface successfully configured!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rclcpp_lifecycle::State &previous_state) +{ + /* Set all commands, states and transmission passthrough to start state */ + for(int i = 0; i < controller_bridges_.size(); ++i) + { + controller_commands_[i].position_ = controller_start_positions_[i]; + controller_commands_[i].velocity_ = 0; + controller_commands_[i].torque_ = 0; + + controller_states_[i].position_ = 0; + controller_states_[i].velocity_ = 0; + controller_states_[i].torque_ = 0; + controller_states_[i].fault = 0; + controller_states_[i].temperature_ = 0; + + controller_transmission_passthrough_[i].position_ = 0; + controller_transmission_passthrough_[i].velocity_ = 0; + controller_transmission_passthrough_[i].torque_ = 0; + + + joint_commands_[i].position_ = 0; + joint_commands_[i].velocity_ = 0; + joint_commands_[i].torque_ = 0; + + joint_states_[i].position_ = 0; + joint_states_[i].velocity_ = 0; + joint_states_[i].torque_ = 0; + joint_states_[i].fault = 0; + joint_states_[i].temperature_ = 0; + + joint_transmission_passthrough_[i].position_ = 0; + joint_transmission_passthrough_[i].velocity_ = 0; + joint_transmission_passthrough_[i].torque_ = 0; + } + + /* Send start command to motors */ + controllers_start_up(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); + controllers_get_states(); + + RCLCPP_INFO(*logger_, "Hardware Interface successfully activated!"); return hardware_interface::CallbackReturn::SUCCESS; } -hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const rclcpp_lifecycle::State &previous_state) +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &previous_state) { - /* Set all commands, states and transmission passthrough to 0 */ - for(int i = 0; i < controller_bridges.size(); ++i) + /* Set all commands, states and transmission passthrough to end state */ + for(int i = 0; i < controller_bridges_.size(); ++i) { - controller_commands_[i].position_ = 0; + controller_commands_[i].position_ = controller_start_positions_[i]; controller_commands_[i].velocity_ = 0; controller_commands_[i].torque_ = 0; @@ -140,7 +199,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r controller_transmission_passthrough_[i].torque_ = 0; - joint_commands_[i].position_; + joint_commands_[i].position_ = 0; joint_commands_[i].velocity_ = 0; joint_commands_[i].torque_ = 0; @@ -155,16 +214,177 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r joint_transmission_passthrough_[i].torque_ = 0; } - /* Start motors */ + + /* Send end commands to motors */ controllers_start_up(); pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); + controllers_get_states(); + + RCLCPP_INFO(*logger_, "Hardware Interface successfully deactivated!"); + return hardware_interface::CallbackReturn::SUCCESS; +} + +hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_cleanup(const rclcpp_lifecycle::State &previous_state) +{ + + /* Deinitialize all motors/remove all flags */ + controllers_init(); + pi3hat_->Cycle(pi3hat_input_); ::usleep(1000); + + /* Get states with prepared controller -> joint map */ controllers_get_states(); + + RCLCPP_INFO(*logger_, "Hardware Interface successfully cleanuped!"); + return hardware_interface::CallbackReturn::SUCCESS; } +std::vector Pi3HatHardwareInterface::export_command_interfaces() +{ + std::vector command_interfaces; + /* Joint commands (before joint -> controller transformation)*/ + for (auto i = 0u; i < info_.joints.size(); i++) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_commands_[i].velocity_))); + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_commands_[i].torque_))); + } + return command_interfaces; +} +std::vector Pi3HatHardwareInterface::export_state_interfaces() +{ + std::vector state_interfaces; + + /* Joint states (after controller -> joint transformation)*/ + for (auto i = 0u; i < info_.joints.size(); i++) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_states_[i].velocity_))); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_states_[i].torque_))); + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "temp", &(joint_states_[i].temperature_))); + } + + /* IMU states (after IMUTransform transformation) */ + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.x", &attitude_.attitude.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.y", &attitude_.attitude.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.z", &attitude_.attitude.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.w", &attitude_.attitude.w)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.x", &attitude_.rate_dps.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.y", &attitude_.rate_dps.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.z", &attitude_.rate_dps.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.x", &attitude_.accel_mps2.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.y", &attitude_.accel_mps2.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.z", &attitude_.accel_mps2.z)); + + return state_interfaces; +} + + +hardware_interface::return_type Pi3HatHardwareInterface::write(const rclcpp::Time &time, const rclcpp::Duration &period) +{ + for (int i = 0; i < joint_controller_number_; ++i) + { + if (std::isnan(joint_commands_[i].position_) || std::isnan(joint_commands_[i].velocity_) || std::isnan(joint_commands_[i].torque_)) + { + RCLCPP_WARN(rclcpp::get_logger("Pi3HatHardwareInterface"), "NaN command for actuator"); + break; + } + } + + joint_to_controller_transform(); + + controllers_make_commands(); + + mjbots::pi3hat::Pi3Hat::Output result = pi3hat_->Cycle(pi3hat_input_); + + if (result.error) + { + RCLCPP_ERROR(rclcpp::get_logger("Pi3HatHardwareInterface"), "Pi3Hat::Cycle() failed!"); + return hardware_interface::return_type::ERROR; + } + + if (result.attitude_present) + { + imu_transform_.transform_attitude(attitude_); + } + + if(result.rx_can_size > 0) + { + controllers_get_states(); + } + + controller_to_joint_transform(); + + return hardware_interface::return_type::OK; +} + +hardware_interface::return_type Pi3HatHardwareInterface::read(const rclcpp::Time &time, const rclcpp::Duration &period) +{ + return hardware_interface::return_type::OK; +} + +void Pi3HatHardwareInterface::joint_to_controller_transform() +{ + for(int i = 0; i < joint_controller_number_; ++i) + { + joint_transmission_passthrough_[i].position_ = joint_commands_[i].position_; + joint_transmission_passthrough_[i].velocity_ = joint_commands_[i].velocity_; + joint_transmission_passthrough_[i].torque_ = joint_commands_[i].torque_; + } + + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->joint_to_actuator(); }); + + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_commands_[i].position_ = controller_transmission_passthrough_[i].position_; + controller_commands_[i].velocity_ = controller_transmission_passthrough_[i].velocity_; + controller_commands_[i].torque_ = controller_transmission_passthrough_[i].torque_; + } +} + +void Pi3HatHardwareInterface::controller_to_joint_transform() +{ + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_transmission_passthrough_[i].position_ = controller_states_[i].position_; + controller_transmission_passthrough_[i].velocity_ = controller_states_[i].velocity_; + controller_transmission_passthrough_[i].torque_ = controller_states_[i].torque_; + } + + std::for_each( + transmissions_.begin(), transmissions_.end(), + [](auto & transmission) { transmission->jactuator_to_joint(); }); + + for(int i = 0; i < joint_controller_number_; ++i) + { + joint_states_[i].position_ = joint_transmission_passthrough_[i].position_; + joint_states_[i].velocity_ = joint_transmission_passthrough_[i].velocity_; + joint_states_[i].torque_ = joint_transmission_passthrough_[i].torque_; + } +} /* TRANSMISSION FUNCTIONS */ void Pi3HatHardwareInterface::append_joint_handles(std::vector& joint_handles, const std::string joint_name, const int joint_index) @@ -465,7 +685,7 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& break; } - controller_bridges.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); + controller_bridges_.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); } std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) @@ -524,38 +744,38 @@ ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const ha void Pi3HatHardwareInterface::controllers_init() { - int size = controller_bridges.size(); + int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { - controller_bridges[i].initialize(tx_can_frames_[i]); + controller_bridges_[i].initialize(tx_can_frames_[i]); } } void Pi3HatHardwareInterface::controllers_start_up() { - int size = controller_bridges.size(); + int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { - controller_bridges[i].start_up(tx_can_frames_[i],controller_commands_[i]); + controller_bridges_[i].start_up(tx_can_frames_[i],controller_commands_[i]); } } void Pi3HatHardwareInterface::controllers_make_commands() { - int size = controller_bridges.size(); + int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { - controller_bridges[i].make_command(tx_can_frames_[i], controller_commands_[i]); + controller_bridges_[i].make_command(tx_can_frames_[i], controller_commands_[i]); } } void Pi3HatHardwareInterface::controllers_get_states() { - int size = controller_bridges.size(); + int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { int joint_id = controller_joint_map_.at(rx_can_frames_[i].id); - controller_bridges[joint_id].get_state(rx_can_frames_[i], controller_states_[joint_id]); + controller_bridges_[joint_id].get_state(rx_can_frames_[i], controller_states_[joint_id]); } } From 2793afa7dcc30b4129b2c000c84ec951343d6922 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 16:56:06 +0200 Subject: [PATCH 077/137] Fixing compilation errors --- .../include/controllers/ControllerBridge.hpp | 31 +-------------- .../controllers/ControllerStructures.hpp | 39 +++++++++++++++++++ .../wrappers/ControllerWrapper.hpp | 4 +- .../controllers/wrappers/MoteusWrapper.hpp | 8 ++-- .../src/pi3hat_hardware_interface.cpp | 15 ++++++- 5 files changed, 59 insertions(+), 38 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 5f68f918..ef15b1e1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -3,42 +3,13 @@ #include "../3rd_libs/pi3hat/pi3hat.h" #include "wrappers/ControllerWrapper.hpp" +#include "ControllerStructures.hpp" #include #include namespace controller_interface { -/* Structure for basic actuator command */ -struct ControllerCommand -{ - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ -}; - -/* Structure for basic actuator state */ -struct ControllerState -{ - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ - double temperature_; /* [Celcius] */ - bool fault = false; -}; - -struct ControllerParameters -{ - double position_max_; - double position_min_; - double position_offset_; - double velocity_max_; - double torque_max_; - int direction_ = 1; - int id_; /* Usage in your bridge (check moteus bridge)*/ - int bus_; /* Usage in your bridge (check moteus bridge)*/ -}; - class ControllerBridge { private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp new file mode 100644 index 00000000..78ff7969 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp @@ -0,0 +1,39 @@ +#ifndef _CONTROLLER_STRUCTURES_HPP_ +#define _CONTROLLER_STRUCTURES_HPP_ + +namespace controller_interface +{ + +/* Structure for basic actuator command */ +struct ControllerCommand +{ + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ +}; + +/* Structure for basic actuator state */ +struct ControllerState +{ + double position_; /* [radians] */ + double velocity_; /* [radians/s] */ + double torque_; /* [Nm] */ + double temperature_; /* [Celcius] */ + bool fault = false; +}; + +struct ControllerParameters +{ + double position_max_; + double position_min_; + double position_offset_; + double velocity_max_; + double torque_max_; + int direction_ = 1; + int id_; /* Usage in your bridge (check moteus bridge)*/ + int bus_; /* Usage in your bridge (check moteus bridge)*/ +}; + +} + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 691631c9..d9ae6f8d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -1,7 +1,7 @@ #ifndef _CONTROLLER_WRAPPER_HPP_ #define _CONTROLLER_WRAPPER_HPP_ -#include "../ControllerBridge.hpp" +#include "../ControllerStructures.hpp" namespace controller_interface @@ -20,7 +20,7 @@ class ControllerWrapper virtual void init_to_tx_frame(CanFrame& tx_frame) = 0; virtual void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; - virtual ~ControllerWrapper() = 0; + virtual ~ControllerWrapper() = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 8107aaf7..0b735b37 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -12,9 +12,9 @@ class MoteusWrapper: public ControllerWrapper private: /* Const coefficients for easy radians - rotations transform */ - static const double rotation_to_radians_ = 2 * M_PI; - static const double radians_to_rotation_ = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ - static const double startup_coefficient_ = 0.05; /* For slow start-up */ + constexpr static double rotation_to_radians_ = 2 * M_PI; + constexpr static double radians_to_rotation_ = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ + constexpr static double startup_coefficient_ = 0.05; /* For slow start-up */ /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; @@ -31,7 +31,7 @@ class MoteusWrapper: public ControllerWrapper void init_to_tx_frame(CanFrame& tx_frame) override; void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; - ~MoteusWrapper() override = default; + // ~MoteusWrapper() override = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 78470669..7a0d4f2c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -44,8 +44,16 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa RCLCPP_FATAL(*logger_, "Error reading motor/controller parameters!"); return hardware_interface::CallbackReturn::ERROR; } - - add_controller_bridge(params, type); + + try + { + add_controller_bridge(params, type); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "Error creating motor controller!"); + return hardware_interface::CallbackReturn::ERROR; + } } /* Prepare transmissions */ @@ -683,6 +691,9 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& case Moteus: wrapper_ptr = create_moteus_wrapper(params); break; + default: + throw std::invalid_argument("Motor type doesn't exist!"); + break; } controller_bridges_.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); From 9c61566c8c3ecb0ccff7e789358a1c7a0d5701e1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 16:58:49 +0200 Subject: [PATCH 078/137] Fixing compilation errors --- .../include/controllers/wrappers/ControllerWrapper.hpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index d9ae6f8d..d4e6f25e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -2,7 +2,7 @@ #define _CONTROLLER_WRAPPER_HPP_ #include "../ControllerStructures.hpp" - +#include "../../3rd_libs/pi3hat/pi3hat.h" namespace controller_interface { From 3b335b7001061e01aa62aef1746c4add490218c1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 17:03:22 +0200 Subject: [PATCH 079/137] Fixing compilation errors --- .../tests/hardware/single_bridge_test.cpp | 13 +++++++++---- 1 file changed, 9 insertions(+), 4 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp index 58d0bc8e..1407fd28 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp @@ -91,12 +91,17 @@ int main(int argc, char** argv) // set stop to moteus controller.initialize(tx_frame); pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000); + ::usleep(10000); controller_command.position_ = 0; - controller.start_up(tx_frame, controller_command); - pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000000); + controller_state.position_ = NaN; + while(std::abs(controller_state.position_) > 0.1) + { + controller.start_up(tx_frame, controller_command); + pi3hat_output = pi3hat.Cycle(input); + ::usleep(2000); + controller.get_state(rx_frame, controller_state); + } std::cout << "Controller successfully started!" << std::endl; From c5baa3e2c976c98f17dec48151d165e66e1c6078 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 17:30:24 +0200 Subject: [PATCH 080/137] Fixing compilation errors --- .../include/controllers/ControllerBridge.hpp | 10 ++++++++-- .../include/controllers/Controllers.hpp | 3 ++- .../src/controllers/ControllerBridge.cpp | 12 ++++++++++++ .../tests/hardware/double_bridge_test.cpp | 3 --- 4 files changed, 22 insertions(+), 6 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index ef15b1e1..ae209d89 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -16,7 +16,7 @@ class ControllerBridge using CanFrame = mjbots::pi3hat::CanFrame; - const std::unique_ptr wrapper_; + std::unique_ptr wrapper_; ControllerParameters params_; @@ -24,7 +24,13 @@ class ControllerBridge ControllerBridge(std::unique_ptr wrapper, const ControllerParameters& params); - void make_command(CanFrame& tx_frame, ControllerCommand& command) const; //POMYSL JESZCZE O TYM + ControllerBridge(const ControllerBridge& other_controller) = delete; + ControllerBridge& operator=(const ControllerBridge& other_controller) = delete; + ControllerBridge(ControllerBridge&& other_controller); + ControllerBridge& operator=(ControllerBridge&& other_controller); + + + void make_command(CanFrame& tx_frame, ControllerCommand& command) const; void get_state(const CanFrame& rx_frame, ControllerState& state) const; void initialize(CanFrame& tx_frame) const; void start_up(CanFrame& tx_frame, ControllerCommand& command) const; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp index 98892c04..ff90ceec 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp @@ -1,8 +1,9 @@ #ifndef _CONTROLLERS_HPP_ #define _CONTROLLERS_HPP_ +#include "wrappers/MoteusWrapper.hpp" #include "ControllerBridge.hpp" #include "wrappers/ControllerWrapper.hpp" -#include "wrappers/MoteusWrapper.hpp" +#include "ControllerStructures.hpp" #endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 57b7fb5a..f5c615f4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -9,6 +9,18 @@ ControllerBridge::ControllerBridge( const ControllerParameters& params): wrapper_(std::move(wrapper)), params_(params){} +ControllerBridge::ControllerBridge(ControllerBridge&& other_controller): + wrapper_(std::move(other_controller.wrapper_)), params_(other_controller.params_) {} + +ControllerBridge& ControllerBridge::operator=(ControllerBridge&& other_controller) +{ + if (this != &other_controller) + { + this->wrapper_ = std::move(other_controller.wrapper_); + this->params_ = other_controller.params_; + } + return *this; +} void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& command) const { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp index b460e5d3..b4445c72 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp @@ -1,7 +1,4 @@ #include "../../include/controllers/Controllers.hpp" - - - #include "../../include/3rd_libs/pi3hat/pi3hat.h" #include "../../include/3rd_libs/pi3hat/realtime.h" #include From 34f7117ea611f6101df0efb3218afd1c0e2f56e9 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 17:34:58 +0200 Subject: [PATCH 081/137] Fixed controller wrappers, fixing tests --- .../tests/hardware/double_bridge_test.cpp | 27 ++++++++++++------- 1 file changed, 17 insertions(+), 10 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp index b4445c72..38122462 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp @@ -48,8 +48,8 @@ int main(int argc, char** argv) // moteus wrapper controller_interface::ControllerParameters params_1; params_1.direction_ = 1; - params_1.position_max_ = 30; - params_1.position_min_ = -30; + params_1.position_max_ = 30 * M_PI; + params_1.position_min_ = -30 * M_PI; params_1.velocity_max_ = 10; params_1.torque_max_ = 1; params_1.bus_ = 1; @@ -57,8 +57,8 @@ int main(int argc, char** argv) controller_interface::ControllerParameters params_2; params_2.direction_ = 1; - params_2.position_max_ = 10; - params_2.position_min_ = -10; + params_2.position_max_ = 10 * M_PI; + params_2.position_min_ = -10 * M_PI; params_2.velocity_max_ = 10; params_2.torque_max_ = 1; params_2.bus_ = 2; @@ -125,15 +125,22 @@ int main(int argc, char** argv) controllers[i].initialize(tx_frame[i]); } pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000); + ::usleep(10000); for(int i = 0; i < controllers.size(); ++i) { controller_commands[i].position_ = 0; controllers[i].start_up(tx_frame[i], controller_commands[i]); } - pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000000); + while(std::abs(controller_states[0].position_) > 0.1 && std::abs(controller_states[1].position_) > 0.1) + { + pi3hat_output = pi3hat.Cycle(input); + ::usleep(2000); + for(int i = 0; i < controllers.size(); ++i) + { + controllers[i].get_state(rx_frame[i], controller_states[i]); + } + } std::cout << "Controllers successfully started!" << std::endl; auto prev = GetNow(); @@ -141,14 +148,14 @@ int main(int argc, char** argv) while(true) { auto now = GetNow(); - controller_commands[0].position_ = 20 * cos(now - prev); - controller_commands[1].position_ = 10 * sin(now - prev); + controller_commands[0].position_ = 20 * M_PI * cos(now - prev); + controller_commands[1].position_ = 10 * M_PI *sin(now - prev); for(int i = 0; i < controllers.size(); ++i) { controllers[i].make_command(tx_frame[i], controller_commands[i]); } pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000); + ::usleep(500); auto mesaure_time = GetNow() - now; frequency = (int) 1/mesaure_time; for(int i = 0; i < controllers.size(); ++i) From c7dbae0cf98293fe9a5d401cdb31e9f1273048b7 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 17:53:25 +0200 Subject: [PATCH 082/137] Fixing includes --- .../src/controllers/ControllerBridge.cpp | 2 +- .../src/pi3hat_hardware_interface.cpp | 42 +++++++++---------- 2 files changed, 22 insertions(+), 22 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index f5c615f4..c4193f05 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -1,4 +1,4 @@ -#include "../../include/controllers/ControllerBridge.hpp" +#include "controllers/ControllerBridge.hpp" using namespace controller_interface; using mjbots::pi3hat::CanFrame; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 7a0d4f2c..39a4ed50 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,4 +1,4 @@ -#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" +#include "pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; using namespace controller_interface; @@ -15,16 +15,17 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa logger_ = std::make_unique( rclcpp::get_logger("Pi3HatHardwareInterface")); - controller_commands_.resize(info_.joints.size()); - controller_states_.resize(info_.joints.size()); - controller_transmission_passthrough_.resize(info_.joints.size()); + joint_controller_number_ = info_.joints.size(); + controller_commands_.resize(joint_controller_number_); + controller_states_.resize(joint_controller_number_); + controller_transmission_passthrough_.resize(joint_controller_number_); - joint_commands_.resize(info_.joints.size()); - joint_states_.resize(info_.joints.size()); - joint_transmission_passthrough_.resize(info_.joints.size()); - joint_controller_number_ = info_.joints.size(); + joint_commands_.resize(joint_controller_number_); + joint_states_.resize(joint_controller_number_); + joint_transmission_passthrough_.resize(joint_controller_number_); + /* Prepare controller bridges */ for (const hardware_interface::ComponentInfo &joint : info_.joints) @@ -93,12 +94,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa pi3hat_input_.wait_for_attitude = true; pi3hat_input_.attitude = &attitude_; - tx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[info_.joints.size()]); - rx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[info_.joints.size()]); + tx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[joint_controller_number_]); + rx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[joint_controller_number_]); - mjbots::pi3hat::Span rx_can_frames_span_(rx_can_frames_.get(), info_.joints.size()); + mjbots::pi3hat::Span rx_can_frames_span_(rx_can_frames_.get(), joint_controller_number_); pi3hat_input_.rx_can = rx_can_frames_span_; - mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), info_.joints.size()); + mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), joint_controller_number_); pi3hat_input_.tx_can = tx_can_frames_span_; config.can[0] = can_config; @@ -144,7 +145,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rclcpp_lifecycle::State &previous_state) { /* Set all commands, states and transmission passthrough to start state */ - for(int i = 0; i < controller_bridges_.size(); ++i) + for(int i = 0; i < joint_controller_number_; ++i) { controller_commands_[i].position_ = controller_start_positions_[i]; controller_commands_[i].velocity_ = 0; @@ -190,7 +191,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rc hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const rclcpp_lifecycle::State &previous_state) { /* Set all commands, states and transmission passthrough to end state */ - for(int i = 0; i < controller_bridges_.size(); ++i) + for(int i = 0; i < joint_controller_number_; ++i) { controller_commands_[i].position_ = controller_start_positions_[i]; controller_commands_[i].velocity_ = 0; @@ -253,7 +254,7 @@ std::vector Pi3HatHardwareInterface::expor std::vector command_interfaces; /* Joint commands (before joint -> controller transformation)*/ - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < joint_controller_number_; i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); @@ -271,7 +272,7 @@ std::vector Pi3HatHardwareInterface::export_ std::vector state_interfaces; /* Joint states (after controller -> joint transformation)*/ - for (auto i = 0u; i < info_.joints.size(); i++) + for (auto i = 0u; i < joint_controller_number_; i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); @@ -442,6 +443,8 @@ void Pi3HatHardwareInterface::load_transmission_data(const hardware_interface::T void Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) { + if(info.transmissions.size() == 0) return; + /* Prepare loaders */ transmission_interface::SimpleTransmissionLoader simple_loader; transmission_interface::FourBarLinkageTransmissionLoader fbl_loader; @@ -696,7 +699,8 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& break; } - controller_bridges_.push_back(controller_interface::ControllerBridge(std::move(wrapper_ptr), params)); + controller_interface::ControllerBridge bridge = controller_interface::ControllerBridge(std::move(wrapper_ptr), params); + controller_bridges_.push_back(std::move(bridge)); } std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) @@ -755,7 +759,6 @@ ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const ha void Pi3HatHardwareInterface::controllers_init() { - int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { controller_bridges_[i].initialize(tx_can_frames_[i]); @@ -764,7 +767,6 @@ void Pi3HatHardwareInterface::controllers_init() void Pi3HatHardwareInterface::controllers_start_up() { - int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { controller_bridges_[i].start_up(tx_can_frames_[i],controller_commands_[i]); @@ -773,7 +775,6 @@ void Pi3HatHardwareInterface::controllers_start_up() void Pi3HatHardwareInterface::controllers_make_commands() { - int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { controller_bridges_[i].make_command(tx_can_frames_[i], controller_commands_[i]); @@ -782,7 +783,6 @@ void Pi3HatHardwareInterface::controllers_make_commands() void Pi3HatHardwareInterface::controllers_get_states() { - int size = controller_bridges_.size(); for(int i = 0; i < joint_controller_number_; ++i) { int joint_id = controller_joint_map_.at(rx_can_frames_[i].id); From b2cc333158cb206c6454cb26633289d4f50a5a98 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 17:54:21 +0200 Subject: [PATCH 083/137] Fixing includes --- .../src/controllers/ControllerBridge.cpp | 2 +- .../src/pi3hat_hardware_interface.cpp | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index c4193f05..f5c615f4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -1,4 +1,4 @@ -#include "controllers/ControllerBridge.hpp" +#include "../../include/controllers/ControllerBridge.hpp" using namespace controller_interface; using mjbots::pi3hat::CanFrame; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 39a4ed50..d6da1b87 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,4 +1,4 @@ -#include "pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" +#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; using namespace controller_interface; @@ -444,7 +444,7 @@ void Pi3HatHardwareInterface::load_transmission_data(const hardware_interface::T void Pi3HatHardwareInterface::create_transmission_interface(const hardware_interface::HardwareInfo &info) { if(info.transmissions.size() == 0) return; - + /* Prepare loaders */ transmission_interface::SimpleTransmissionLoader simple_loader; transmission_interface::FourBarLinkageTransmissionLoader fbl_loader; From 0c8e76e6fcd2b37695b9e5c6f4692cb041d500f4 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:26:13 +0200 Subject: [PATCH 084/137] Added config file --- .../pi3hat_hardware_interface/CMakeLists.txt | 94 +++++++++++++++++++ .../fastrtps_profile_no_shmem.xml | 19 ++++ .../pi3hat_hardware_interface/package.xml | 6 ++ .../pi3hat_hardware_interface.xml | 9 ++ 4 files changed, 128 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/fastrtps_profile_no_shmem.xml create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index 83f20c1c..e2f8a501 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -7,6 +7,84 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) +find_package(pluginlib REQUIRED) +find_package(hardware_interface REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rclcpp_lifecycle REQUIRED) + +add_library(pi3hat_hardware_interface SHARED + src/pi3hat_hardware_interface.cpp +) + +add_library(pi3hat SHARED + include/3rd_libs/pi3hat/pi3hat.cc +) + +add_library(controllers SHARED + src/controllers/ControllerBridge.cpp + src/controllers/wrappers/ControllerWrapper.cpp + src/controllers/wrappers/MoteusWrapper.cpp +) + +add_library(imu_transform SHARED + src/imu_transform/IMUTransform.cpp +) + +# add include directory +target_include_directories(pi3hat_hardware_interface + PRIVATE + include +) + +target_link_libraries(pi3hat_hardware_interface + pi3hat + controllers + imu_transform + bcm_host +) + +ament_target_dependencies( + pi3hat_hardware_interface + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + transmission_interface +) + +pluginlib_export_plugin_description_file(hardware_interface pi3hat_hardware_interface.xml) + +# install +install( + TARGETS ${PROJECT_NAME} + DESTINATION lib +) + +install( + TARGETS pi3hat + DESTINATION lib +) + +install( + TARGETS controllers + DESTINATION lib +) + +install( + TARGETS imu_transform + DESTINATION lib +) + +install( + DIRECTORY include/ + DESTINATION include +) + +install( + DIRECTORY test/ + DESTINATION test +) + # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) @@ -23,4 +101,20 @@ if(BUILD_TESTING) ament_lint_auto_find_test_dependencies() endif() + +# exports +ament_export_include_directories( + include +) +ament_export_libraries( + ${PROJECT_NAME} +) +ament_export_dependencies( + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + transmission_interface +) + ament_package() diff --git a/src/meldog_hardware/pi3hat_hardware_interface/fastrtps_profile_no_shmem.xml b/src/meldog_hardware/pi3hat_hardware_interface/fastrtps_profile_no_shmem.xml new file mode 100644 index 00000000..9b338e68 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/fastrtps_profile_no_shmem.xml @@ -0,0 +1,19 @@ + + + + + CustomUdpTransport + UDPv4 + + + + + + + CustomUdpTransport + + + false + + + \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/package.xml b/src/meldog_hardware/pi3hat_hardware_interface/package.xml index 6c59bdec..c5402c89 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/package.xml +++ b/src/meldog_hardware/pi3hat_hardware_interface/package.xml @@ -9,6 +9,12 @@ ament_cmake + hardware_interface + pluginlib + rclcpp + rclcpp_lifecycle + transmission_interface + ament_lint_auto ament_lint_common diff --git a/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml b/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml new file mode 100644 index 00000000..5f7e9a01 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml @@ -0,0 +1,9 @@ + + + + + mjbots pi3hat interface by Bartlomiej Krajewski. Thanks to G-Levine for inspiration and open-source code + + + + \ No newline at end of file From 291d02f65adbdd3afb682f9dcfeb13849d407575 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:31:43 +0200 Subject: [PATCH 085/137] Fixed CMake --- src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt | 1 + 1 file changed, 1 insertion(+) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index e2f8a501..e9b1fdee 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -11,6 +11,7 @@ find_package(pluginlib REQUIRED) find_package(hardware_interface REQUIRED) find_package(rclcpp REQUIRED) find_package(rclcpp_lifecycle REQUIRED) +find_package(transmission_interface REQUIRED) add_library(pi3hat_hardware_interface SHARED src/pi3hat_hardware_interface.cpp From b9803bf7fc7d2c02a5491a5ef88a88328bf353a1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:34:39 +0200 Subject: [PATCH 086/137] Fixed compilation errors --- .../include/imu_transform/IMUTransform.hpp | 2 +- .../src/controllers/wrappers/MoteusWrapper.cpp | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp index 4f1c0184..6d055c73 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp @@ -11,7 +11,7 @@ namespace IMU class IMUTransform { private: - static const double degrees_to_radians = 2 * M_PI / 360.0; + constexpr static double degrees_to_radians = 2 * M_PI / 360.0; public: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 1db7222d..174a870d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -31,7 +31,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ + if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians_; From db79f58f5315b0b720de1884acffe49ed72e077b Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:36:31 +0200 Subject: [PATCH 087/137] Fixing compilation errors --- .../pi3hat_hardware_interface/pi3hat_hardware_interface.hpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 50866d37..89c3e040 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -35,8 +35,8 @@ #include "../controllers/Controllers.hpp" #include "../imu_transform/IMUTransform.hpp" -#include "../pi3hat/pi3hat.h" -#include "../pi3hat/realtime.h" +#include "../3rd_libs/pi3hat/pi3hat.h" +#include "../3rd_libs/pi3hat/realtime.h" #include "visibility_control.hpp" From a17420a954fe94a38b9b2e8fd1caf475314f1730 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:42:07 +0200 Subject: [PATCH 088/137] Fixing compilation errors --- .../pi3hat_hardware_interface/pi3hat_hardware_interface.hpp | 4 ++-- .../src/pi3hat_hardware_interface.cpp | 6 +++--- 2 files changed, 5 insertions(+), 5 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 89c3e040..6591ab4d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -149,12 +149,12 @@ namespace pi3hat_hardware_interface Remember to change this function in source code */ WrapperType choose_wrapper_type(const std::string& type); - std::unique_ptr create_moteus_wrapper(const ControllerParameters& params); + std::unique_ptr create_moteus_wrapper(const controller_interface::ControllerParameters& params); /* Function for creating moteus wrappers (here u can add your own wrapper) */ void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); - ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); + controller_interface::ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); /* FUNCTION FOR CONTROLLERS */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index d6da1b87..dab68be0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -254,7 +254,7 @@ std::vector Pi3HatHardwareInterface::expor std::vector command_interfaces; /* Joint commands (before joint -> controller transformation)*/ - for (auto i = 0u; i < joint_controller_number_; i++) + for (int i = ; i < joint_controller_number_; i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); @@ -272,7 +272,7 @@ std::vector Pi3HatHardwareInterface::export_ std::vector state_interfaces; /* Joint states (after controller -> joint transformation)*/ - for (auto i = 0u; i < joint_controller_number_; i++) + for (int i = 0; i < joint_controller_number_; i++) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); @@ -385,7 +385,7 @@ void Pi3HatHardwareInterface::controller_to_joint_transform() std::for_each( transmissions_.begin(), transmissions_.end(), - [](auto & transmission) { transmission->jactuator_to_joint(); }); + [](auto & transmission) { transmission->actuator_to_joint(); }); for(int i = 0; i < joint_controller_number_; ++i) { From b5c62685595b894c28d82140ae6803413aa72467 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:49:44 +0200 Subject: [PATCH 089/137] Fixing compilation errors --- .../pi3hat_hardware_interface.hpp | 4 ++-- .../src/pi3hat_hardware_interface.cpp | 14 +++++--------- 2 files changed, 7 insertions(+), 11 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 6591ab4d..9e4a5df1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -110,10 +110,10 @@ namespace pi3hat_hardware_interface IMU::IMUTransform imu_transform_; /* TX CAN frames */ - std::shared_ptr tx_can_frames_; + std::vector tx_can_frames_; /* RX CAN frames */ - std::shared_ptr rx_can_frames_; + std::vector rx_can_frames_; /* Container for rx_frame_id (diffrent for diffrent controller type) to joint_index maping */ std::unordered_map controller_joint_map_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index dab68be0..992f20ee 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -94,12 +94,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa pi3hat_input_.wait_for_attitude = true; pi3hat_input_.attitude = &attitude_; - tx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[joint_controller_number_]); - rx_can_frames_ = std::make_shared(new mjbots::pi3hat::CanFrame[joint_controller_number_]); + tx_can_frames_.resize(joint_controller_number_); + rx_can_frames_.resize(joint_controller_number_); - mjbots::pi3hat::Span rx_can_frames_span_(rx_can_frames_.get(), joint_controller_number_); + mjbots::pi3hat::Span rx_can_frames_span_(&rx_can_frames_[0], joint_controller_number_); pi3hat_input_.rx_can = rx_can_frames_span_; - mjbots::pi3hat::Span tx_can_frames_span_(tx_can_frames_.get(), joint_controller_number_); + mjbots::pi3hat::Span tx_can_frames_span_(&tx_can_frames_[0], joint_controller_number_); pi3hat_input_.tx_can = tx_can_frames_span_; config.can[0] = can_config; @@ -254,7 +254,7 @@ std::vector Pi3HatHardwareInterface::expor std::vector command_interfaces; /* Joint commands (before joint -> controller transformation)*/ - for (int i = ; i < joint_controller_number_; i++) + for (int i = 0; i < joint_controller_number_; i++) { command_interfaces.emplace_back(hardware_interface::CommandInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); @@ -577,8 +577,6 @@ void Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface:: } load_transmission_data(transmission_info, transmission, loader); - std::vector joint_handles; - std::vector actuator_handles; if(transmission_info.joints.size() != 2) { @@ -638,8 +636,6 @@ void Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface: } load_transmission_data(transmission_info, transmission, loader); - std::vector joint_handles; - std::vector actuator_handles; if(transmission_info.joints.size() != 2) { From 17a8030384b4094dd5720c6cdc0189d282a81d0d Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 18:51:57 +0200 Subject: [PATCH 090/137] Changed directory for tests --- .../{tests => test}/hardware/double_bridge_test.cpp | 0 .../{tests => test}/hardware/simple_test.cpp | 0 .../{tests => test}/hardware/single_bridge_test.cpp | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/{tests => test}/hardware/double_bridge_test.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/{tests => test}/hardware/simple_test.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/{tests => test}/hardware/single_bridge_test.cpp (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/double_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/simple_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/tests/hardware/single_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp From ba0bf2bb82353d9337d46f89ce7ab56cec6ac472 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 19:32:18 +0200 Subject: [PATCH 091/137] Edited pi3hat interface, compilation successful --- .../src/pi3hat_hardware_interface.cpp | 121 +++++++++++++----- 1 file changed, 87 insertions(+), 34 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 992f20ee..57697cac 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -255,13 +255,33 @@ std::vector Pi3HatHardwareInterface::expor /* Joint commands (before joint -> controller transformation)*/ for (int i = 0; i < joint_controller_number_; i++) - { - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_commands_[i].velocity_))); - command_interfaces.emplace_back(hardware_interface::CommandInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_commands_[i].torque_))); + { + if(!(info_.joints[i].command_interfaces.size() > 0)) + { + RCLCPP_WARN(*logger_, "Zero command interfaces for joint %s!", info_.joints[i].name); + } + for(const auto& command_interface: info_.joints[i].command_interfaces) + { + if(command_interface.name == hardware_interface::HW_IF_POSITION) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_commands_[i].position_))); + } + else if(command_interface.name == hardware_interface::HW_IF_VELOCITY) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_commands_[i].velocity_))); + } + else if(command_interface.name == hardware_interface::HW_IF_EFFORT) + { + command_interfaces.emplace_back(hardware_interface::CommandInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_commands_[i].torque_))); + } + else + { + RCLCPP_WARN(*logger_, "%s is wrong type of command interface, omitted!", command_interface.name); + } + } } return command_interfaces; @@ -274,37 +294,70 @@ std::vector Pi3HatHardwareInterface::export_ /* Joint states (after controller -> joint transformation)*/ for (int i = 0; i < joint_controller_number_; i++) { - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_states_[i].velocity_))); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_states_[i].torque_))); - state_interfaces.emplace_back(hardware_interface::StateInterface( - info_.joints[i].name, "temp", &(joint_states_[i].temperature_))); + if(!(info_.joints[i].state_interfaces.size() > 0)) + { + RCLCPP_WARN(*logger_, "Zero state interfaces for joint %s!", info_.joints[i].name); + } + for(const auto& state_interface: info_.joints[i].state_interfaces) + { + if(state_interface.name == hardware_interface::HW_IF_POSITION) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); + } + if(state_interface.name == hardware_interface::HW_IF_VELOCITY) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_states_[i].velocity_))); + } + if(state_interface.name == hardware_interface::HW_IF_EFFORT) + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_states_[i].torque_))); + } + if(state_interface.name == "temperature") + { + state_interfaces.emplace_back(hardware_interface::StateInterface( + info_.joints[i].name, "temperature", &(joint_states_[i].temperature_))); + } + else + { + RCLCPP_WARN(*logger_, "%s is wrong type of state interface, omitted!", state_interface.name); + } + } } /* IMU states (after IMUTransform transformation) */ + if(info_.sensors.size() == 0) + { + RCLCPP_WARN(*logger_, "IMU: state interface was not configured!"); + return state_interfaces; + } + + if(info_.sensors[0].state_interfaces.size() != 10) + { + RCLCPP_WARN(*logger_, "IMU: some states were not configured!"); + } + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.x", &attitude_.attitude.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.y", &attitude_.attitude.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.z", &attitude_.attitude.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "orientation.w", &attitude_.attitude.w)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.x", &attitude_.rate_dps.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.y", &attitude_.rate_dps.y)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "angular_velocity.z", &attitude_.rate_dps.z)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.x", &attitude_.accel_mps2.x)); + state_interfaces.emplace_back(hardware_interface::StateInterface( + "imu_sensor", "linear_acceleration.y", &attitude_.accel_mps2.y)); state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.x", &attitude_.attitude.x)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.y", &attitude_.attitude.y)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.z", &attitude_.attitude.z)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "orientation.w", &attitude_.attitude.w)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.x", &attitude_.rate_dps.x)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.y", &attitude_.rate_dps.y)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "angular_velocity.z", &attitude_.rate_dps.z)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.x", &attitude_.accel_mps2.x)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.y", &attitude_.accel_mps2.y)); - state_interfaces.emplace_back(hardware_interface::StateInterface( - "imu_sensor", "linear_acceleration.z", &attitude_.accel_mps2.z)); + "imu_sensor", "linear_acceleration.z", &attitude_.accel_mps2.z)); return state_interfaces; } From 93463b8812cab5db285fe27ddfe51e70cddf4bac Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 29 Jul 2024 20:02:17 +0200 Subject: [PATCH 092/137] Added urdf tests --- .../src/pi3hat_hardware_interface.cpp | 6 +- .../test/test_double_motor.urdf.xacro | 165 ++++++++++++++++++ .../test/test_single_motor.urdf.xacro | 110 ++++++++++++ 3 files changed, 278 insertions(+), 3 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 57697cac..4f2f1c5c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -36,8 +36,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa { params = get_controller_parameters(joint); - std::string type_string = joint.parameters.at("motor_type"); - controller_start_positions_.push_back(std::stod(joint.parameters.at("motor_start_position"))); + std::string type_string = joint.parameters.at("controller_type"); + controller_start_positions_.push_back(std::stod(joint.parameters.at("motor_position_start"))); type = choose_wrapper_type(type_string); } catch(const std::exception& e) @@ -333,7 +333,7 @@ std::vector Pi3HatHardwareInterface::export_ RCLCPP_WARN(*logger_, "IMU: state interface was not configured!"); return state_interfaces; } - + if(info_.sensors[0].state_interfaces.size() != 10) { RCLCPP_WARN(*logger_, "IMU: some states were not configured!"); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro new file mode 100644 index 00000000..205fb8e8 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro @@ -0,0 +1,165 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + pi3hat_hardware_interface/Pi3HatHardwareInterface + 0 + 0 + 0 + + + + moteus + 1 + 1 + + + 1 + 0.0 + 1.0 + 30.0 + -30.0 + 10.0 + 1 + + + + + + + + + + + + + + moteus + 2 + 2 + + + 1 + 0.0 + 1.0 + 30.0 + -30.0 + 10.0 + 1 + + + + + + + + + + + + + transmission_interface/FourBarLinkageTransmission + + 10 + + + 10 + + + 0.0 + 0.0 + + + 0.0 + 1.0 + + + + + + + + + + + + + + + + + + \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro new file mode 100644 index 00000000..c5f2ac39 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro @@ -0,0 +1,110 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + pi3hat_hardware_interface/Pi3HatHardwareInterface + 0 + 0 + 0 + + + + moteus + 1 + 1 + + + 1 + 0.0 + 1.0 + 30.0 + -30.0 + 10.0 + 1 + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + + 2.0 + 0.0 + + + + + + + + + + + + + + + + + + \ No newline at end of file From 94b08801f5f6e48ff34adc2ae73d56d13a1038d6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 30 Jul 2024 22:43:09 +0200 Subject: [PATCH 093/137] Adding default constructors for wrappers and edited starting controllers in pi3hat interface --- .../include/controllers/ControllerBridge.hpp | 2 + .../wrappers/ControllerWrapper.hpp | 5 ++ .../controllers/wrappers/MoteusWrapper.hpp | 2 +- .../src/pi3hat_hardware_interface.cpp | 52 +++++++++++++++---- 4 files changed, 49 insertions(+), 12 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index ae209d89..e3e08f00 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -34,6 +34,8 @@ class ControllerBridge void get_state(const CanFrame& rx_frame, ControllerState& state) const; void initialize(CanFrame& tx_frame) const; void start_up(CanFrame& tx_frame, ControllerCommand& command) const; + + ~ControllerBridge() = default; }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index d4e6f25e..1d00d1a0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -15,6 +15,11 @@ class ControllerWrapper using CanFrame = mjbots::pi3hat::CanFrame; ControllerWrapper() = default; + ControllerWrapper(const ControllerWrapper& other) = default; + ControllerWrapper(ControllerWrapper&& other) = default; + ControllerWrapper& operator=(const ControllerWrapper& other) = default; + ControllerWrapper& operator=(ControllerWrapper&& other) = default; + virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void init_to_tx_frame(CanFrame& tx_frame) = 0; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 0b735b37..113ed60c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -7,7 +7,7 @@ namespace controller_interface { -class MoteusWrapper: public ControllerWrapper +class MoteusWrapper final: public ControllerWrapper { private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 4f2f1c5c..62ac8a0c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -179,12 +179,24 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rc /* Send start command to motors */ - controllers_start_up(); - pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000000); - controllers_get_states(); + RCLCPP_INFO(*logger_, "Motors reaching starting position!"); + double error_sum = NaN; - RCLCPP_INFO(*logger_, "Hardware Interface successfully activated!"); + while(error_sum > 0.01) + { + error_sum = 0; + controllers_start_up(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(5000); + controllers_get_states(); + for(int i = 0; i < joint_controller_number_; ++i) + { + error_sum += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); + } + } + + RCLCPP_INFO(*logger_, "Motors reached starting position!"); + RCLCPP_INFO(*logger_, "Hardware Interface successfully activated!!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -225,11 +237,23 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const /* Send end commands to motors */ - controllers_start_up(); - pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000000); - controllers_get_states(); - + RCLCPP_INFO(*logger_, "Motors reaching ending position!"); + double error_sum = NaN; + + while(error_sum > 0.01) + { + error_sum = 0; + controllers_start_up(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(5000); + controllers_get_states(); + for(int i = 0; i < joint_controller_number_; ++i) + { + error_sum += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); + } + } + + RCLCPP_INFO(*logger_, "Motors reached ending position!"); RCLCPP_INFO(*logger_, "Hardware Interface successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -858,4 +882,10 @@ void Pi3HatHardwareInterface::create_controller_joint_map() { throw std::logic_error("Controller -> Joint map has diffrent length!"); } -} \ No newline at end of file +} + +#include "pluginlib/class_list_macros.hpp" + +PLUGINLIB_EXPORT_CLASS( + pi3hat_hardware_interface::Pi3HatHardwareInterface, + hardware_interface::SystemInterface) \ No newline at end of file From ed2df70d80bd9621241a186b93de68870473e1fe Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 31 Jul 2024 22:34:59 +0200 Subject: [PATCH 094/137] Edited single_motor_test urdf --- .../pi3hat_hardware_interface.hpp | 1 + .../test/test_single_motor.urdf.xacro | 35 +++++++++++++++---- 2 files changed, 29 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 9e4a5df1..38a94286 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -80,6 +80,7 @@ namespace pi3hat_hardware_interface ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::return_type write( const rclcpp::Time &time, const rclcpp::Duration &period) override; + private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro index c5f2ac39..86d36fc4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro @@ -7,32 +7,32 @@ - + - + - + - + - + - + - + @@ -51,6 +51,27 @@ + + + + + + + + + + + + + + + + + + + + + pi3hat_hardware_interface/Pi3HatHardwareInterface From 1a619ec9eef808469bc016ccb9e39ce31ff163e9 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 1 Aug 2024 22:59:43 +0200 Subject: [PATCH 095/137] Changed constructor for MoteusWrapper class --- .../controllers/wrappers/MoteusWrapper.hpp | 7 ++-- .../pi3hat_hardware_interface.hpp | 3 +- .../controllers/wrappers/MoteusWrapper.cpp | 36 +++++++++++++------ .../src/pi3hat_hardware_interface.cpp | 33 +++++------------ .../test/hardware/double_bridge_test.cpp | 31 ++-------------- .../test/hardware/single_bridge_test.cpp | 19 +--------- 6 files changed, 41 insertions(+), 88 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 113ed60c..d957ac48 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -3,6 +3,7 @@ #include "ControllerWrapper.hpp" #include "../../3rd_libs/moteus/moteus.h" +#include namespace controller_interface { @@ -18,14 +19,12 @@ class MoteusWrapper final: public ControllerWrapper /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; - mjbots::moteus::Controller moteus_controller_; + std::unique_ptr moteus_controller_; public: using CanFrame = mjbots::pi3hat::CanFrame; - MoteusWrapper( - const mjbots::moteus::Controller::Options& options, - const mjbots::moteus::PositionMode::Command& command); + MoteusWrapper(const ControllerParameters params); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 38a94286..663cff8b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -81,6 +81,7 @@ namespace pi3hat_hardware_interface hardware_interface::return_type write( const rclcpp::Time &time, const rclcpp::Duration &period) override; + ~Pi3HatHardwareInterface(); private: @@ -150,8 +151,6 @@ namespace pi3hat_hardware_interface Remember to change this function in source code */ WrapperType choose_wrapper_type(const std::string& type); - std::unique_ptr create_moteus_wrapper(const controller_interface::ControllerParameters& params); - /* Function for creating moteus wrappers (here u can add your own wrapper) */ void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 174a870d..89a945bb 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -2,13 +2,29 @@ using namespace controller_interface; -MoteusWrapper::MoteusWrapper( - const mjbots::moteus::Controller::Options& options, - const mjbots::moteus::PositionMode::Command& command): +MoteusWrapper::MoteusWrapper(const ControllerParameters params): + ControllerWrapper() +{ + /* moteus options */ + using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + /* moteus command format (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + + /* moteus command (it will be copied to wrapper) */ + position_command_.maximum_torque = params.torque_max_; + position_command_.velocity_limit = params.velocity_max_; - ControllerWrapper(), - position_command_(command), - moteus_controller_(mjbots::moteus::Controller(options)) {} + moteus_controller_ = std::make_unique(moteus_options); +} void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) @@ -19,7 +35,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm position_command_.feedforward_torque = command.torque_; /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(position_command_); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame */ tx_frame.id = can_fd_frame.arbitration_id; @@ -31,7 +47,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ + if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_->options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians_; @@ -44,7 +60,7 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) { /* create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakeStop(); /* Copy data from CANFD frame to CAN frame*/ tx_frame.id = can_fd_frame.arbitration_id; @@ -65,7 +81,7 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo start_command.velocity_limit = startup_coefficient_ * position_command_.velocity_limit * radians_to_rotation_; /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(start_command); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakePosition(start_command); /* Copy data from CANFD frame to CAN frame */ tx_frame.id = can_fd_frame.arbitration_id; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 62ac8a0c..229b7d00 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -765,7 +765,7 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& switch(type) { case Moteus: - wrapper_ptr = create_moteus_wrapper(params); + wrapper_ptr = std::make_unique(params); break; default: throw std::invalid_argument("Motor type doesn't exist!"); @@ -776,30 +776,6 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& controller_bridges_.push_back(std::move(bridge)); } -std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) -{ - /* moteus options */ - using mjbots::moteus::Controller; - using controller_interface::MoteusWrapper; - Controller::Options moteus_options; - moteus_options.bus = params.bus_; - moteus_options.id = params.id_; - - /* moteus command format (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - format.velocity_limit= mjbots::moteus::kFloat; - moteus_options.position_format = format; - - /* moteus command (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Command moteus_command; - moteus_command.maximum_torque = params.torque_max_; - moteus_command.velocity_limit = params.velocity_max_; - - return std::make_unique(moteus_options, moteus_command); -} - Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) { if(type == "moteus") @@ -884,6 +860,13 @@ void Pi3HatHardwareInterface::create_controller_joint_map() } } + +Pi3HatHardwareInterface::~Pi3HatHardwareInterface() +{ + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); +} + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp index 38122462..ec1df269 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp @@ -64,42 +64,15 @@ int main(int argc, char** argv) params_2.bus_ = 2; params_2.id_ = 2; - // moteusues options - using mjbots::moteus::Controller; - Controller::Options moteus_1_options; - moteus_1_options.bus = 1; - moteus_1_options.id = 1; - - Controller::Options moteus_2_options; - moteus_2_options.bus = 2; - moteus_2_options.id = 2; - - // moteus command format (it will be copied to wrapper) - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - moteus_1_options.position_format = format; - moteus_2_options.position_format = format; - - //moteus command (it will be copied to wrapper) - mjbots::moteus::PositionMode::Command moteus_1_command; - moteus_1_command.maximum_torque = params_1.torque_max_; - moteus_1_command.velocity_limit = params_1.velocity_max_; - - mjbots::moteus::PositionMode::Command moteus_2_command; - moteus_2_command.maximum_torque = params_2.torque_max_; - moteus_2_command.velocity_limit = params_2.velocity_max_; - - std::vector controllers; std::vector controller_commands; std::vector controller_states; - controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_1_command); + controller_interface::MoteusWrapper moteus_wrapper_1(params_1); std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); - controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_1_command); + controller_interface::MoteusWrapper moteus_wrapper_2(params_2); std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 1407fd28..eb45b3da 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -54,24 +54,7 @@ int main(int argc, char** argv) params.bus_ = 1; params.id_ = 1; - // moteus options - using mjbots::moteus::Controller; - Controller::Options moteus_options; - moteus_options.bus = 1; - moteus_options.id = 1; - - // moteus command format (it will be copied to wrapper) - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - moteus_options.position_format = format; - - //moteus command (it will be copied to wrapper) - mjbots::moteus::PositionMode::Command moteus_command; - moteus_command.maximum_torque = params.torque_max_; - moteus_command.velocity_limit = params.velocity_max_; - - controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); + controller_interface::MoteusWrapper moteus_wrapper(params); std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); From c337c6127e1a5a176fcb6352f14afe1229cab2af Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 13:02:27 +0200 Subject: [PATCH 096/137] Fixed issiue with moving wrappers to bridge --- .../include/controllers/wrappers/MoteusWrapper.hpp | 5 +++++ .../src/pi3hat_hardware_interface.cpp | 4 ++++ .../test/hardware/double_bridge_test.cpp | 4 ++-- .../test/hardware/single_bridge_test.cpp | 2 +- 4 files changed, 12 insertions(+), 3 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index d957ac48..0a8c463d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -25,6 +25,11 @@ class MoteusWrapper final: public ControllerWrapper using CanFrame = mjbots::pi3hat::CanFrame; MoteusWrapper(const ControllerParameters params); + MoteusWrapper(const MoteusWrapper& other) = delete; + MoteusWrapper& operator=(const MoteusWrapper& other) = delete; + MoteusWrapper(MoteusWrapper&& other) = default; + MoteusWrapper& operator=(MoteusWrapper&& other) = default; /* Wrappers needs to be moved (they have unique_ptr) */ + void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 229b7d00..d017e3e0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -778,6 +778,10 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) { + /* Changing string to lower case */ + std::transform(type.begin(), type.end(), type.begin(), + [](unsigned char c){ return std::tolower(c); }); + if(type == "moteus") { return WrapperType::Moteus; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp index ec1df269..e707c6b9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp @@ -69,11 +69,11 @@ int main(int argc, char** argv) std::vector controller_states; controller_interface::MoteusWrapper moteus_wrapper_1(params_1); - std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); + std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(std::move(moteus_wrapper_1)); controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); controller_interface::MoteusWrapper moteus_wrapper_2(params_2); - std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); + std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(std::move(moteus_wrapper_2)); controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); controllers.push_back(std::move(controller_1)); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index eb45b3da..52115a87 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -55,7 +55,7 @@ int main(int argc, char** argv) params.id_ = 1; controller_interface::MoteusWrapper moteus_wrapper(params); - std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(std::move(moteus_wrapper)); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); From f76d35db9efa9c5066ca329aa9ecc79733dff76b Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 13:37:54 +0200 Subject: [PATCH 097/137] Changing pointer to moteus_controller object --- .../include/controllers/wrappers/MoteusWrapper.hpp | 9 ++------- .../src/controllers/wrappers/MoteusWrapper.cpp | 9 +++++++-- 2 files changed, 9 insertions(+), 9 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 0a8c463d..5b0d8185 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -19,23 +19,18 @@ class MoteusWrapper final: public ControllerWrapper /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; - std::unique_ptr moteus_controller_; + mjbots::moteus::Controller* moteus_controller_; public: using CanFrame = mjbots::pi3hat::CanFrame; MoteusWrapper(const ControllerParameters params); - MoteusWrapper(const MoteusWrapper& other) = delete; - MoteusWrapper& operator=(const MoteusWrapper& other) = delete; - MoteusWrapper(MoteusWrapper&& other) = default; - MoteusWrapper& operator=(MoteusWrapper&& other) = default; /* Wrappers needs to be moved (they have unique_ptr) */ - void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; - // ~MoteusWrapper() override = default; + ~MoteusWrapper(); }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 89a945bb..bb9a0771 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -3,7 +3,7 @@ using namespace controller_interface; MoteusWrapper::MoteusWrapper(const ControllerParameters params): - ControllerWrapper() + ControllerWrapper(), moteus_controller_(), position_command_() { /* moteus options */ using mjbots::moteus::Controller; @@ -23,7 +23,7 @@ MoteusWrapper::MoteusWrapper(const ControllerParameters params): position_command_.maximum_torque = params.torque_max_; position_command_.velocity_limit = params.velocity_max_; - moteus_controller_ = std::make_unique(moteus_options); + moteus_controller_ = new Controller(moteus_options); } @@ -89,3 +89,8 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } + +MoteusWrapper::~MoteusWrapper() +{ + delete moteus_controller_; +} \ No newline at end of file From c317db7b8a4f7cefeb0b84466f2dc26928b2fe1d Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 13:49:38 +0200 Subject: [PATCH 098/137] Changed strategy for making moteus wrappers --- .../controllers/wrappers/MoteusWrapper.hpp | 10 ++-- .../controllers/wrappers/MoteusWrapper.cpp | 47 +++++++++---------- .../test/hardware/single_bridge_test.cpp | 3 +- 3 files changed, 30 insertions(+), 30 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 5b0d8185..032a025d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -19,21 +19,23 @@ class MoteusWrapper final: public ControllerWrapper /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; - mjbots::moteus::Controller* moteus_controller_; + mjbots::moteus::Controller moteus_controller_; public: using CanFrame = mjbots::pi3hat::CanFrame; - MoteusWrapper(const ControllerParameters params); + MoteusWrapper(const ControllerParameters params, + mjbots::moteus::Controller::Options moteus_options); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; - ~MoteusWrapper(); - }; +/* Function for creating unique pointer for moteus wrapper (used in pi3hat_hardware_interface) */ +std::unique_ptr make_moteus_wrapper(const ControllerParameters params); + }; #endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index bb9a0771..f6df21e1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -2,28 +2,13 @@ using namespace controller_interface; -MoteusWrapper::MoteusWrapper(const ControllerParameters params): - ControllerWrapper(), moteus_controller_(), position_command_() +MoteusWrapper::MoteusWrapper(const ControllerParameters params, + mjbots::moteus::Controller::Options moteus_options): + ControllerWrapper(), moteus_controller_(moteus_options), position_command_() { - /* moteus options */ - using mjbots::moteus::Controller; - using controller_interface::MoteusWrapper; - Controller::Options moteus_options; - moteus_options.bus = params.bus_; - moteus_options.id = params.id_; - - /* moteus command format (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - format.velocity_limit= mjbots::moteus::kFloat; - moteus_options.position_format = format; - /* moteus command (it will be copied to wrapper) */ position_command_.maximum_torque = params.torque_max_; position_command_.velocity_limit = params.velocity_max_; - - moteus_controller_ = new Controller(moteus_options); } @@ -35,7 +20,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm position_command_.feedforward_torque = command.torque_; /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakePosition(position_command_); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame */ tx_frame.id = can_fd_frame.arbitration_id; @@ -47,7 +32,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_->options().id) return; /* This should not happen! (map frame to wrapper first) */ + if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); state.position_ = result.position * rotation_to_radians_; @@ -60,7 +45,7 @@ void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) { /* create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakeStop(); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); /* Copy data from CANFD frame to CAN frame*/ tx_frame.id = can_fd_frame.arbitration_id; @@ -81,7 +66,7 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo start_command.velocity_limit = startup_coefficient_ * position_command_.velocity_limit * radians_to_rotation_; /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_->MakePosition(start_command); + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(start_command); /* Copy data from CANFD frame to CAN frame */ tx_frame.id = can_fd_frame.arbitration_id; @@ -90,7 +75,21 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } -MoteusWrapper::~MoteusWrapper() + +std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters params) { - delete moteus_controller_; + /* moteus options */ + using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + /* moteus command format (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + return std::make_unique(params, moteus_options); } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 52115a87..03124d5d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -54,8 +54,7 @@ int main(int argc, char** argv) params.bus_ = 1; params.id_ = 1; - controller_interface::MoteusWrapper moteus_wrapper(params); - std::unique_ptr moteus_wrapper_ptr = std::make_unique(std::move(moteus_wrapper)); + std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); From df988b294f217d92162e5d76960e518eb6d1272c Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 13:59:11 +0200 Subject: [PATCH 099/137] Fixing moteus issiue --- .../src/controllers/wrappers/MoteusWrapper.cpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index f6df21e1..02abe8df 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -91,5 +91,7 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C format.maximum_torque = mjbots::moteus::kFloat; format.velocity_limit= mjbots::moteus::kFloat; moteus_options.position_format = format; - return std::make_unique(params, moteus_options); + + MoteusWrapper wrapper = MoteusWrapper(params, moteus_options); + return std::make_unique(wrapper); } \ No newline at end of file From 9b9d48ad0735cea2b1a55f55daf735e42f631cd2 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:14:43 +0200 Subject: [PATCH 100/137] Fixing moteus issiue --- .../include/controllers/wrappers/MoteusWrapper.hpp | 7 +++++++ .../src/controllers/wrappers/MoteusWrapper.cpp | 8 +++++++- 2 files changed, 14 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 032a025d..43ac8bf1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -26,11 +26,18 @@ class MoteusWrapper final: public ControllerWrapper MoteusWrapper(const ControllerParameters params, mjbots::moteus::Controller::Options moteus_options); + MoteusWrapper(const MoteusWrapper& other); + MoteusWrapper& operator=(const MoteusWrapper& other) = delete; + MoteusWrapper(MoteusWrapper&& other); + MoteusWrapper& operator=(MoteusWrapper&& other) = delete; + + void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; + ~MoteusWrapper() = default; }; /* Function for creating unique pointer for moteus wrapper (used in pi3hat_hardware_interface) */ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 02abe8df..54634513 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -94,4 +94,10 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C MoteusWrapper wrapper = MoteusWrapper(params, moteus_options); return std::make_unique(wrapper); -} \ No newline at end of file +} + +MoteusWrapper::MoteusWrapper(const MoteusWrapper& other): + ControllerWrapper(), moteus_controller_(other.moteus_controller_.options()), position_command_(other.position_command_) {} + +MoteusWrapper::MoteusWrapper(MoteusWrapper&& other): + ControllerWrapper(), moteus_controller_(other.moteus_controller_.options()), position_command_(other.position_command_) {} \ No newline at end of file From 6eca9057b6968c31585c1ee4660ad8c1fdf993c7 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:20:41 +0200 Subject: [PATCH 101/137] Fixing moteus issiue --- .../src/controllers/wrappers/MoteusWrapper.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 54634513..cd68de5e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -92,8 +92,7 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C format.velocity_limit= mjbots::moteus::kFloat; moteus_options.position_format = format; - MoteusWrapper wrapper = MoteusWrapper(params, moteus_options); - return std::make_unique(wrapper); + return std::make_unique(params, moteus_options); } MoteusWrapper::MoteusWrapper(const MoteusWrapper& other): From 6f55e664e5986a0d7da4f60d84e452ce4d1cdcd3 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:29:00 +0200 Subject: [PATCH 102/137] Added second test for moteus controller --- .../test/hardware/simple_test_2.cpp | 89 +++++++++++++++++++ 1 file changed, 89 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp new file mode 100644 index 00000000..404a249f --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp @@ -0,0 +1,89 @@ +#include "../../include/3rd_libs/pi3hat/pi3hat.h" +#include "../../include/3rd_libs/pi3hat/realtime.h" +#include "../../include/3rd_libs/moteus/moteus.h" + + +static double GetNow() +{ + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; +}; + +int main(int argc, char** argv) +{ + // moteus part + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = 1; + moteus_options.id = 1; + std::unique_ptr moteus_ptr; + { + Controller moteus_controller(moteus_options); + moteus_ptr = std::make_unique(moteus_controller); + }; + + mjbots::moteus::PositionMode::Command moteus_command; + + //pi3hat part + + mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; + pi3hat_configuration.attitude_rate_hz = 1000; + + mjbots::pi3hat::CanFrame tx_frame; + mjbots::pi3hat::CanFrame rx_frame[10]; + tx_frame.id = 1; + tx_frame.bus = 1; + tx_frame.expect_reply = true; + mjbots::pi3hat::Span tx_span(&tx_frame, 1); + mjbots::pi3hat::Span rx_span(rx_frame, 10); + mjbots::pi3hat::Attitude attitude; + + mjbots::pi3hat::Pi3Hat::Input input; + mjbots::pi3hat::Pi3Hat::Output pi3hat_output; + input.tx_can = tx_span; + input.rx_can = rx_span; + input.attitude = &attitude; + input.request_attitude = true; + + std::unique_ptr pi3hat = std::make_unique(pi3hat_configuration); + + mjbots::moteus::CanFdFrame can_fd_frame = moteus_ptr->MakeStop(); + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + + pi3hat_output = pi3hat->Cycle(input); + std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; + // Buffer for printing info + char buf[4096] = {}; + + auto prev = GetNow(); + int frequency; + std::string status_line; + while(true) + { + auto now = GetNow(); + double position_ = 5 * sin(now - prev); + moteus_command.position = position_; + mjbots::moteus::CanFdFrame can_fd_frame = moteus_ptr->MakePosition(moteus_command); + + /* Copy data from CANFD frame to CAN frame*/ + tx_frame.bus = can_fd_frame.bus; + tx_frame.id = can_fd_frame.arbitration_id; // DZIEKI TEMU DZIALAAA + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); + + ::usleep(1000); + auto mesaure_time = GetNow() - now; + frequency = (int) 1/mesaure_time; + pi3hat_output = pi3hat->Cycle(input); + mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); + double state_position = result.position; + ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", + frequency, position_, state_position ); + ::fflush(::stdout); + + }; + return 0; +} \ No newline at end of file From 2bf5ad2d489d570744c9cf32543c328b6ad2b22a Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:32:48 +0200 Subject: [PATCH 103/137] Edited second test for moteus controller --- .../test/hardware/simple_test_2.cpp | 10 ++++++---- 1 file changed, 6 insertions(+), 4 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp index 404a249f..3a6cce10 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp @@ -3,6 +3,11 @@ #include "../../include/3rd_libs/moteus/moteus.h" +std::unique_ptr make_moteus_controller(mjbots::moteus::Controller::Options moteus_options) +{ + mjbots::moteus::Controller controller(moteus_options); + return std::make_unique(controller); +} static double GetNow() { struct timespec ts = {}; @@ -19,10 +24,7 @@ int main(int argc, char** argv) moteus_options.bus = 1; moteus_options.id = 1; std::unique_ptr moteus_ptr; - { - Controller moteus_controller(moteus_options); - moteus_ptr = std::make_unique(moteus_controller); - }; + moteus_ptr = make_moteus_controller(moteus_options); mjbots::moteus::PositionMode::Command moteus_command; From 18c059a010b0b77363019a56c74d731973069541 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:36:20 +0200 Subject: [PATCH 104/137] Added test for moteus wrapper --- .../test/hardware/single_wrapper_test.cpp | 108 ++++++++++++++++++ 1 file changed, 108 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp new file mode 100644 index 00000000..b4964f71 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp @@ -0,0 +1,108 @@ +#include "../../include/controllers/Controllers.hpp" + + + +#include "../../include/3rd_libs/pi3hat/pi3hat.h" +#include "../../include/3rd_libs/pi3hat/realtime.h" +#include +#include +#include +#include + + +static double GetNow() +{ + struct timespec ts = {}; + ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); + return static_cast(ts.tv_sec) + + static_cast(ts.tv_nsec) / 1e9; +}; + +int main(int argc, char** argv) +{ + // pi3hat + mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; + pi3hat_configuration.attitude_rate_hz = 1000; + + + mjbots::pi3hat::CanFrame tx_frame; + mjbots::pi3hat::CanFrame rx_frame; + tx_frame.expect_reply = true; + + mjbots::pi3hat::Span tx_span(&tx_frame, 1); + mjbots::pi3hat::Span rx_span(&rx_frame, 1); + mjbots::pi3hat::Attitude attitude; + + mjbots::pi3hat::Pi3Hat::Input input; + + input.tx_can = tx_span; + input.rx_can = rx_span; + input.attitude = &attitude; + input.request_attitude = true; + + mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); + + // pi3hat output + mjbots::pi3hat::Pi3Hat::Output pi3hat_output; + + // moteus wrapper + controller_interface::ControllerParameters params; + params.direction_ = 1; + params.position_max_ = 10; + params.position_min_ = -10; + params.velocity_max_ = 4; + params.torque_max_ = 1; + params.bus_ = 1; + params.id_ = 1; + + std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); + + + controller_interface::ControllerCommand controller_command; + controller_command.velocity_ = 0; + controller_command.torque_ = 0; + + controller_interface::ControllerState controller_state; + + std::cout << "Options for controller succesfully initialized!" << std::endl; + + + mjbots::pi3hat::ConfigureRealtime(0); + std::cout << "Realtime control activated!" << std::endl; + + // set stop to moteus + moteus_wrapper_ptr->init_to_tx_frame(tx_frame); + pi3hat_output = pi3hat.Cycle(input); + ::usleep(10000); + + controller_command.position_ = 0; + controller_state.position_ = NaN; + while(std::abs(controller_state.position_) > 0.1) + { + moteus_wrapper_ptr->start_pos_to_tx_frame(tx_frame, controller_command); + pi3hat_output = pi3hat.Cycle(input); + ::usleep(2000); + moteus_wrapper_ptr->rx_frame_to_state(rx_frame, controller_state); + } + + std::cout << "Controller successfully started!" << std::endl; + + auto prev = GetNow(); + int frequency; + while(true) + { + auto now = GetNow(); + controller_command.position_ = 5 * sin(now - prev); + moteus_wrapper_ptr->command_to_tx_frame(tx_frame, controller_command); + pi3hat_output = pi3hat.Cycle(input); + ::usleep(1000); + auto mesaure_time = GetNow() - now; + frequency = (int) 1/mesaure_time; + moteus_wrapper_ptr->rx_frame_to_state(rx_frame, controller_state); + ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", + frequency, controller_command.position_, controller_state.position_); + ::fflush(::stdout); + } + + return 0; +} \ No newline at end of file From dab498d3b37b5556c40f130185239849313206b8 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:45:36 +0200 Subject: [PATCH 105/137] Fixed MoteusWrapper constructor --- .../src/controllers/wrappers/MoteusWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index cd68de5e..409a0fee 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -4,7 +4,7 @@ using namespace controller_interface; MoteusWrapper::MoteusWrapper(const ControllerParameters params, mjbots::moteus::Controller::Options moteus_options): - ControllerWrapper(), moteus_controller_(moteus_options), position_command_() + ControllerWrapper(), moteus_controller_(mjbots::moteus::Controller(moteus_options)), position_command_() { /* moteus command (it will be copied to wrapper) */ position_command_.maximum_torque = params.torque_max_; From e79d33e06515e7c3f02ecf2f3415d55f82c95c85 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 14:57:00 +0200 Subject: [PATCH 106/137] Got back to Edited single_motor_test urdf --- .../controllers/wrappers/MoteusWrapper.hpp | 18 +-- .../pi3hat_hardware_interface.hpp | 3 +- .../controllers/wrappers/MoteusWrapper.cpp | 41 ++----- .../src/pi3hat_hardware_interface.cpp | 37 ++++-- .../test/hardware/double_bridge_test.cpp | 35 +++++- .../test/hardware/simple_test_2.cpp | 91 --------------- .../test/hardware/single_bridge_test.cpp | 20 +++- .../test/hardware/single_wrapper_test.cpp | 108 ------------------ 8 files changed, 89 insertions(+), 264 deletions(-) delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 43ac8bf1..113ed60c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -3,7 +3,6 @@ #include "ControllerWrapper.hpp" #include "../../3rd_libs/moteus/moteus.h" -#include namespace controller_interface { @@ -24,24 +23,17 @@ class MoteusWrapper final: public ControllerWrapper public: using CanFrame = mjbots::pi3hat::CanFrame; - MoteusWrapper(const ControllerParameters params, - mjbots::moteus::Controller::Options moteus_options); - MoteusWrapper(const MoteusWrapper& other); - MoteusWrapper& operator=(const MoteusWrapper& other) = delete; - MoteusWrapper(MoteusWrapper&& other); - MoteusWrapper& operator=(MoteusWrapper&& other) = delete; - - + MoteusWrapper( + const mjbots::moteus::Controller::Options& options, + const mjbots::moteus::PositionMode::Command& command); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; - ~MoteusWrapper() = default; -}; + // ~MoteusWrapper() override = default; -/* Function for creating unique pointer for moteus wrapper (used in pi3hat_hardware_interface) */ -std::unique_ptr make_moteus_wrapper(const ControllerParameters params); +}; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 663cff8b..38a94286 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -81,7 +81,6 @@ namespace pi3hat_hardware_interface hardware_interface::return_type write( const rclcpp::Time &time, const rclcpp::Duration &period) override; - ~Pi3HatHardwareInterface(); private: @@ -151,6 +150,8 @@ namespace pi3hat_hardware_interface Remember to change this function in source code */ WrapperType choose_wrapper_type(const std::string& type); + std::unique_ptr create_moteus_wrapper(const controller_interface::ControllerParameters& params); + /* Function for creating moteus wrappers (here u can add your own wrapper) */ void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 409a0fee..174a870d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -2,14 +2,13 @@ using namespace controller_interface; -MoteusWrapper::MoteusWrapper(const ControllerParameters params, - mjbots::moteus::Controller::Options moteus_options): - ControllerWrapper(), moteus_controller_(mjbots::moteus::Controller(moteus_options)), position_command_() -{ - /* moteus command (it will be copied to wrapper) */ - position_command_.maximum_torque = params.torque_max_; - position_command_.velocity_limit = params.velocity_max_; -} +MoteusWrapper::MoteusWrapper( + const mjbots::moteus::Controller::Options& options, + const mjbots::moteus::PositionMode::Command& command): + + ControllerWrapper(), + position_command_(command), + moteus_controller_(mjbots::moteus::Controller(options)) {} void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) @@ -74,29 +73,3 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } - - -std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters params) -{ - /* moteus options */ - using mjbots::moteus::Controller; - using controller_interface::MoteusWrapper; - Controller::Options moteus_options; - moteus_options.bus = params.bus_; - moteus_options.id = params.id_; - - /* moteus command format (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - format.velocity_limit= mjbots::moteus::kFloat; - moteus_options.position_format = format; - - return std::make_unique(params, moteus_options); -} - -MoteusWrapper::MoteusWrapper(const MoteusWrapper& other): - ControllerWrapper(), moteus_controller_(other.moteus_controller_.options()), position_command_(other.position_command_) {} - -MoteusWrapper::MoteusWrapper(MoteusWrapper&& other): - ControllerWrapper(), moteus_controller_(other.moteus_controller_.options()), position_command_(other.position_command_) {} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index d017e3e0..62ac8a0c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -765,7 +765,7 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& switch(type) { case Moteus: - wrapper_ptr = std::make_unique(params); + wrapper_ptr = create_moteus_wrapper(params); break; default: throw std::invalid_argument("Motor type doesn't exist!"); @@ -776,12 +776,32 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& controller_bridges_.push_back(std::move(bridge)); } -Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) +std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) { - /* Changing string to lower case */ - std::transform(type.begin(), type.end(), type.begin(), - [](unsigned char c){ return std::tolower(c); }); + /* moteus options */ + using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + /* moteus command format (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + + /* moteus command (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Command moteus_command; + moteus_command.maximum_torque = params.torque_max_; + moteus_command.velocity_limit = params.velocity_max_; + + return std::make_unique(moteus_options, moteus_command); +} +Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) +{ if(type == "moteus") { return WrapperType::Moteus; @@ -864,13 +884,6 @@ void Pi3HatHardwareInterface::create_controller_joint_map() } } - -Pi3HatHardwareInterface::~Pi3HatHardwareInterface() -{ - on_deactivate(rclcpp_lifecycle::State()); - on_cleanup(rclcpp_lifecycle::State()); -} - #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp index e707c6b9..38122462 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp @@ -64,16 +64,43 @@ int main(int argc, char** argv) params_2.bus_ = 2; params_2.id_ = 2; + // moteusues options + using mjbots::moteus::Controller; + Controller::Options moteus_1_options; + moteus_1_options.bus = 1; + moteus_1_options.id = 1; + + Controller::Options moteus_2_options; + moteus_2_options.bus = 2; + moteus_2_options.id = 2; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_1_options.position_format = format; + moteus_2_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_1_command; + moteus_1_command.maximum_torque = params_1.torque_max_; + moteus_1_command.velocity_limit = params_1.velocity_max_; + + mjbots::moteus::PositionMode::Command moteus_2_command; + moteus_2_command.maximum_torque = params_2.torque_max_; + moteus_2_command.velocity_limit = params_2.velocity_max_; + + std::vector controllers; std::vector controller_commands; std::vector controller_states; - controller_interface::MoteusWrapper moteus_wrapper_1(params_1); - std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(std::move(moteus_wrapper_1)); + controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_1_command); + std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); - controller_interface::MoteusWrapper moteus_wrapper_2(params_2); - std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(std::move(moteus_wrapper_2)); + controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_1_command); + std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); controllers.push_back(std::move(controller_1)); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp deleted file mode 100644 index 3a6cce10..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test_2.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "../../include/3rd_libs/pi3hat/pi3hat.h" -#include "../../include/3rd_libs/pi3hat/realtime.h" -#include "../../include/3rd_libs/moteus/moteus.h" - - -std::unique_ptr make_moteus_controller(mjbots::moteus::Controller::Options moteus_options) -{ - mjbots::moteus::Controller controller(moteus_options); - return std::make_unique(controller); -} -static double GetNow() -{ - struct timespec ts = {}; - ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); - return static_cast(ts.tv_sec) + - static_cast(ts.tv_nsec) / 1e9; -}; - -int main(int argc, char** argv) -{ - // moteus part - using mjbots::moteus::Controller; - Controller::Options moteus_options; - moteus_options.bus = 1; - moteus_options.id = 1; - std::unique_ptr moteus_ptr; - moteus_ptr = make_moteus_controller(moteus_options); - - mjbots::moteus::PositionMode::Command moteus_command; - - //pi3hat part - - mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; - pi3hat_configuration.attitude_rate_hz = 1000; - - mjbots::pi3hat::CanFrame tx_frame; - mjbots::pi3hat::CanFrame rx_frame[10]; - tx_frame.id = 1; - tx_frame.bus = 1; - tx_frame.expect_reply = true; - mjbots::pi3hat::Span tx_span(&tx_frame, 1); - mjbots::pi3hat::Span rx_span(rx_frame, 10); - mjbots::pi3hat::Attitude attitude; - - mjbots::pi3hat::Pi3Hat::Input input; - mjbots::pi3hat::Pi3Hat::Output pi3hat_output; - input.tx_can = tx_span; - input.rx_can = rx_span; - input.attitude = &attitude; - input.request_attitude = true; - - std::unique_ptr pi3hat = std::make_unique(pi3hat_configuration); - - mjbots::moteus::CanFdFrame can_fd_frame = moteus_ptr->MakeStop(); - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - - pi3hat_output = pi3hat->Cycle(input); - std::cout << "Started: " << pi3hat_output.rx_can_size << std::endl; - // Buffer for printing info - char buf[4096] = {}; - - auto prev = GetNow(); - int frequency; - std::string status_line; - while(true) - { - auto now = GetNow(); - double position_ = 5 * sin(now - prev); - moteus_command.position = position_; - mjbots::moteus::CanFdFrame can_fd_frame = moteus_ptr->MakePosition(moteus_command); - - /* Copy data from CANFD frame to CAN frame*/ - tx_frame.bus = can_fd_frame.bus; - tx_frame.id = can_fd_frame.arbitration_id; // DZIEKI TEMU DZIALAAA - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); - - ::usleep(1000); - auto mesaure_time = GetNow() - now; - frequency = (int) 1/mesaure_time; - pi3hat_output = pi3hat->Cycle(input); - mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame[0].data, rx_frame[0].size); - double state_position = result.position; - ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", - frequency, position_, state_position ); - ::fflush(::stdout); - - }; - return 0; -} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 03124d5d..1407fd28 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -54,7 +54,25 @@ int main(int argc, char** argv) params.bus_ = 1; params.id_ = 1; - std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); + // moteus options + using mjbots::moteus::Controller; + Controller::Options moteus_options; + moteus_options.bus = 1; + moteus_options.id = 1; + + // moteus command format (it will be copied to wrapper) + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + moteus_options.position_format = format; + + //moteus command (it will be copied to wrapper) + mjbots::moteus::PositionMode::Command moteus_command; + moteus_command.maximum_torque = params.torque_max_; + moteus_command.velocity_limit = params.velocity_max_; + + controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp deleted file mode 100644 index b4964f71..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_wrapper_test.cpp +++ /dev/null @@ -1,108 +0,0 @@ -#include "../../include/controllers/Controllers.hpp" - - - -#include "../../include/3rd_libs/pi3hat/pi3hat.h" -#include "../../include/3rd_libs/pi3hat/realtime.h" -#include -#include -#include -#include - - -static double GetNow() -{ - struct timespec ts = {}; - ::clock_gettime(CLOCK_MONOTONIC_RAW, &ts); - return static_cast(ts.tv_sec) + - static_cast(ts.tv_nsec) / 1e9; -}; - -int main(int argc, char** argv) -{ - // pi3hat - mjbots::pi3hat::Pi3Hat::Configuration pi3hat_configuration; - pi3hat_configuration.attitude_rate_hz = 1000; - - - mjbots::pi3hat::CanFrame tx_frame; - mjbots::pi3hat::CanFrame rx_frame; - tx_frame.expect_reply = true; - - mjbots::pi3hat::Span tx_span(&tx_frame, 1); - mjbots::pi3hat::Span rx_span(&rx_frame, 1); - mjbots::pi3hat::Attitude attitude; - - mjbots::pi3hat::Pi3Hat::Input input; - - input.tx_can = tx_span; - input.rx_can = rx_span; - input.attitude = &attitude; - input.request_attitude = true; - - mjbots::pi3hat::Pi3Hat pi3hat(pi3hat_configuration); - - // pi3hat output - mjbots::pi3hat::Pi3Hat::Output pi3hat_output; - - // moteus wrapper - controller_interface::ControllerParameters params; - params.direction_ = 1; - params.position_max_ = 10; - params.position_min_ = -10; - params.velocity_max_ = 4; - params.torque_max_ = 1; - params.bus_ = 1; - params.id_ = 1; - - std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); - - - controller_interface::ControllerCommand controller_command; - controller_command.velocity_ = 0; - controller_command.torque_ = 0; - - controller_interface::ControllerState controller_state; - - std::cout << "Options for controller succesfully initialized!" << std::endl; - - - mjbots::pi3hat::ConfigureRealtime(0); - std::cout << "Realtime control activated!" << std::endl; - - // set stop to moteus - moteus_wrapper_ptr->init_to_tx_frame(tx_frame); - pi3hat_output = pi3hat.Cycle(input); - ::usleep(10000); - - controller_command.position_ = 0; - controller_state.position_ = NaN; - while(std::abs(controller_state.position_) > 0.1) - { - moteus_wrapper_ptr->start_pos_to_tx_frame(tx_frame, controller_command); - pi3hat_output = pi3hat.Cycle(input); - ::usleep(2000); - moteus_wrapper_ptr->rx_frame_to_state(rx_frame, controller_state); - } - - std::cout << "Controller successfully started!" << std::endl; - - auto prev = GetNow(); - int frequency; - while(true) - { - auto now = GetNow(); - controller_command.position_ = 5 * sin(now - prev); - moteus_wrapper_ptr->command_to_tx_frame(tx_frame, controller_command); - pi3hat_output = pi3hat.Cycle(input); - ::usleep(1000); - auto mesaure_time = GetNow() - now; - frequency = (int) 1/mesaure_time; - moteus_wrapper_ptr->rx_frame_to_state(rx_frame, controller_state); - ::printf("f, pos_c, pos_s=(%d, %7.3f, %7.3f)\r", - frequency, controller_command.position_, controller_state.position_); - ::fflush(::stdout); - } - - return 0; -} \ No newline at end of file From 7748548e25596e1e42d0c7abda50fb0d5313fbfa Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 15:02:49 +0200 Subject: [PATCH 107/137] Added function for creating moteus wrapper --- .../controllers/wrappers/MoteusWrapper.hpp | 3 +++ .../controllers/wrappers/MoteusWrapper.cpp | 24 +++++++++++++++++++ .../test/hardware/single_bridge_test.cpp | 3 +-- 3 files changed, 28 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 113ed60c..1ef87763 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -3,6 +3,7 @@ #include "ControllerWrapper.hpp" #include "../../3rd_libs/moteus/moteus.h" +#include namespace controller_interface { @@ -35,6 +36,8 @@ class MoteusWrapper final: public ControllerWrapper }; +std::unique_ptr make_moteus_wrapper(const ControllerParameters& params); + }; #endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 174a870d..19b99871 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -73,3 +73,27 @@ void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCo tx_frame.size = can_fd_frame.size; std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } + +std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters& params) +{ + /* moteus options */ + using mjbots::moteus::Controller; + using controller_interface::MoteusWrapper; + Controller::Options moteus_options; + moteus_options.bus = params.bus_; + moteus_options.id = params.id_; + + /* moteus command format (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Format format; + format.feedforward_torque = mjbots::moteus::kFloat; + format.maximum_torque = mjbots::moteus::kFloat; + format.velocity_limit= mjbots::moteus::kFloat; + moteus_options.position_format = format; + + /* moteus command (it will be copied to wrapper) */ + mjbots::moteus::PositionMode::Command moteus_command; + moteus_command.maximum_torque = params.torque_max_; + moteus_command.velocity_limit = params.velocity_max_; + + return std::make_unique(moteus_options, moteus_command); +} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 1407fd28..996c2276 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -71,8 +71,7 @@ int main(int argc, char** argv) moteus_command.maximum_torque = params.torque_max_; moteus_command.velocity_limit = params.velocity_max_; - controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); - std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); From ede35a2beec0b2e15382a8f757ee38742d817e23 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 15:05:42 +0200 Subject: [PATCH 108/137] Changed make_moteus_wrapper --- .../src/controllers/wrappers/MoteusWrapper.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 19b99871..1d3c2f30 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -94,6 +94,8 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C mjbots::moteus::PositionMode::Command moteus_command; moteus_command.maximum_torque = params.torque_max_; moteus_command.velocity_limit = params.velocity_max_; - - return std::make_unique(moteus_options, moteus_command); + + controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + return moteus_wrapper_ptr; } \ No newline at end of file From 1cfa3a58678a145b7ac59b62af4d7a80f190fa7e Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 15:06:58 +0200 Subject: [PATCH 109/137] Changed make_moteus_wrapper --- .../src/controllers/wrappers/MoteusWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 1d3c2f30..b040cf7d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -96,6 +96,6 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C moteus_command.velocity_limit = params.velocity_max_; controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); - std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); + std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); return moteus_wrapper_ptr; } \ No newline at end of file From 3893689081106573fbfdb0451908d996656c99f1 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 15:24:22 +0200 Subject: [PATCH 110/137] Fixed moteus wrapper error --- .../src/controllers/wrappers/MoteusWrapper.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index b040cf7d..755df81a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -93,7 +93,7 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C /* moteus command (it will be copied to wrapper) */ mjbots::moteus::PositionMode::Command moteus_command; moteus_command.maximum_torque = params.torque_max_; - moteus_command.velocity_limit = params.velocity_max_; + //moteus_command.velocity_limit = params.velocity_max_; // Creates error, check later controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); From 9c9ca20497080499e3aece5be4cd94773c1b63d6 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 16:02:07 +0200 Subject: [PATCH 111/137] Making tests for pi3hat_hardware_interface --- .../pi3hat_hardware_interface/CMakeLists.txt | 11 + .../bringup/launch/spawn_robot.launch.py | 68 ++++++ .../pi3hat_hardware_interface.hpp | 2 - .../pi3hat_hardware_interface/package.xml | 2 + .../src/pi3hat_hardware_interface.cpp | 26 +-- .../{ => urdf}/test_double_motor.urdf.xacro | 0 .../{ => urdf}/test_single_motor.urdf.xacro | 2 +- .../meldog_launch_tests/CMakeLists.txt | 31 +++ src/meldog_tests/meldog_launch_tests/LICENSE | 202 ++++++++++++++++++ .../launch/view_robot.launch.py | 40 ++++ .../meldog_launch_tests/package.xml | 20 ++ test.urdf | 0 12 files changed, 376 insertions(+), 28 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py rename src/meldog_hardware/pi3hat_hardware_interface/test/{ => urdf}/test_double_motor.urdf.xacro (100%) rename src/meldog_hardware/pi3hat_hardware_interface/test/{ => urdf}/test_single_motor.urdf.xacro (98%) create mode 100644 src/meldog_tests/meldog_launch_tests/CMakeLists.txt create mode 100644 src/meldog_tests/meldog_launch_tests/LICENSE create mode 100644 src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py create mode 100644 src/meldog_tests/meldog_launch_tests/package.xml create mode 100644 test.urdf diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index e9b1fdee..83abfb9b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -55,7 +55,13 @@ ament_target_dependencies( pluginlib_export_plugin_description_file(hardware_interface pi3hat_hardware_interface.xml) + # install +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + install( TARGETS ${PROJECT_NAME} DESTINATION lib @@ -86,6 +92,11 @@ install( DESTINATION test ) +install( + DIRECTORY test/urdf + DESTINATION share//${PROJECT_NAME}/ +) + # uncomment the following section in order to fill in # further dependencies manually. # find_package( REQUIRED) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py new file mode 100644 index 00000000..41ca0483 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py @@ -0,0 +1,68 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "package", + default_value="pi3hat_hardware_interface", + ) + ) + + declared_arguments.append( + DeclareLaunchArgument( + "urdf_file", + default_value="test_single_motor.urdf.xacro", + ) + ) + + + # Initialize Arguments + package_name = LaunchConfiguration("package") + urdf_file = LaunchConfiguration("urdf_file") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare(package_name), + "test", + "urdf", + urdf_file, + ] + ), + ] + ) + robot_description = {"robot_description": robot_description_content} + + # rviz_config_file = PathJoinSubstitution( + # [FindPackageShare("ros2_control_demo_description"), "rrbot/rviz", "rrbot.rviz"] + # ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + + + nodes = [ + robot_state_pub_node, + ] + + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 38a94286..5071d72e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -150,8 +150,6 @@ namespace pi3hat_hardware_interface Remember to change this function in source code */ WrapperType choose_wrapper_type(const std::string& type); - std::unique_ptr create_moteus_wrapper(const controller_interface::ControllerParameters& params); - /* Function for creating moteus wrappers (here u can add your own wrapper) */ void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/package.xml b/src/meldog_hardware/pi3hat_hardware_interface/package.xml index c5402c89..9b7802ca 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/package.xml +++ b/src/meldog_hardware/pi3hat_hardware_interface/package.xml @@ -18,6 +18,8 @@ ament_lint_auto ament_lint_common + ros2launch + ament_cmake diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 62ac8a0c..a370a5b5 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -765,7 +765,7 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& switch(type) { case Moteus: - wrapper_ptr = create_moteus_wrapper(params); + wrapper_ptr = make_moteus_wrapper(params); break; default: throw std::invalid_argument("Motor type doesn't exist!"); @@ -776,30 +776,6 @@ void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& controller_bridges_.push_back(std::move(bridge)); } -std::unique_ptr Pi3HatHardwareInterface::create_moteus_wrapper(const ControllerParameters& params) -{ - /* moteus options */ - using mjbots::moteus::Controller; - using controller_interface::MoteusWrapper; - Controller::Options moteus_options; - moteus_options.bus = params.bus_; - moteus_options.id = params.id_; - - /* moteus command format (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Format format; - format.feedforward_torque = mjbots::moteus::kFloat; - format.maximum_torque = mjbots::moteus::kFloat; - format.velocity_limit= mjbots::moteus::kFloat; - moteus_options.position_format = format; - - /* moteus command (it will be copied to wrapper) */ - mjbots::moteus::PositionMode::Command moteus_command; - moteus_command.maximum_torque = params.torque_max_; - moteus_command.velocity_limit = params.velocity_max_; - - return std::make_unique(moteus_options, moteus_command); -} - Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) { if(type == "moteus") diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_double_motor.urdf.xacro similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/test_double_motor.urdf.xacro rename to src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_double_motor.urdf.xacro diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro similarity index 98% rename from src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro rename to src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro index 86d36fc4..95fd1b54 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro @@ -92,7 +92,7 @@ 30.0 -30.0 10.0 - 1 + 1 diff --git a/src/meldog_tests/meldog_launch_tests/CMakeLists.txt b/src/meldog_tests/meldog_launch_tests/CMakeLists.txt new file mode 100644 index 00000000..1321a60a --- /dev/null +++ b/src/meldog_tests/meldog_launch_tests/CMakeLists.txt @@ -0,0 +1,31 @@ +cmake_minimum_required(VERSION 3.8) +project(meldog_launch_tests) + +if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") + add_compile_options(-Wall -Wextra -Wpedantic) +endif() + +# find dependencies +find_package(ament_cmake REQUIRED) +# uncomment the following section in order to fill in +# further dependencies manually. +# find_package( REQUIRED) + +if(BUILD_TESTING) + find_package(ament_lint_auto REQUIRED) + # the following line skips the linter which checks for copyrights + # comment the line when a copyright and license is added to all source files + set(ament_cmake_copyright_FOUND TRUE) + # the following line skips cpplint (only works in a git repo) + # comment the line when this package is in a git repo and when + # a copyright and license is added to all source files + set(ament_cmake_cpplint_FOUND TRUE) + ament_lint_auto_find_test_dependencies() +endif() + +install(DIRECTORY + launch + DESTINATION share/${PROJECT_NAME}/ +) + +ament_package() diff --git a/src/meldog_tests/meldog_launch_tests/LICENSE b/src/meldog_tests/meldog_launch_tests/LICENSE new file mode 100644 index 00000000..d6456956 --- /dev/null +++ b/src/meldog_tests/meldog_launch_tests/LICENSE @@ -0,0 +1,202 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. + + END OF TERMS AND CONDITIONS + + APPENDIX: How to apply the Apache License to your work. + + To apply the Apache License to your work, attach the following + boilerplate notice, with the fields enclosed by brackets "[]" + replaced with your own identifying information. (Don't include + the brackets!) The text should be enclosed in the appropriate + comment syntax for the file format. We also recommend that a + file or class name and description of purpose be included on the + same "printed page" as the copyright notice for easier + identification within third-party archives. + + Copyright [yyyy] [name of copyright owner] + + Licensed under the Apache License, Version 2.0 (the "License"); + you may not use this file except in compliance with the License. + You may obtain a copy of the License at + + http://www.apache.org/licenses/LICENSE-2.0 + + Unless required by applicable law or agreed to in writing, software + distributed under the License is distributed on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. + See the License for the specific language governing permissions and + limitations under the License. diff --git a/src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py b/src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py new file mode 100644 index 00000000..dc76b9ee --- /dev/null +++ b/src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py @@ -0,0 +1,40 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + declared_arguments.append( + DeclareLaunchArgument( + "gui", + default_value="true", + description="Start RViz2 automatically with this launch file.", + ) + ) + + # Initialize Arguments + gui = LaunchConfiguration("gui") + + rviz_node = Node( + package="rviz2", + executable="rviz2", + name="rviz2", + output="log", + # arguments=["-d", rviz_config_file], + condition=IfCondition(gui), + ) + + + + nodes = [ + rviz_node + ] + + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_tests/meldog_launch_tests/package.xml b/src/meldog_tests/meldog_launch_tests/package.xml new file mode 100644 index 00000000..3f892dbb --- /dev/null +++ b/src/meldog_tests/meldog_launch_tests/package.xml @@ -0,0 +1,20 @@ + + + + meldog_launch_tests + 0.0.0 + TODO: Package description + bartek + Apache-2.0 + + ament_cmake + + ament_lint_auto + ament_lint_common + + ros2launch + + + ament_cmake + + diff --git a/test.urdf b/test.urdf new file mode 100644 index 00000000..e69de29b From 02e9071ad63eedfc66ce08e5a90651b4e42ad810 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 16:04:34 +0200 Subject: [PATCH 112/137] Fixed Cmake --- src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index 83abfb9b..b28678a1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -58,7 +58,7 @@ pluginlib_export_plugin_description_file(hardware_interface pi3hat_hardware_inte # install install(DIRECTORY - launch + bringup/launch DESTINATION share/${PROJECT_NAME}/ ) From cf555ee0f3791579549c786e8048b75e0a2c8ddd Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 16:22:30 +0200 Subject: [PATCH 113/137] Changed launch file --- .../pi3hat_hardware_interface/CMakeLists.txt | 2 +- .../bringup/launch/spawn_robot.launch.py | 4 ++++ .../test/urdf/test_single_motor.urdf.xacro | 10 +++++----- 3 files changed, 10 insertions(+), 6 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index b28678a1..4d8d3ae1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -94,7 +94,7 @@ install( install( DIRECTORY test/urdf - DESTINATION share//${PROJECT_NAME}/ + DESTINATION share/${PROJECT_NAME}/ ) # uncomment the following section in order to fill in diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py index 41ca0483..bb45100e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py @@ -59,6 +59,10 @@ def generate_launch_description(): parameters=[robot_description], ) + joint_state_publisher = Node( + package = "joint_state_publisher", + executable = "joint_state_publisher" + ) nodes = [ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro index 95fd1b54..6ac0f76e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro @@ -30,11 +30,11 @@ - - - - - + + + + + From 14790b82de88790c284cc81a20ababd09ab1155e Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 16:26:58 +0200 Subject: [PATCH 114/137] Editing launchfiles --- src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt | 2 +- .../bringup/launch/spawn_robot.launch.py | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index 4d8d3ae1..f6ae4422 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -94,7 +94,7 @@ install( install( DIRECTORY test/urdf - DESTINATION share/${PROJECT_NAME}/ + DESTINATION share/${PROJECT_NAME}/urdf ) # uncomment the following section in order to fill in diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py index bb45100e..c3166cb1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py @@ -39,7 +39,6 @@ def generate_launch_description(): PathJoinSubstitution( [ FindPackageShare(package_name), - "test", "urdf", urdf_file, ] From 87b2c67d7665fa415df27ba9440133a7e77ce8cd Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 2 Aug 2024 16:27:55 +0200 Subject: [PATCH 115/137] Editing launchfiles --- .../bringup/launch/spawn_robot.launch.py | 1 + 1 file changed, 1 insertion(+) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py index c3166cb1..ceeb70cb 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/spawn_robot.launch.py @@ -66,6 +66,7 @@ def generate_launch_description(): nodes = [ robot_state_pub_node, + joint_state_publisher, ] return LaunchDescription(declared_arguments + nodes) \ No newline at end of file From 38066e0d946f668930e35cac06b8e5a8a2204947 Mon Sep 17 00:00:00 2001 From: Bartek Date: Sat, 3 Aug 2024 21:21:48 +0200 Subject: [PATCH 116/137] New changes for testing on pi3hat --- .../pi3hat_hardware_interface/CMakeLists.txt | 9 +- .../bringup/config/test_single_motor.yaml | 18 +++ .../launch/test_single_motor.launch.py | 102 +++++++++++++++ .../pi3hat_hardware_interface.hpp | 2 +- .../pi3hat_hardware_interface/package.xml | 7 ++ .../src/pi3hat_hardware_interface.cpp | 3 +- .../test/urdf/test_single_motor.urdf.xacro | 117 ++++++++---------- 7 files changed, 187 insertions(+), 71 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_single_motor.yaml create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index f6ae4422..9559bd88 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -62,6 +62,12 @@ install(DIRECTORY DESTINATION share/${PROJECT_NAME}/ ) +install(DIRECTORY + bringup/config + DESTINATION share/${PROJECT_NAME}/ + +) + install( TARGETS ${PROJECT_NAME} DESTINATION lib @@ -94,7 +100,7 @@ install( install( DIRECTORY test/urdf - DESTINATION share/${PROJECT_NAME}/urdf + DESTINATION share/${PROJECT_NAME} ) # uncomment the following section in order to fill in @@ -121,6 +127,7 @@ ament_export_include_directories( ament_export_libraries( ${PROJECT_NAME} ) + ament_export_dependencies( hardware_interface pluginlib diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_single_motor.yaml b/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_single_motor.yaml new file mode 100644 index 00000000..d3c5bc17 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_single_motor.yaml @@ -0,0 +1,18 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + + +forward_position_controller: + ros__parameters: + type: forward_command_controller/ForwardCommandController + joints: + - joint_1 + interface_name: position \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py new file mode 100644 index 00000000..8051fb54 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py @@ -0,0 +1,102 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit, OnExecutionComplete +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) + + # Initialize Arguments + robot_controller = LaunchConfiguration("robot_controller") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("pi3hat_hardware_interface"), + "urdf", + "test_single_motor.urdf.xacro", + ] + ) + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("pi3hat_hardware_interface"), + "config", + "test_single_motor.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[{'robot_description': robot_description}, + robot_controllers], + output="both", + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "-c", "/controller_manager", "--param-file", robot_controllers,], + ) + + # # Delay start of joint_state_broadcaster after `robot_controller` + # # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + delay_joint_state_broadcaster_after_control_node = RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=control_node, + on_completion=[joint_state_broadcaster_spawner], + ) + ) + + delay_forward_controller_after_control_node = RegisterEventHandler( + event_handler=OnExecutionComplete( + target_action=control_node, + on_completion=[robot_controller_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + robot_controller_spawner, + delay_forward_controller_after_control_node, + delay_joint_state_broadcaster_after_control_node, + # delay_joint_state_broadcaster_after_robot_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 5071d72e..fccab10f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -8,6 +8,7 @@ #include #include #include +#include #include "rclcpp/clock.hpp" #include "rclcpp/duration.hpp" @@ -155,7 +156,6 @@ namespace pi3hat_hardware_interface controller_interface::ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); - /* FUNCTION FOR CONTROLLERS */ void controllers_init(); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/package.xml b/src/meldog_hardware/pi3hat_hardware_interface/package.xml index 9b7802ca..fa8bed50 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/package.xml +++ b/src/meldog_hardware/pi3hat_hardware_interface/package.xml @@ -18,6 +18,13 @@ ament_lint_auto ament_lint_common + + controller_manager + forward_command_controller + joint_state_broadcaster + robot_state_publisher + ros2controlcli + xacro ros2launch diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index a370a5b5..4ca9b4d4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -781,7 +781,8 @@ Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_typ if(type == "moteus") { return WrapperType::Moteus; - }; + } + return WrapperType::Moteus; } ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const hardware_interface::ComponentInfo& joint_info) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro index 6ac0f76e..3cb24043 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro @@ -1,75 +1,56 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - + @@ -109,7 +90,7 @@ transmission_interface/SimpleTransmission - 2.0 + 16.0 0.0 From e663efc8cc288a385495444d16e2602a23a2bc7d Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 4 Aug 2024 18:36:16 +0200 Subject: [PATCH 117/137] Changed CMake file --- .../pi3hat_hardware_interface/CMakeLists.txt | 186 ++++++++++++------ 1 file changed, 121 insertions(+), 65 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index 9559bd88..b79689d2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -6,106 +6,166 @@ if(CMAKE_COMPILER_IS_GNUCXX OR CMAKE_CXX_COMPILER_ID MATCHES "Clang") endif() # find dependencies -find_package(ament_cmake REQUIRED) -find_package(pluginlib REQUIRED) -find_package(hardware_interface REQUIRED) -find_package(rclcpp REQUIRED) -find_package(rclcpp_lifecycle REQUIRED) -find_package(transmission_interface REQUIRED) -add_library(pi3hat_hardware_interface SHARED +set(THIS_PACKAGE_INCLUDE_DEPENDS + pluginlib + hardware_interface + rclcpp + rclcpp_lifecycle + transmission_interface + +) +find_package(ament_cmake REQUIRED) +foreach(Dependency IN ITEMS ${THIS_PACKAGE_INCLUDE_DEPENDS}) + find_package(${Dependency} REQUIRED) +endforeach() + +## Hardware Interface +add_library( + pi3hat_hardware_interface + SHARED src/pi3hat_hardware_interface.cpp ) +# add include directory +target_include_directories(pi3hat_hardware_interface + PUBLIC + "$" + "$") + +target_compile_features( + pi3hat_hardware_interface PUBLIC cxx_std_17) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(${PROJECT_NAME} PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") + -add_library(pi3hat SHARED + +## Pi3hat library by mjbots +add_library( + pi3hat + SHARED include/3rd_libs/pi3hat/pi3hat.cc ) +# add include directory +target_include_directories(pi3hat + PUBLIC + "$" + "$") + +target_compile_features( + pi3hat PUBLIC cxx_std_17) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(pi3hat PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") + + + +## Custom controller bridge library add_library(controllers SHARED src/controllers/ControllerBridge.cpp src/controllers/wrappers/ControllerWrapper.cpp src/controllers/wrappers/MoteusWrapper.cpp ) -add_library(imu_transform SHARED +# add include directory +target_include_directories(controllers + PUBLIC + "$" + "$") + +target_compile_features( + controllers PUBLIC cxx_std_17) + +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(controllers PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") + + +## Custom library for IMU transformations +add_library( + imu_transform + SHARED src/imu_transform/IMUTransform.cpp ) # add include directory -target_include_directories(pi3hat_hardware_interface - PRIVATE - include -) +target_include_directories(imu_transform + PUBLIC + "$" + "$") + +target_compile_features( + imu_transform PUBLIC cxx_std_17) -target_link_libraries(pi3hat_hardware_interface - pi3hat - controllers - imu_transform - bcm_host -) +# Causes the visibility macros to use dllexport rather than dllimport, +# which is appropriate when building the dll but not consuming it. +target_compile_definitions(imu_transform PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") -ament_target_dependencies( - pi3hat_hardware_interface - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - transmission_interface -) -pluginlib_export_plugin_description_file(hardware_interface pi3hat_hardware_interface.xml) +# target_include_directories( +# pi3hat_hardware_interface +# PRIVATE +# include +# ) -# install -install(DIRECTORY - bringup/launch - DESTINATION share/${PROJECT_NAME}/ +target_link_libraries( + pi3hat_hardware_interface + PUBLIC pi3hat + PUBLIC controllers + PUBLIC imu_transform + PUBLIC bcm_host ) -install(DIRECTORY - bringup/config - DESTINATION share/${PROJECT_NAME}/ - +ament_target_dependencies( + pi3hat_hardware_interface PUBLIC + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) -install( - TARGETS ${PROJECT_NAME} - DESTINATION lib -) -install( - TARGETS pi3hat - DESTINATION lib -) +## Export hardware plugins +pluginlib_export_plugin_description_file(hardware_interface pi3hat_hardware_interface.xml) -install( - TARGETS controllers - DESTINATION lib -) -install( - TARGETS imu_transform - DESTINATION lib +## Install +install(TARGETS + ${PROJECT_NAME} + pi3hat + controllers + imu_transform + + EXPORT export_${PROJECT_NAME} + ARCHIVE DESTINATION lib + LIBRARY DESTINATION lib + RUNTIME DESTINATION bin ) install( DIRECTORY include/ - DESTINATION include + DESTINATION include/${PROJECT_NAME} ) install( DIRECTORY test/ - DESTINATION test + DESTINATION test/${PROJECT_NAME} ) install( - DIRECTORY test/urdf + DIRECTORY urdf/ DESTINATION share/${PROJECT_NAME} ) +install(DIRECTORY + bringup/launch + DESTINATION share/${PROJECT_NAME}/ +) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +install(DIRECTORY + bringup/config + DESTINATION share/${PROJECT_NAME}/ +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) @@ -124,16 +184,12 @@ endif() ament_export_include_directories( include ) -ament_export_libraries( - ${PROJECT_NAME} -) +ament_export_targets( + export_${PROJECT_NAME} HAS_LIBRARY_TARGET +) ament_export_dependencies( - hardware_interface - pluginlib - rclcpp - rclcpp_lifecycle - transmission_interface + ${THIS_PACKAGE_INCLUDE_DEPENDS} ) ament_package() From a648bce3572b3bb247d02d07eeff81bcfaab5eaf Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 4 Aug 2024 18:36:53 +0200 Subject: [PATCH 118/137] Fixing includes --- .../include/actuator_wrappers/ActuatorWrapperBase.hpp | 2 +- .../include/actuator_wrappers/MoteusWrapper.hpp | 2 +- .../include/controllers/ControllerBridge.hpp | 2 +- .../include/controllers/wrappers/ControllerWrapper.hpp | 2 +- .../include/controllers/wrappers/MoteusWrapper.hpp | 2 +- .../include/imu_transform/IMUTransform.hpp | 2 +- .../pi3hat_hardware_interface.hpp | 8 ++++---- .../src/actuator_wrappers/MoteusWrapper.cpp | 3 ++- .../src/controllers/ControllerBridge.cpp | 3 ++- .../src/controllers/wrappers/ControllerWrapper.cpp | 3 ++- .../src/controllers/wrappers/MoteusWrapper.cpp | 3 ++- .../src/imu_transform/IMUTransform.cpp | 3 ++- .../src/pi3hat_hardware_interface.cpp | 3 ++- .../test/hardware/double_bridge_test.cpp | 7 ++++--- .../test/hardware/simple_test.cpp | 6 +++--- .../test/hardware/single_bridge_test.cpp | 6 +++--- .../{test => }/urdf/test_double_motor.urdf.xacro | 0 .../{test => }/urdf/test_single_motor.urdf.xacro | 0 18 files changed, 32 insertions(+), 25 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/{test => }/urdf/test_double_motor.urdf.xacro (100%) rename src/meldog_hardware/pi3hat_hardware_interface/{test => }/urdf/test_single_motor.urdf.xacro (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp index 4b5621a7..489ef54f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp @@ -1,7 +1,7 @@ #ifndef _MOTOR_WRAPPER_BASE_ #define _MOTOR_WRAPPER_BASE_ -#include "../3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/pi3hat.h" #include /* Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp index c97f855e..be24831a 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp @@ -2,7 +2,7 @@ #define _MOTEUS_ACTUATOR_WRAPPER_ -#include "../moteus/moteus.h" +#include "3rd_libs/moteus/moteus.h" #include "ActuatorWrapperBase.hpp" namespace actuator_wrappers diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index e3e08f00..52f42f0b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -1,7 +1,7 @@ #ifndef _CONTROLLER_BRIDGE_HPP_ #define _CONTROLLER_BRIDGE_HPP_ -#include "../3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/pi3hat.h" #include "wrappers/ControllerWrapper.hpp" #include "ControllerStructures.hpp" #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 1d00d1a0..f7750c15 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -2,7 +2,7 @@ #define _CONTROLLER_WRAPPER_HPP_ #include "../ControllerStructures.hpp" -#include "../../3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/pi3hat.h" namespace controller_interface { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 1ef87763..bb79f879 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -2,7 +2,7 @@ #define _MOTEUS_WRAPPER_HPP_ #include "ControllerWrapper.hpp" -#include "../../3rd_libs/moteus/moteus.h" +#include "3rd_libs/moteus/moteus.h" #include namespace controller_interface diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp index 6d055c73..4baf6c9c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp @@ -2,7 +2,7 @@ #define _IMU_ #include -#include "../3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/pi3hat.h" #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index fccab10f..3f7d4986 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -34,10 +34,10 @@ #include "transmission_interface/differential_transmission_loader.hpp" #include "transmission_interface/transmission_interface_exception.hpp" -#include "../controllers/Controllers.hpp" -#include "../imu_transform/IMUTransform.hpp" -#include "../3rd_libs/pi3hat/pi3hat.h" -#include "../3rd_libs/pi3hat/realtime.h" +#include "controllers/Controllers.hpp" +#include "imu_transform/IMUTransform.hpp" +#include "3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/realtime.h" #include "visibility_control.hpp" diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp index 6ec9a638..f5c14ab3 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp @@ -1,4 +1,5 @@ -#include "../../include/actuator_wrappers/MoteusWrapper.hpp" +#include "actuator_wrappers/MoteusWrapper.hpp" +//#include "../../include/actuator_wrappers/MoteusWrapper.hpp" /* Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index f5c615f4..678d7463 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -1,4 +1,5 @@ -#include "../../include/controllers/ControllerBridge.hpp" +#include "controllers/ControllerBridge.hpp" +//#include "../../include/controllers/ControllerBridge.hpp" using namespace controller_interface; using mjbots::pi3hat::CanFrame; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp index 05c9a19e..f9341694 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp @@ -1,4 +1,5 @@ -#include "../../../include/controllers/wrappers/ControllerWrapper.hpp" +#include "controllers/wrappers/ControllerWrapper.hpp" +//#include "../../../include/controllers/wrappers/ControllerWrapper.hpp" using namespace controller_interface; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 755df81a..fbf5dab8 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -1,4 +1,5 @@ -#include "../../../include/controllers/wrappers/MoteusWrapper.hpp" +#include "controllers/wrappers/MoteusWrapper.hpp" +//#include "../../../include/controllers/wrappers/MoteusWrapper.hpp" using namespace controller_interface; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp index cb3ec9a6..5947f02f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp @@ -1,4 +1,5 @@ -#include "../../include/imu_transform/IMUTransform.hpp" +#include "imu_transform/IMUTransform.hpp" +//#include "../../include/imu_transform/IMUTransform.hpp" using namespace IMU; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 4ca9b4d4..cb444369 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,4 +1,5 @@ -#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" +#include "pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" +//#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; using namespace controller_interface; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp index 38122462..abe5dc31 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp @@ -1,6 +1,7 @@ -#include "../../include/controllers/Controllers.hpp" -#include "../../include/3rd_libs/pi3hat/pi3hat.h" -#include "../../include/3rd_libs/pi3hat/realtime.h" +#include "controllers/Controllers.hpp" +#include "3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/realtime.h" +//#include "../../include/3rd_libs/pi3hat/realtime.h" #include #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp index cd92f5bf..10220b9f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp @@ -1,6 +1,6 @@ -#include "../../include/3rd_libs/pi3hat/pi3hat.h" -#include "../../include/3rd_libs/pi3hat/realtime.h" -#include "../../include/3rd_libs/moteus/moteus.h" +#include "3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/realtime.h" +#include "3rd_libs/moteus/moteus.h" static double GetNow() diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 996c2276..5f87937d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -1,9 +1,9 @@ -#include "../../include/controllers/Controllers.hpp" +#include "controllers/Controllers.hpp" -#include "../../include/3rd_libs/pi3hat/pi3hat.h" -#include "../../include/3rd_libs/pi3hat/realtime.h" +#include "3rd_libs/pi3hat/pi3hat.h" +#include "3rd_libs/pi3hat/realtime.h" #include #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_double_motor.urdf.xacro rename to src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/urdf/test_single_motor.urdf.xacro rename to src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro From 9787b4834fd450986a0b51df52f118f76f17586a Mon Sep 17 00:00:00 2001 From: Bartek Date: Sun, 4 Aug 2024 19:11:20 +0200 Subject: [PATCH 119/137] Changes for launch tests and pi3hat interface --- .../pi3hat_hardware_interface/pi3hat_hardware_interface.hpp | 4 ++++ .../pi3hat_hardware_interface/pi3hat_hardware_interface.xml | 4 +++- .../src/pi3hat_hardware_interface.cpp | 6 ++++++ .../launch/{view_robot.launch.py => rviz2.launch.py} | 0 4 files changed, 13 insertions(+), 1 deletion(-) rename src/meldog_tests/meldog_launch_tests/launch/{view_robot.launch.py => rviz2.launch.py} (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 3f7d4986..f9166835 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -81,6 +81,10 @@ namespace pi3hat_hardware_interface ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC hardware_interface::return_type write( const rclcpp::Time &time, const rclcpp::Duration &period) override; + + + ROS2_CONTROL_PI3HAT_HARDWARE_PUBLIC + ~Pi3HatHardwareInterface(); private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml b/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml index 5f7e9a01..64f9f919 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml +++ b/src/meldog_hardware/pi3hat_hardware_interface/pi3hat_hardware_interface.xml @@ -1,6 +1,8 @@ - + mjbots pi3hat interface by Bartlomiej Krajewski. Thanks to G-Levine for inspiration and open-source code diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index cb444369..28b631cc 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -862,6 +862,12 @@ void Pi3HatHardwareInterface::create_controller_joint_map() } } +Pi3HatHardwareInterface::~Pi3HatHardwareInterface() +{ + on_deactivate(rclcpp_lifecycle::State()); + on_cleanup(rclcpp_lifecycle::State()); +} + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( diff --git a/src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py b/src/meldog_tests/meldog_launch_tests/launch/rviz2.launch.py similarity index 100% rename from src/meldog_tests/meldog_launch_tests/launch/view_robot.launch.py rename to src/meldog_tests/meldog_launch_tests/launch/rviz2.launch.py From 4174ecd4f2660de68f11f0aad978685a2497f069 Mon Sep 17 00:00:00 2001 From: Bartek Date: Mon, 5 Aug 2024 20:04:48 +0200 Subject: [PATCH 120/137] Finished tests for pi3hat, all passed! --- .../pi3hat_hardware_interface/CMakeLists.txt | 12 +- .../bringup/config/test_double_motor.yaml | 19 ++ .../launch/test_double_motor.launch.py | 112 ++++++++++ .../launch/test_single_motor.launch.py | 40 ++-- .../include/controllers/ControllerBridge.hpp | 2 + .../src/controllers/ControllerBridge.cpp | 7 +- .../src/pi3hat_hardware_interface.cpp | 25 ++- .../urdf/test_double_motor.urdf.xacro | 206 +++++++++++------- .../urdf/test_single_motor.urdf.xacro | 34 +-- 9 files changed, 327 insertions(+), 130 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_double_motor.yaml create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_double_motor.launch.py diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index b79689d2..6c49e189 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -144,18 +144,18 @@ install(TARGETS ) install( - DIRECTORY include/ - DESTINATION include/${PROJECT_NAME} + DIRECTORY include + DESTINATION include/${PROJECT_NAME}/ ) install( - DIRECTORY test/ - DESTINATION test/${PROJECT_NAME} + DIRECTORY test + DESTINATION test/${PROJECT_NAME}/ ) install( - DIRECTORY urdf/ - DESTINATION share/${PROJECT_NAME} + DIRECTORY urdf + DESTINATION share/${PROJECT_NAME}/ ) install(DIRECTORY bringup/launch diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_double_motor.yaml b/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_double_motor.yaml new file mode 100644 index 00000000..93ee2c91 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/config/test_double_motor.yaml @@ -0,0 +1,19 @@ +controller_manager: + ros__parameters: + update_rate: 500 # Hz + + joint_state_broadcaster: + type: joint_state_broadcaster/JointStateBroadcaster + + forward_position_controller: + type: forward_command_controller/ForwardCommandController + + + +forward_position_controller: + ros__parameters: + type: forward_command_controller/ForwardCommandController + joints: + - joint_1 + - joint_2 + interface_name: position \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_double_motor.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_double_motor.launch.py new file mode 100644 index 00000000..3458fc80 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_double_motor.launch.py @@ -0,0 +1,112 @@ +from launch import LaunchDescription +from launch.actions import DeclareLaunchArgument, RegisterEventHandler +from launch.conditions import IfCondition +from launch.event_handlers import OnProcessExit, OnExecutionComplete +from launch.substitutions import Command, FindExecutable, LaunchConfiguration, PathJoinSubstitution + +from launch_ros.actions import Node +from launch_ros.substitutions import FindPackageShare + + +def generate_launch_description(): + # Declare arguments + declared_arguments = [] + + declared_arguments.append( + DeclareLaunchArgument( + "robot_controller", + default_value="forward_position_controller", + description="Robot controller to start.", + ) + ) + + # Initialize Arguments + robot_controller = LaunchConfiguration("robot_controller") + + # Get URDF via xacro + robot_description_content = Command( + [ + PathJoinSubstitution([FindExecutable(name="xacro")]), + " ", + PathJoinSubstitution( + [ + FindPackageShare("pi3hat_hardware_interface"), + "urdf", + "test_double_motor.urdf.xacro", + ] + ) + ] + ) + robot_description = {"robot_description": robot_description_content} + + robot_controllers = PathJoinSubstitution( + [ + FindPackageShare("pi3hat_hardware_interface"), + "config", + "test_double_motor.yaml", + ] + ) + + control_node = Node( + package="controller_manager", + executable="ros2_control_node", + parameters=[{'robot_description': robot_description}, + robot_controllers], + output="both", + remappings=[ + ("~/robot_description", "/robot_description"), + ], + ) + + robot_state_pub_node = Node( + package="robot_state_publisher", + executable="robot_state_publisher", + output="both", + parameters=[robot_description], + ) + + joint_state_broadcaster_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], + ) + + robot_controller_spawner = Node( + package="controller_manager", + executable="spawner", + arguments=[robot_controller, "--controller-manager", "/controller_manager", "--param-file", robot_controllers,], + ) + + # # Delay start of joint_state_broadcaster after `robot_controller` + # # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. + # delay_joint_state_broadcaster_after_control_node = RegisterEventHandler( + # event_handler=OnExecutionComplete( + # target_action=control_node, + # on_completion=[joint_state_broadcaster_spawner], + # ) + # ) + + # delay_forward_controller_after_control_node = RegisterEventHandler( + # event_handler=OnExecutionComplete( + # target_action=control_node, + # on_completion=[robot_controller_spawner], + # ) + # ) + + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], + ) + ) + + nodes = [ + control_node, + robot_state_pub_node, + robot_controller_spawner, + # delay_forward_controller_after_control_node, + # delay_joint_state_broadcaster_after_control_node, + delay_joint_state_broadcaster_after_robot_controller_spawner, + ] + + return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py index 8051fb54..0c50a323 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py +++ b/src/meldog_hardware/pi3hat_hardware_interface/bringup/launch/test_single_motor.launch.py @@ -53,6 +53,9 @@ def generate_launch_description(): parameters=[{'robot_description': robot_description}, robot_controllers], output="both", + remappings=[ + ("~/robot_description", "/robot_description"), + ], ) robot_state_pub_node = Node( @@ -65,28 +68,35 @@ def generate_launch_description(): joint_state_broadcaster_spawner = Node( package="controller_manager", executable="spawner", - arguments=["joint_state_broadcaster", "-c", "/controller_manager"], + arguments=["joint_state_broadcaster", "--controller-manager", "/controller_manager"], ) robot_controller_spawner = Node( package="controller_manager", executable="spawner", - arguments=[robot_controller, "-c", "/controller_manager", "--param-file", robot_controllers,], + arguments=[robot_controller, "--controller-manager", "/controller_manager", "--param-file", robot_controllers,], ) # # Delay start of joint_state_broadcaster after `robot_controller` # # TODO(anyone): This is a workaround for flaky tests. Remove when fixed. - delay_joint_state_broadcaster_after_control_node = RegisterEventHandler( - event_handler=OnExecutionComplete( - target_action=control_node, - on_completion=[joint_state_broadcaster_spawner], - ) - ) + # delay_joint_state_broadcaster_after_control_node = RegisterEventHandler( + # event_handler=OnExecutionComplete( + # target_action=control_node, + # on_completion=[joint_state_broadcaster_spawner], + # ) + # ) + + # delay_forward_controller_after_control_node = RegisterEventHandler( + # event_handler=OnExecutionComplete( + # target_action=control_node, + # on_completion=[robot_controller_spawner], + # ) + # ) - delay_forward_controller_after_control_node = RegisterEventHandler( - event_handler=OnExecutionComplete( - target_action=control_node, - on_completion=[robot_controller_spawner], + delay_joint_state_broadcaster_after_robot_controller_spawner = RegisterEventHandler( + event_handler=OnProcessExit( + target_action=robot_controller_spawner, + on_exit=[joint_state_broadcaster_spawner], ) ) @@ -94,9 +104,9 @@ def generate_launch_description(): control_node, robot_state_pub_node, robot_controller_spawner, - delay_forward_controller_after_control_node, - delay_joint_state_broadcaster_after_control_node, - # delay_joint_state_broadcaster_after_robot_controller_spawner, + # delay_forward_controller_after_control_node, + # delay_joint_state_broadcaster_after_control_node, + delay_joint_state_broadcaster_after_robot_controller_spawner, ] return LaunchDescription(declared_arguments + nodes) \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index 52f42f0b..ff986b5d 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -35,6 +35,8 @@ class ControllerBridge void initialize(CanFrame& tx_frame) const; void start_up(CanFrame& tx_frame, ControllerCommand& command) const; + ControllerParameters get_params(); + ~ControllerBridge() = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 678d7463..91d89d47 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -39,7 +39,7 @@ void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& comma void ControllerBridge::get_state(const CanFrame& rx_frame, ControllerState& state) const { wrapper_->rx_frame_to_state(rx_frame, state); - state.position_ = params_.direction_ * state.position_; + state.position_ = params_.direction_ * (state.position_ - params_.position_offset_); state.velocity_ = params_.direction_ * state.velocity_; state.torque_ = params_.direction_ * state.torque_; } @@ -59,4 +59,9 @@ void ControllerBridge::start_up(CanFrame& tx_frame, ControllerCommand& command) tx_frame.expect_reply = true; wrapper_->start_pos_to_tx_frame(tx_frame, command); +} + +ControllerParameters ControllerBridge::get_params() +{ + return params_; } \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 28b631cc..8e19ce64 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -122,8 +122,10 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r /* Initialize all motors/remove all flags */ controllers_init(); - pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000); + auto result = pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); + + RCLCPP_INFO(*logger_, "%d", result.rx_can_size); /* Create rx_frame.id -> joint map */ try @@ -304,7 +306,7 @@ std::vector Pi3HatHardwareInterface::expor } else { - RCLCPP_WARN(*logger_, "%s is wrong type of command interface, omitted!", command_interface.name); + RCLCPP_WARN(*logger_, "%s is wrong type of command interface, omitted!", command_interface.name.c_str()); } } } @@ -321,7 +323,7 @@ std::vector Pi3HatHardwareInterface::export_ { if(!(info_.joints[i].state_interfaces.size() > 0)) { - RCLCPP_WARN(*logger_, "Zero state interfaces for joint %s!", info_.joints[i].name); + RCLCPP_WARN(*logger_, "Zero state interfaces for joint %s!", info_.joints[i].name.c_str()); } for(const auto& state_interface: info_.joints[i].state_interfaces) { @@ -330,24 +332,24 @@ std::vector Pi3HatHardwareInterface::export_ state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_POSITION, &(joint_states_[i].position_))); } - if(state_interface.name == hardware_interface::HW_IF_VELOCITY) + else if(state_interface.name == hardware_interface::HW_IF_VELOCITY) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_VELOCITY, &(joint_states_[i].velocity_))); } - if(state_interface.name == hardware_interface::HW_IF_EFFORT) + else if(state_interface.name == hardware_interface::HW_IF_EFFORT) { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, hardware_interface::HW_IF_EFFORT, &(joint_states_[i].torque_))); } - if(state_interface.name == "temperature") + else if(state_interface.name == "temperature") { state_interfaces.emplace_back(hardware_interface::StateInterface( info_.joints[i].name, "temperature", &(joint_states_[i].temperature_))); } else { - RCLCPP_WARN(*logger_, "%s is wrong type of state interface, omitted!", state_interface.name); + RCLCPP_WARN(*logger_, "%s is wrong type of state interface, omitted!", state_interface.name.c_str()); } } } @@ -846,12 +848,13 @@ void Pi3HatHardwareInterface::create_controller_joint_map() for(int i = 0; i < joint_controller_number_; ++i) { int joint_id = i; - int controller_id = tx_can_frames_[i].id; + int controller_id = controller_bridges_[i].get_params().id_; for(int j = 0; j < joint_controller_number_; ++j) { - if(controller_id == rx_can_frames_[j].id) + RCLCPP_INFO(*logger_, "Joint: %d, Controller: %d, Frame id: %d, Frame bus: %d", joint_id, controller_id, rx_can_frames_[j].id, rx_can_frames_[j].bus); + if(controller_id == ((rx_can_frames_[j].id>> 8) & 0x7f)) { - std::pair controller_joint_pair(controller_id, joint_id); + std::pair controller_joint_pair(rx_can_frames_[j].id, joint_id); controller_joint_map_.emplace(controller_joint_pair); } } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro index 205fb8e8..a118db74 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro @@ -1,77 +1,123 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -90,10 +136,10 @@ 1 0.0 1.0 - 30.0 - -30.0 + 500.0 + -500.0 10.0 - 1 + 1.0 @@ -115,10 +161,10 @@ 1 0.0 1.0 - 30.0 - -30.0 + 500.0 + -500.0 10.0 - 1 + 1 @@ -133,14 +179,14 @@ transmission_interface/FourBarLinkageTransmission - 10 + 16.0 - 10 + 16.0 0.0 - 0.0 + 1.0 0.0 diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro index 3cb24043..eda6f5a0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro @@ -69,9 +69,9 @@ 1 0.0 - 1.0 - 30.0 - -30.0 + 0.0 + 500.0 + -500.0 10.0 1 @@ -80,10 +80,10 @@ - - - - + + + + @@ -96,16 +96,16 @@ - - - - - - - - - - + + + + + + + + + + From f7d58868d88d62b98b630662d6dc89fcad7d0954 Mon Sep 17 00:00:00 2001 From: Bartek Date: Tue, 6 Aug 2024 23:06:08 +0200 Subject: [PATCH 121/137] Cleaned up pi3hat_interface --- .../src/pi3hat_hardware_interface.cpp | 20 ++++++++----------- 1 file changed, 8 insertions(+), 12 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 8e19ce64..0ea840b0 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -113,19 +113,19 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa pi3hat_ = std::make_shared(config); mjbots::pi3hat::ConfigureRealtime(0); - RCLCPP_INFO(*logger_, "Hardware Interface successfully initialized!"); return hardware_interface::CallbackReturn::SUCCESS; } hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const rclcpp_lifecycle::State &previous_state) { - /* Initialize all motors/remove all flags */ - controllers_init(); - auto result = pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000000); - - RCLCPP_INFO(*logger_, "%d", result.rx_can_size); + /* Initialize all motors/remove all flags (5 times in 5 seconds) */ + for(int i = 0; i < 5; ++i) + { + controllers_init(); + auto result = pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); + } /* Create rx_frame.id -> joint map */ try @@ -141,7 +141,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r /* Get states with prepared controller -> joint map */ controllers_get_states(); - RCLCPP_INFO(*logger_, "Hardware Interface successfully configured!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -199,7 +198,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rc } RCLCPP_INFO(*logger_, "Motors reached starting position!"); - RCLCPP_INFO(*logger_, "Hardware Interface successfully activated!!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -257,7 +255,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const } RCLCPP_INFO(*logger_, "Motors reached ending position!"); - RCLCPP_INFO(*logger_, "Hardware Interface successfully deactivated!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -267,12 +264,11 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_cleanup(const rcl /* Deinitialize all motors/remove all flags */ controllers_init(); pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000); + ::usleep(1000000); /* Get states with prepared controller -> joint map */ controllers_get_states(); - RCLCPP_INFO(*logger_, "Hardware Interface successfully cleanuped!"); return hardware_interface::CallbackReturn::SUCCESS; } From 41617590bd850afe703ec4fcb8d6f08b3cef8b59 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 7 Aug 2024 19:18:12 +0200 Subject: [PATCH 122/137] Removed start_up, added make_query --- .../include/controllers/ControllerBridge.hpp | 2 +- .../wrappers/ControllerWrapper.hpp | 2 +- .../controllers/wrappers/MoteusWrapper.hpp | 5 +-- .../src/controllers/ControllerBridge.cpp | 17 ++++------ .../controllers/wrappers/MoteusWrapper.cpp | 33 +++++++------------ 5 files changed, 21 insertions(+), 38 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index ff986b5d..cb5986ea 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -31,9 +31,9 @@ class ControllerBridge void make_command(CanFrame& tx_frame, ControllerCommand& command) const; + void make_query(CanFrame& tx_frame) const; void get_state(const CanFrame& rx_frame, ControllerState& state) const; void initialize(CanFrame& tx_frame) const; - void start_up(CanFrame& tx_frame, ControllerCommand& command) const; ControllerParameters get_params(); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index f7750c15..3a106378 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -21,9 +21,9 @@ class ControllerWrapper ControllerWrapper& operator=(ControllerWrapper&& other) = default; virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; + virtual void query_to_tx_frame(CanFrame& tx_frame) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void init_to_tx_frame(CanFrame& tx_frame) = 0; - virtual void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual ~ControllerWrapper() = default; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index bb79f879..5fdd4e8c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -15,7 +15,6 @@ class MoteusWrapper final: public ControllerWrapper /* Const coefficients for easy radians - rotations transform */ constexpr static double rotation_to_radians_ = 2 * M_PI; constexpr static double radians_to_rotation_ = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ - constexpr static double startup_coefficient_ = 0.05; /* For slow start-up */ /* Command structure for moteus object*/ mjbots::moteus::PositionMode::Command position_command_; @@ -28,11 +27,9 @@ class MoteusWrapper final: public ControllerWrapper const mjbots::moteus::Controller::Options& options, const mjbots::moteus::PositionMode::Command& command); void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; + void query_to_tx_frame(CanFrame& tx_frame) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; - void start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) override; - - // ~MoteusWrapper() override = default; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 91d89d47..2a24cbc2 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -36,6 +36,12 @@ void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& comma } +void ControllerBridge::make_query(CanFrame& tx_frame) const +{ + tx_frame.expect_reply = true; + wrapper_->query_to_tx_frame(tx_frame); +} + void ControllerBridge::get_state(const CanFrame& rx_frame, ControllerState& state) const { wrapper_->rx_frame_to_state(rx_frame, state); @@ -50,17 +56,6 @@ void ControllerBridge::initialize(CanFrame& tx_frame) const wrapper_->init_to_tx_frame(tx_frame); } -void ControllerBridge::start_up(CanFrame& tx_frame, ControllerCommand& command) const -{ - command.position_ = params_.direction_* std::clamp(command.position_, - params_.position_min_, params_.position_max_) + params_.position_offset_; - command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); - command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); - - tx_frame.expect_reply = true; - wrapper_->start_pos_to_tx_frame(tx_frame, command); -} - ControllerParameters ControllerBridge::get_params() { return params_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index fbf5dab8..1237bd3f 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -29,6 +29,18 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } +void MoteusWrapper::query_to_tx_frame(CanFrame& tx_frame) +{ + /* Create CANFD frame*/ + mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeQuery(); + + /* Copy data from CANFD frame to CAN frame */ + tx_frame.id = can_fd_frame.arbitration_id; + tx_frame.bus = can_fd_frame.bus; + tx_frame.size = can_fd_frame.size; + std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); +} + void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { /* Parse data from RX CAN frame to Result object */ @@ -54,27 +66,6 @@ void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } -void MoteusWrapper::start_pos_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) -{ - /* Change command values for start-up (crate seperate command struct) */ - mjbots::moteus::PositionMode::Command start_command; - - start_command.position = command.position_ * radians_to_rotation_; - start_command.velocity = command.velocity_ * radians_to_rotation_; - start_command.feedforward_torque = command.torque_; - start_command.maximum_torque = startup_coefficient_ * position_command_.maximum_torque; - start_command.velocity_limit = startup_coefficient_ * position_command_.velocity_limit * radians_to_rotation_; - - /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(start_command); - - /* Copy data from CANFD frame to CAN frame */ - tx_frame.id = can_fd_frame.arbitration_id; - tx_frame.bus = can_fd_frame.bus; - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); -} - std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters& params) { /* moteus options */ From 495d3bfb8da2047912274f4b83e3954d4e588d56 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 7 Aug 2024 19:23:11 +0200 Subject: [PATCH 123/137] Added 0 initialization to controller structures --- .../controllers/ControllerStructures.hpp | 28 +++++++++---------- 1 file changed, 14 insertions(+), 14 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp index 78ff7969..c0efa1bd 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp @@ -7,31 +7,31 @@ namespace controller_interface /* Structure for basic actuator command */ struct ControllerCommand { - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ + double position_ = 0; /* [radians] */ + double velocity_ = 0; /* [radians/s] */ + double torque_ = 0; /* [Nm] */ }; /* Structure for basic actuator state */ struct ControllerState { - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ - double temperature_; /* [Celcius] */ + double position_ = 0; /* [radians] */ + double velocity_ = 0; /* [radians/s] */ + double torque_ = 0; /* [Nm] */ + double temperature_ = 0; /* [Celcius] */ bool fault = false; }; struct ControllerParameters { - double position_max_; - double position_min_; - double position_offset_; - double velocity_max_; - double torque_max_; + double position_max_ = 0; + double position_min_ = 0; + double position_offset_ = 0; + double velocity_max_ = 0; + double torque_max_ = 0; int direction_ = 1; - int id_; /* Usage in your bridge (check moteus bridge)*/ - int bus_; /* Usage in your bridge (check moteus bridge)*/ + int id_ = 0; /* Usage in your bridge (check moteus bridge)*/ + int bus_ = 0; /* Usage in your bridge (check moteus bridge)*/ }; } From 92d9d0c68f562c77d1e51cef090ecc0345aa2d51 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 7 Aug 2024 19:23:46 +0200 Subject: [PATCH 124/137] Changed initialization for motors and getting ids from motors --- .../pi3hat_hardware_interface.hpp | 7 +- .../src/pi3hat_hardware_interface.cpp | 142 +++++++++++++----- 2 files changed, 108 insertions(+), 41 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index f9166835..e1e5c727 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -128,9 +128,6 @@ namespace pi3hat_hardware_interface std::vector controller_states_; std::vector controller_commands_; - /* Controller start positions after initialization */ - std::vector controller_start_positions_; - /* For transmission interface */ std::vector controller_transmission_passthrough_; @@ -163,10 +160,10 @@ namespace pi3hat_hardware_interface /* FUNCTION FOR CONTROLLERS */ void controllers_init(); - void controllers_start_up(); - void controllers_make_commands(); + void controllers_make_queries(); + void controllers_get_states(); void create_controller_joint_map(); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 0ea840b0..f35c2992 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -38,7 +38,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa params = get_controller_parameters(joint); std::string type_string = joint.parameters.at("controller_type"); - controller_start_positions_.push_back(std::stod(joint.parameters.at("motor_position_start"))); type = choose_wrapper_type(type_string); } catch(const std::exception& e) @@ -119,13 +118,27 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const rclcpp_lifecycle::State &previous_state) { - /* Initialize all motors/remove all flags (5 times in 5 seconds) */ - for(int i = 0; i < 5; ++i) + /* Initialize all motors/remove all flags and make query for state */ + + controllers_init(); + auto result = pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); + + /* Get all rx_frames ids (be sure there are no duplicates) */ + + std::vector rx_ids; + rx_ids.resize(joint_controller_number_); + do { - controllers_init(); - auto result = pi3hat_->Cycle(pi3hat_input_); - ::usleep(1000000); - } + controllers_make_queries(); + result = pi3hat_->Cycle(pi3hat_input_); + ::usleep(100000); + for(int i = 0; i < joint_controller_number_; ++i) + { + rx_ids[i] = rx_can_frames_[i].id; + } + } + while(std::adjacent_find(rx_ids.begin(), rx_ids.end()) != rx_ids.end()); /* Create rx_frame.id -> joint map */ try @@ -149,7 +162,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rc /* Set all commands, states and transmission passthrough to start state */ for(int i = 0; i < joint_controller_number_; ++i) { - controller_commands_[i].position_ = controller_start_positions_[i]; + controller_commands_[i].position_ = 0; controller_commands_[i].velocity_ = 0; controller_commands_[i].torque_ = 0; @@ -180,24 +193,55 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rc } - /* Send start command to motors */ + /* Make slow start from actual motor position to 0.0 for 10 seconds + (offset included in controller bridges) */ RCLCPP_INFO(*logger_, "Motors reaching starting position!"); - double error_sum = NaN; - while(error_sum > 0.01) + double max_time = 10; /* [s] */ + double local_time = 0; /* [s] */ + int sleep_time = 5000; /* [ms] */ + controllers_get_states(); + std::vector slopes; + std::vector interceptes; + slopes.resize(joint_controller_number_); + interceptes.resize(joint_controller_number_); + + for(int i = 0; i < joint_controller_number_; ++i) { - error_sum = 0; - controllers_start_up(); + slopes[i] = -controller_states_[i].position_ / max_time; + interceptes[i] = controller_states_[i].position_; + } + + + double error_norm = NaN; + + while(local_time < max_time && error_norm > 0.01) + { + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_commands_[i].position_ = slopes[i] * local_time + interceptes[i]; + } + error_norm = 0; + controllers_make_commands(); pi3hat_->Cycle(pi3hat_input_); - ::usleep(5000); + ::usleep(sleep_time); controllers_get_states(); for(int i = 0; i < joint_controller_number_; ++i) { - error_sum += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); + error_norm += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); } - } - RCLCPP_INFO(*logger_, "Motors reached starting position!"); + local_time += ((double)sleep_time) / 1000000.0; + + } + if(error_norm > 0.1) + { + RCLCPP_WARN(*logger_, "Motors didn't reached starting position!"); + } + else + { + RCLCPP_INFO(*logger_, "Motors reached starting position!"); + } return hardware_interface::CallbackReturn::SUCCESS; } @@ -206,7 +250,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const /* Set all commands, states and transmission passthrough to end state */ for(int i = 0; i < joint_controller_number_; ++i) { - controller_commands_[i].position_ = controller_start_positions_[i]; + controller_commands_[i].position_ = 0; controller_commands_[i].velocity_ = 0; controller_commands_[i].torque_ = 0; @@ -237,24 +281,53 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const } - /* Send end commands to motors */ - RCLCPP_INFO(*logger_, "Motors reaching ending position!"); - double error_sum = NaN; - - while(error_sum > 0.01) + /* Make slow start from actual motor position to 0.0 for 10 seconds + (offset included in controller bridges) */ + RCLCPP_INFO(*logger_, "Motors reaching starting position!"); + + double max_time = 10; /* [s] */ + double local_time = 0; /* [s] */ + int sleep_time = 5000; /* [ms] */ + controllers_get_states(); + std::vector slopes; + std::vector interceptes; + slopes.resize(joint_controller_number_); + interceptes.resize(joint_controller_number_); + + for(int i = 0; i < joint_controller_number_; ++i) { - error_sum = 0; - controllers_start_up(); + slopes[i] = -controller_states_[i].position_ / max_time; + interceptes[i] = controller_states_[i].position_; + } + + + double error_norm = NaN; + + while(local_time < max_time && error_norm > 0.01) + { + for(int i = 0; i < joint_controller_number_; ++i) + { + controller_commands_[i].position_ = slopes[i] * local_time + interceptes[i]; + } + error_norm = 0; + controllers_make_commands(); pi3hat_->Cycle(pi3hat_input_); - ::usleep(5000); + ::usleep(sleep_time); controllers_get_states(); for(int i = 0; i < joint_controller_number_; ++i) { - error_sum += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); + error_norm += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); } + local_time += ((double)sleep_time) / 1000000.0; + } + if(error_norm > 0.1) + { + RCLCPP_WARN(*logger_, "Motors didn't reached starting position!"); + } + else + { + RCLCPP_INFO(*logger_, "Motors reached starting position!"); } - - RCLCPP_INFO(*logger_, "Motors reached ending position!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -266,9 +339,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_cleanup(const rcl pi3hat_->Cycle(pi3hat_input_); ::usleep(1000000); - /* Get states with prepared controller -> joint map */ - controllers_get_states(); - return hardware_interface::CallbackReturn::SUCCESS; } @@ -814,19 +884,19 @@ void Pi3HatHardwareInterface::controllers_init() } } -void Pi3HatHardwareInterface::controllers_start_up() +void Pi3HatHardwareInterface::controllers_make_commands() { for(int i = 0; i < joint_controller_number_; ++i) { - controller_bridges_[i].start_up(tx_can_frames_[i],controller_commands_[i]); + controller_bridges_[i].make_command(tx_can_frames_[i], controller_commands_[i]); } } -void Pi3HatHardwareInterface::controllers_make_commands() +void Pi3HatHardwareInterface::controllers_make_queries() { for(int i = 0; i < joint_controller_number_; ++i) { - controller_bridges_[i].make_command(tx_can_frames_[i], controller_commands_[i]); + controller_bridges_[i].make_query(tx_can_frames_[i]); } } From a154542be54b65aee58d70cc5a2279de3835ffc7 Mon Sep 17 00:00:00 2001 From: Bartek Date: Wed, 7 Aug 2024 19:24:03 +0200 Subject: [PATCH 125/137] Updated tests --- .../test/hardware/double_bridge_test.cpp | 2 +- .../test/hardware/single_bridge_test.cpp | 2 +- .../urdf/test_double_motor.urdf.xacro | 6 ++---- .../urdf/test_single_motor.urdf.xacro | 1 - 4 files changed, 4 insertions(+), 7 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp index abe5dc31..9063c327 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp @@ -131,7 +131,7 @@ int main(int argc, char** argv) for(int i = 0; i < controllers.size(); ++i) { controller_commands[i].position_ = 0; - controllers[i].start_up(tx_frame[i], controller_commands[i]); + controllers[i].make_command(tx_frame[i], controller_commands[i]); } while(std::abs(controller_states[0].position_) > 0.1 && std::abs(controller_states[1].position_) > 0.1) { diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp index 5f87937d..aaeb2ec6 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp @@ -96,7 +96,7 @@ int main(int argc, char** argv) controller_state.position_ = NaN; while(std::abs(controller_state.position_) > 0.1) { - controller.start_up(tx_frame, controller_command); + controller.make_command(tx_frame, controller_command); pi3hat_output = pi3hat.Cycle(input); ::usleep(2000); controller.get_state(rx_frame, controller_state); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro index a118db74..9fbfb8e8 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro @@ -134,8 +134,7 @@ 1 - 0.0 - 1.0 + 0.0 500.0 -500.0 10.0 @@ -159,8 +158,7 @@ 1 - 0.0 - 1.0 + 0.0 500.0 -500.0 10.0 diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro index eda6f5a0..e9b20284 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro @@ -68,7 +68,6 @@ 1 - 0.0 0.0 500.0 -500.0 From 49493f0eae3b35d2a404e78ebebb4aa2bb7d5bda Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 8 Aug 2024 23:04:41 +0200 Subject: [PATCH 126/137] Cleaned CMakeLists --- .../pi3hat_hardware_interface/CMakeLists.txt | 49 ++++--------------- 1 file changed, 10 insertions(+), 39 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt index 6c49e189..6329e67e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt +++ b/src/meldog_hardware/pi3hat_hardware_interface/CMakeLists.txt @@ -40,10 +40,9 @@ target_compile_features( target_compile_definitions(${PROJECT_NAME} PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") - ## Pi3hat library by mjbots add_library( - pi3hat + pi3hat SHARED include/3rd_libs/pi3hat/pi3hat.cc ) @@ -51,17 +50,11 @@ add_library( # add include directory target_include_directories(pi3hat PUBLIC - "$" - "$") + "$") target_compile_features( pi3hat PUBLIC cxx_std_17) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(pi3hat PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") - - ## Custom controller bridge library add_library(controllers SHARED @@ -73,16 +66,11 @@ add_library(controllers SHARED # add include directory target_include_directories(controllers PUBLIC - "$" - "$") + "$") target_compile_features( controllers PUBLIC cxx_std_17) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(controllers PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") - ## Custom library for IMU transformations add_library( @@ -94,23 +82,11 @@ add_library( # add include directory target_include_directories(imu_transform PUBLIC - "$" - "$") + "$") target_compile_features( imu_transform PUBLIC cxx_std_17) -# Causes the visibility macros to use dllexport rather than dllimport, -# which is appropriate when building the dll but not consuming it. -target_compile_definitions(imu_transform PRIVATE "PI3HAT_HARDWARE_INTERFACE_BUILDING_DLL") - - - -# target_include_directories( -# pi3hat_hardware_interface -# PRIVATE -# include -# ) target_link_libraries( pi3hat_hardware_interface @@ -136,7 +112,7 @@ install(TARGETS pi3hat controllers imu_transform - + EXPORT export_${PROJECT_NAME} ARCHIVE DESTINATION lib LIBRARY DESTINATION lib @@ -144,27 +120,22 @@ install(TARGETS ) install( - DIRECTORY include - DESTINATION include/${PROJECT_NAME}/ -) - -install( - DIRECTORY test - DESTINATION test/${PROJECT_NAME}/ + DIRECTORY include/${PROJECT_NAME}/ + DESTINATION include/${PROJECT_NAME} ) install( DIRECTORY urdf - DESTINATION share/${PROJECT_NAME}/ + DESTINATION share/${PROJECT_NAME} ) install(DIRECTORY bringup/launch - DESTINATION share/${PROJECT_NAME}/ + DESTINATION share/${PROJECT_NAME} ) install(DIRECTORY bringup/config - DESTINATION share/${PROJECT_NAME}/ + DESTINATION share/${PROJECT_NAME} ) if(BUILD_TESTING) From 7c80bfb1e96e01661ae2a4b25372f13436603ad9 Mon Sep 17 00:00:00 2001 From: Bartek Date: Thu, 8 Aug 2024 23:05:47 +0200 Subject: [PATCH 127/137] Renamed tests for controller bridges --- .../test/{hardware => controllers}/double_bridge_test.cpp | 0 .../test/{hardware => controllers}/simple_test.cpp | 0 .../test/{hardware => controllers}/single_bridge_test.cpp | 0 3 files changed, 0 insertions(+), 0 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/test/{hardware => controllers}/double_bridge_test.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/test/{hardware => controllers}/simple_test.cpp (100%) rename src/meldog_hardware/pi3hat_hardware_interface/test/{hardware => controllers}/single_bridge_test.cpp (100%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/double_bridge_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/hardware/double_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/double_bridge_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/simple_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/hardware/simple_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/simple_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/single_bridge_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/hardware/single_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/single_bridge_test.cpp From 02af7e2605ffd1564fc68daa399bf4343db70aaf Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 14:57:57 +0200 Subject: [PATCH 128/137] Added function for controller bridge --- .../include/controllers/ControllerBridge.hpp | 11 +++++++++++ .../controllers/wrappers/ControllerWrapper.hpp | 1 + .../include/controllers/wrappers/MoteusWrapper.hpp | 1 + .../src/controllers/ControllerBridge.cpp | 5 +++++ .../src/controllers/wrappers/MoteusWrapper.cpp | 7 ++++++- 5 files changed, 24 insertions(+), 1 deletion(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index cb5986ea..b00a5e29 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -30,11 +30,22 @@ class ControllerBridge ControllerBridge& operator=(ControllerBridge&& other_controller); + /* Transform controller command to data in TX CAN frame */ void make_command(CanFrame& tx_frame, ControllerCommand& command) const; + + /* Transform query to data in TX CAN frame */ void make_query(CanFrame& tx_frame) const; + + /* Transform RX CAN frame to controller state */ void get_state(const CanFrame& rx_frame, ControllerState& state) const; + + /* Initialize controller */ void initialize(CanFrame& tx_frame) const; + /* Get id from RX CAN frame (sometimes raw id from rx frame needs to be transformed)*/ + int get_id(const CanFrame& rx_frame); + + /* Get parameters that were set by user */ ControllerParameters get_params(); ~ControllerBridge() = default; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index 3a106378..cb449191 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -24,6 +24,7 @@ class ControllerWrapper virtual void query_to_tx_frame(CanFrame& tx_frame) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; virtual void init_to_tx_frame(CanFrame& tx_frame) = 0; + virtual int get_id_from_rx_frame(const CanFrame& rx_frame) = 0; virtual ~ControllerWrapper() = default; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index 5fdd4e8c..b0979897 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -30,6 +30,7 @@ class MoteusWrapper final: public ControllerWrapper void query_to_tx_frame(CanFrame& tx_frame) override; void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) override; void init_to_tx_frame(CanFrame& tx_frame) override; + int get_id_from_rx_frame(const CanFrame& rx_frame) override; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 2a24cbc2..f26cdd84 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -56,6 +56,11 @@ void ControllerBridge::initialize(CanFrame& tx_frame) const wrapper_->init_to_tx_frame(tx_frame); } +int ControllerBridge::get_id(const CanFrame& rx_frame) +{ + return wrapper_->get_id_from_rx_frame(rx_frame); +} + ControllerParameters ControllerBridge::get_params() { return params_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 1237bd3f..8b69cc95 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -66,6 +66,11 @@ void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); } +int MoteusWrapper::get_id_from_rx_frame(const CanFrame& rx_frame) +{ + return ((rx_frame.id>> 8) & 0x7f); +} + std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters& params) { /* moteus options */ @@ -90,4 +95,4 @@ std::unique_ptr controller_interface::make_moteus_wrapper(const C controller_interface::MoteusWrapper moteus_wrapper(moteus_options, moteus_command); std::unique_ptr moteus_wrapper_ptr = std::make_unique(moteus_wrapper); return moteus_wrapper_ptr; -} \ No newline at end of file +} From 2d770eeb9b748f648cc97cecbc2437804f50462c Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 14:58:32 +0200 Subject: [PATCH 129/137] Changed start for motors and added params for can buses --- .../pi3hat_hardware_interface.hpp | 5 + .../src/pi3hat_hardware_interface.cpp | 221 +++++++----------- .../urdf/test_double_motor.urdf.xacro | 27 ++- .../urdf/test_single_motor.urdf.xacro | 24 +- 4 files changed, 142 insertions(+), 135 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index e1e5c727..42c318d4 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -207,6 +207,11 @@ namespace pi3hat_hardware_interface /* FUNCTIONS FOR INITIALIZING PI3HAT/CAN INTERFACE */ + + + /* UTILITY FUNCTIONS */ + + bool string_to_bool(const std::string& str); }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index f35c2992..d0142edb 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -68,16 +68,13 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa return hardware_interface::CallbackReturn::ERROR; } - /* Standard CAN config */ - mjbots::pi3hat::Pi3Hat::CanConfiguration can_config; - - /* Configure the Pi3Hat for 1000hz IMU sampling */ + /* Configure the IMU in Pi3hat */ mjbots::pi3hat::Pi3Hat::Configuration config; - config.attitude_rate_hz = 1000; /* Set the mounting orientation of the IMU */ try { + config.attitude_rate_hz = std::stoi(info_.hardware_parameters.at("imu_sampling_rate")); config.mounting_deg.yaw = std::stod(info_.hardware_parameters.at("imu_mounting_deg.yaw")); config.mounting_deg.pitch = std::stod(info_.hardware_parameters.at("imu_mounting_deg.pitch")); config.mounting_deg.roll = std::stod(info_.hardware_parameters.at("imu_mounting_deg.roll")); @@ -89,6 +86,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa } /* Initialize the Pi3Hat input */ + pi3hat_input_ = mjbots::pi3hat::Pi3Hat::Input(); pi3hat_input_.request_attitude = true; pi3hat_input_.wait_for_attitude = true; @@ -102,11 +100,28 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa mjbots::pi3hat::Span tx_can_frames_span_(&tx_can_frames_[0], joint_controller_number_); pi3hat_input_.tx_can = tx_can_frames_span_; - config.can[0] = can_config; - config.can[1] = can_config; - config.can[2] = can_config; - config.can[3] = can_config; - config.can[4] = can_config; + /* Configure each CAN bus */ + mjbots::pi3hat::Pi3Hat::CanConfiguration can_config; + + std::string fdcan_frame = "fdcan_frame"; + std::string auto_retransmission = "automatic_retransmission"; + std::string bitrate_switch = "bitrate_switch"; + + for(int i = 0; i < 5; ++i) + { + std::string can_channel = "can_" + std::to_string(i + 1) + "_"; + try + { + config.can[i].fdcan_frame = string_to_bool(info_.hardware_parameters.at(can_channel + fdcan_frame)); + config.can[i].automatic_retransmission = string_to_bool(info_.hardware_parameters.at(can_channel + auto_retransmission)); + config.can[i].bitrate_switch = string_to_bool(info_.hardware_parameters.at(can_channel + bitrate_switch)); + } + catch(const std::exception& e) + { + RCLCPP_FATAL(*logger_, "Error reading CAN %d bus parameters!", i + 1); + return hardware_interface::CallbackReturn::ERROR; + } + } /* Initialize the Pi3Hat and realtime options */ pi3hat_ = std::make_shared(config); @@ -160,88 +175,49 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rclcpp_lifecycle::State &previous_state) { /* Set all commands, states and transmission passthrough to start state */ - for(int i = 0; i < joint_controller_number_; ++i) - { - controller_commands_[i].position_ = 0; - controller_commands_[i].velocity_ = 0; - controller_commands_[i].torque_ = 0; - - controller_states_[i].position_ = 0; - controller_states_[i].velocity_ = 0; - controller_states_[i].torque_ = 0; - controller_states_[i].fault = 0; - controller_states_[i].temperature_ = 0; - - controller_transmission_passthrough_[i].position_ = 0; - controller_transmission_passthrough_[i].velocity_ = 0; - controller_transmission_passthrough_[i].torque_ = 0; - - - joint_commands_[i].position_ = 0; - joint_commands_[i].velocity_ = 0; - joint_commands_[i].torque_ = 0; - - joint_states_[i].position_ = 0; - joint_states_[i].velocity_ = 0; - joint_states_[i].torque_ = 0; - joint_states_[i].fault = 0; - joint_states_[i].temperature_ = 0; - - joint_transmission_passthrough_[i].position_ = 0; - joint_transmission_passthrough_[i].velocity_ = 0; - joint_transmission_passthrough_[i].torque_ = 0; - } + // for(int i = 0; i < joint_controller_number_; ++i) + // { + // controller_commands_[i].position_ = 0; + // controller_commands_[i].velocity_ = 0; + // controller_commands_[i].torque_ = 0; + + // controller_states_[i].position_ = 0; + // controller_states_[i].velocity_ = 0; + // controller_states_[i].torque_ = 0; + // controller_states_[i].fault = 0; + // controller_states_[i].temperature_ = 0; + + // controller_transmission_passthrough_[i].position_ = 0; + // controller_transmission_passthrough_[i].velocity_ = 0; + // controller_transmission_passthrough_[i].torque_ = 0; + + + // joint_commands_[i].position_ = 0; + // joint_commands_[i].velocity_ = 0; + // joint_commands_[i].torque_ = 0; + + // joint_states_[i].position_ = 0; + // joint_states_[i].velocity_ = 0; + // joint_states_[i].torque_ = 0; + // joint_states_[i].fault = 0; + // joint_states_[i].temperature_ = 0; + + // joint_transmission_passthrough_[i].position_ = 0; + // joint_transmission_passthrough_[i].velocity_ = 0; + // joint_transmission_passthrough_[i].torque_ = 0; + // } - /* Make slow start from actual motor position to 0.0 for 10 seconds - (offset included in controller bridges) */ + /* Make start from actual motor position to 0.0 (offset included in controller bridges) */ RCLCPP_INFO(*logger_, "Motors reaching starting position!"); - double max_time = 10; /* [s] */ - double local_time = 0; /* [s] */ - int sleep_time = 5000; /* [ms] */ + controllers_make_commands(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); controllers_get_states(); - std::vector slopes; - std::vector interceptes; - slopes.resize(joint_controller_number_); - interceptes.resize(joint_controller_number_); - - for(int i = 0; i < joint_controller_number_; ++i) - { - slopes[i] = -controller_states_[i].position_ / max_time; - interceptes[i] = controller_states_[i].position_; - } - - - double error_norm = NaN; - - while(local_time < max_time && error_norm > 0.01) - { - for(int i = 0; i < joint_controller_number_; ++i) - { - controller_commands_[i].position_ = slopes[i] * local_time + interceptes[i]; - } - error_norm = 0; - controllers_make_commands(); - pi3hat_->Cycle(pi3hat_input_); - ::usleep(sleep_time); - controllers_get_states(); - for(int i = 0; i < joint_controller_number_; ++i) - { - error_norm += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); - } - - local_time += ((double)sleep_time) / 1000000.0; + + RCLCPP_INFO(*logger_, "Motors reached starting position!"); - } - if(error_norm > 0.1) - { - RCLCPP_WARN(*logger_, "Motors didn't reached starting position!"); - } - else - { - RCLCPP_INFO(*logger_, "Motors reached starting position!"); - } return hardware_interface::CallbackReturn::SUCCESS; } @@ -285,49 +261,12 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const (offset included in controller bridges) */ RCLCPP_INFO(*logger_, "Motors reaching starting position!"); - double max_time = 10; /* [s] */ - double local_time = 0; /* [s] */ - int sleep_time = 5000; /* [ms] */ + controllers_make_commands(); + pi3hat_->Cycle(pi3hat_input_); + ::usleep(1000000); controllers_get_states(); - std::vector slopes; - std::vector interceptes; - slopes.resize(joint_controller_number_); - interceptes.resize(joint_controller_number_); - - for(int i = 0; i < joint_controller_number_; ++i) - { - slopes[i] = -controller_states_[i].position_ / max_time; - interceptes[i] = controller_states_[i].position_; - } - - - double error_norm = NaN; - - while(local_time < max_time && error_norm > 0.01) - { - for(int i = 0; i < joint_controller_number_; ++i) - { - controller_commands_[i].position_ = slopes[i] * local_time + interceptes[i]; - } - error_norm = 0; - controllers_make_commands(); - pi3hat_->Cycle(pi3hat_input_); - ::usleep(sleep_time); - controllers_get_states(); - for(int i = 0; i < joint_controller_number_; ++i) - { - error_norm += std::abs(controller_states_[i].position_ - controller_commands_[i].position_); - } - local_time += ((double)sleep_time) / 1000000.0; - } - if(error_norm > 0.1) - { - RCLCPP_WARN(*logger_, "Motors didn't reached starting position!"); - } - else - { - RCLCPP_INFO(*logger_, "Motors reached starting position!"); - } + + RCLCPP_INFO(*logger_, "Motors reached starting position!"); return hardware_interface::CallbackReturn::SUCCESS; } @@ -351,7 +290,7 @@ std::vector Pi3HatHardwareInterface::expor { if(!(info_.joints[i].command_interfaces.size() > 0)) { - RCLCPP_WARN(*logger_, "Zero command interfaces for joint %s!", info_.joints[i].name); + RCLCPP_WARN(*logger_, "Zero command interfaces for joint %s!", info_.joints[i].name.c_str()); } for(const auto& command_interface: info_.joints[i].command_interfaces) { @@ -917,8 +856,9 @@ void Pi3HatHardwareInterface::create_controller_joint_map() int controller_id = controller_bridges_[i].get_params().id_; for(int j = 0; j < joint_controller_number_; ++j) { + int id_from_rx_frame = controller_bridges_[i].get_id(rx_can_frames_[j]); RCLCPP_INFO(*logger_, "Joint: %d, Controller: %d, Frame id: %d, Frame bus: %d", joint_id, controller_id, rx_can_frames_[j].id, rx_can_frames_[j].bus); - if(controller_id == ((rx_can_frames_[j].id>> 8) & 0x7f)) + if(controller_id == id_from_rx_frame) { std::pair controller_joint_pair(rx_can_frames_[j].id, joint_id); controller_joint_map_.emplace(controller_joint_pair); @@ -933,10 +873,29 @@ void Pi3HatHardwareInterface::create_controller_joint_map() Pi3HatHardwareInterface::~Pi3HatHardwareInterface() { - on_deactivate(rclcpp_lifecycle::State()); + //on_deactivate(rclcpp_lifecycle::State()); // motors dont reach starting position, better to just turn them off on_cleanup(rclcpp_lifecycle::State()); } + +bool Pi3HatHardwareInterface::string_to_bool(const std::string& str) +{ + if(str == "true") + { + return true; + } + else if(str == "false") + { + return false; + } + else + { + throw std::invalid_argument("Wrong string value for boolean"); + } +} + + + #include "pluginlib/class_list_macros.hpp" PLUGINLIB_EXPORT_CLASS( diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro index 9fbfb8e8..ad08e330 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_double_motor.urdf.xacro @@ -122,9 +122,30 @@ pi3hat_hardware_interface/Pi3HatHardwareInterface - 0 - 0 - 0 + 0 + 0 + 0 + 1000 + + true + true + true + + true + true + true + + true + true + true + + true + true + true + + true + true + true diff --git a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro index e9b20284..c10cff39 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro +++ b/src/meldog_hardware/pi3hat_hardware_interface/urdf/test_single_motor.urdf.xacro @@ -59,6 +59,28 @@ 0 0 0 + 1000 + + true + true + true + + true + true + true + + true + true + true + + true + true + true + + true + true + true + @@ -72,7 +94,7 @@ 500.0 -500.0 10.0 - 1 + 0.2 From 61b912db11179a5db8accd92d66afa5dd6e42591 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 16:24:23 +0200 Subject: [PATCH 130/137] Changed constructor for ControllerBridge --- .../include/controllers/ControllerBridge.hpp | 6 +++--- .../include/controllers/Controllers.hpp | 3 +-- .../controllers/wrappers/WrappersTypes.hpp | 7 +++++++ .../src/controllers/ControllerBridge.cpp | 16 ++++++++++++++-- 4 files changed, 25 insertions(+), 7 deletions(-) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index b00a5e29..e50f8e12 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -2,14 +2,14 @@ #define _CONTROLLER_BRIDGE_HPP_ #include "3rd_libs/pi3hat/pi3hat.h" -#include "wrappers/ControllerWrapper.hpp" +#include "wrappers/WrappersTypes.hpp" #include "ControllerStructures.hpp" +#include #include #include namespace controller_interface { - class ControllerBridge { private: @@ -21,7 +21,7 @@ class ControllerBridge public: - ControllerBridge(std::unique_ptr wrapper, + ControllerBridge(std::string wrapper_type, const ControllerParameters& params); ControllerBridge(const ControllerBridge& other_controller) = delete; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp index ff90ceec..5ee76e76 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp @@ -1,9 +1,8 @@ #ifndef _CONTROLLERS_HPP_ #define _CONTROLLERS_HPP_ -#include "wrappers/MoteusWrapper.hpp" #include "ControllerBridge.hpp" -#include "wrappers/ControllerWrapper.hpp" #include "ControllerStructures.hpp" +#include "wrappers/WrappersTypes.hpp" #endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp new file mode 100644 index 00000000..bac6c6bc --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp @@ -0,0 +1,7 @@ +#ifndef _WRAPPER_TYPES_HPP_ +#define _WRAPPER_TYPES_HPP_ + +#include "ControllerWrapper.hpp" +#include "MoteusWrapper.hpp" + +#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index f26cdd84..80fa8082 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -6,9 +6,21 @@ using mjbots::pi3hat::CanFrame; ControllerBridge::ControllerBridge( - std::unique_ptr wrapper, + std::string wrapper_type, const ControllerParameters& params): - wrapper_(std::move(wrapper)), params_(params){} + wrapper_(nullptr), params_(params) +{ + if(wrapper_type == "moteus") + { + wrapper_ = make_moteus_wrapper(params); + } + + + else + { + throw std::invalid_argument("Wrong type of wrapper!"); + } +} ControllerBridge::ControllerBridge(ControllerBridge&& other_controller): wrapper_(std::move(other_controller.wrapper_)), params_(other_controller.params_) {} From ec32ad7534b8e6ae41d2a882cfd805b13614e7ec Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 16:24:44 +0200 Subject: [PATCH 131/137] Changed tests --- .../double_moteus_bridge_test.cpp} | 10 +++------- .../test/controllers/moteus/moteus_tests_compile.sh | 5 +++++ .../{simple_test.cpp => moteus/simple_moteus_test.cpp} | 0 .../single_moteus_bridge_test.cpp} | 3 +-- 4 files changed, 9 insertions(+), 9 deletions(-) rename src/meldog_hardware/pi3hat_hardware_interface/test/controllers/{double_bridge_test.cpp => moteus/double_moteus_bridge_test.cpp} (88%) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/moteus_tests_compile.sh rename src/meldog_hardware/pi3hat_hardware_interface/test/controllers/{simple_test.cpp => moteus/simple_moteus_test.cpp} (100%) rename src/meldog_hardware/pi3hat_hardware_interface/test/controllers/{single_bridge_test.cpp => moteus/single_moteus_bridge_test.cpp} (94%) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/double_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp similarity index 88% rename from src/meldog_hardware/pi3hat_hardware_interface/test/controllers/double_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp index 9063c327..9fbf2d25 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/double_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp @@ -96,13 +96,9 @@ int main(int argc, char** argv) std::vector controller_commands; std::vector controller_states; - controller_interface::MoteusWrapper moteus_wrapper_1(moteus_1_options, moteus_1_command); - std::unique_ptr moteus_wrapper_ptr_1 = std::make_unique(moteus_wrapper_1); - controller_interface::ControllerBridge controller_1(std::move(moteus_wrapper_ptr_1), params_1); - - controller_interface::MoteusWrapper moteus_wrapper_2(moteus_2_options, moteus_1_command); - std::unique_ptr moteus_wrapper_ptr_2 = std::make_unique(moteus_wrapper_2); - controller_interface::ControllerBridge controller_2(std::move(moteus_wrapper_ptr_2), params_2); + + controller_interface::ControllerBridge controller_1("moteus", params_1); + controller_interface::ControllerBridge controller_2("moteus", params_2); controllers.push_back(std::move(controller_1)); controllers.push_back(std::move(controller_2)); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/moteus_tests_compile.sh b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/moteus_tests_compile.sh new file mode 100644 index 00000000..3ed5ee47 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/moteus_tests_compile.sh @@ -0,0 +1,5 @@ +#!/bin/bash + +g++ -o simple_moteus_test simple_moteus_test.cpp -I../../include ../../include/3rd_libs/pi3hat/pi3hat.cc -lbcm_host +g++ -o single_moteus_bridge_test single_moteus_bridge_test.cpp -I../../include ../../include/3rd_libs/pi3hat/pi3hat.cc ../../src/controllers/ControllerBridge.cpp ../../src/controllers/wrappers/* -lbcm_host +g++ -o double_moteus_bridge_test double_moteus_bridge_test.cpp -I../../include ../../include/3rd_libs/pi3hat/pi3hat.cc ../../src/controllers/ControllerBridge.cpp ../../src/controllers/wrappers/* -lbcm_host \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/simple_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp similarity index 100% rename from src/meldog_hardware/pi3hat_hardware_interface/test/controllers/simple_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/single_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp similarity index 94% rename from src/meldog_hardware/pi3hat_hardware_interface/test/controllers/single_bridge_test.cpp rename to src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp index aaeb2ec6..e8cfa464 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/single_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp @@ -71,8 +71,7 @@ int main(int argc, char** argv) moteus_command.maximum_torque = params.torque_max_; moteus_command.velocity_limit = params.velocity_max_; - std::unique_ptr moteus_wrapper_ptr = controller_interface::make_moteus_wrapper(params); - controller_interface::ControllerBridge controller(std::move(moteus_wrapper_ptr), params); + controller_interface::ControllerBridge controller("moteus", params); controller_interface::ControllerCommand controller_command; From 7f54d6f88774e1b9cb6e81d1de289e41aeefa741 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 16:25:05 +0200 Subject: [PATCH 132/137] Removed controller bridge creation for constructor --- .../pi3hat_hardware_interface.hpp | 29 +-- .../src/pi3hat_hardware_interface.cpp | 195 +++--------------- 2 files changed, 36 insertions(+), 188 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 42c318d4..1eb9f92b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -88,12 +88,6 @@ namespace pi3hat_hardware_interface private: - - /* Here add your controller wrapper type */ - enum WrapperType - { - Moteus = 0, - }; /* UTILITY ROS2 OBJECTS: */ std::unique_ptr logger_; @@ -148,13 +142,6 @@ namespace pi3hat_hardware_interface /* FUNCTION FOR INITIALIZATION */ - /* Function for choosing wrappers (here u can add your own wrapper) - Remember to change this function in source code */ - WrapperType choose_wrapper_type(const std::string& type); - - /* Function for creating moteus wrappers (here u can add your own wrapper) */ - void add_controller_bridge(const controller_interface::ControllerParameters& params, const WrapperType type); - controller_interface::ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); /* FUNCTION FOR CONTROLLERS */ @@ -180,19 +167,11 @@ namespace pi3hat_hardware_interface /* Function for creating all transmissions */ void create_transmission_interface(const hardware_interface::HardwareInfo &info); - /* Functions for creating simple transmission */ - void create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names); - - /* Functions for creating four bar linkage transmission */ - void create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names); - - /* Functions for creating differential transmission */ - void create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names); + /* Function for creating one transmission */ + void create_transmission(const hardware_interface::TransmissionInfo& transmission_info, std::string type, + transmission_interface::TransmissionLoader& loader, const std::vector& joint_names); - /* Functions for checking, if data passed from urdf is correct */ + /* Functions for loading transmission data */ void load_transmission_data(const hardware_interface::TransmissionInfo& transmission_info, transmission_interface::TransmissionSharedPtr& transmission, transmission_interface::TransmissionLoader& loader); diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index d0142edb..76bea1e9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -32,13 +32,11 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa for (const hardware_interface::ComponentInfo &joint : info_.joints) { controller_interface::ControllerParameters params; - WrapperType type; + std::string wrapper_type; try { params = get_controller_parameters(joint); - - std::string type_string = joint.parameters.at("controller_type"); - type = choose_wrapper_type(type_string); + wrapper_type = joint.parameters.at("controller_type"); } catch(const std::exception& e) { @@ -48,7 +46,8 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_init(const hardwa try { - add_controller_bridge(params, type); + ControllerBridge controller_bridge(wrapper_type, params); + controller_bridges_.push_back(std::move(controller_bridge)); } catch(const std::exception& e) { @@ -551,7 +550,7 @@ void Pi3HatHardwareInterface::create_transmission_interface(const hardware_inter { try { - create_simple_transmission(transmission_info, simple_loader, joint_names); + create_transmission(transmission_info, "SimpleTransmission", simple_loader, joint_names); } catch(const transmission_interface::TransmissionInterfaceException& e) { @@ -568,7 +567,7 @@ void Pi3HatHardwareInterface::create_transmission_interface(const hardware_inter { try { - create_fbl_transmission(transmission_info, fbl_loader, joint_names); + create_transmission(transmission_info, "FourBarLinkageTransmission", fbl_loader, joint_names); } catch(const transmission_interface::TransmissionInterfaceException& e) { @@ -585,7 +584,7 @@ void Pi3HatHardwareInterface::create_transmission_interface(const hardware_inter { try { - create_diff_transmission(transmission_info, diff_loader, joint_names); + create_transmission(transmission_info, "DifferentialTransmission", diff_loader, joint_names); } catch(const transmission_interface::TransmissionInterfaceException& e) { @@ -595,162 +594,56 @@ void Pi3HatHardwareInterface::create_transmission_interface(const hardware_inter } } -void Pi3HatHardwareInterface::create_simple_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::SimpleTransmissionLoader& loader, const std::vector& joint_names) + void Pi3HatHardwareInterface::create_transmission(const hardware_interface::TransmissionInfo& transmission_info, std::string type, + transmission_interface::TransmissionLoader& loader, const std::vector& joint_names) { - RCLCPP_INFO(*logger_, "Simple transmission initialization starting!"); + RCLCPP_INFO(*logger_, "%s initialization starting!", type.c_str()); std::shared_ptr transmission; - if (transmission_info.type != "transmission_interface/SimpleTransmission") - { - RCLCPP_FATAL(*logger_, "This is not SimpleTransmission!"); - throw transmission_interface::TransmissionInterfaceException("This is not SimpleTransmission!"); // this should not happen! - } - - load_transmission_data(transmission_info, transmission, loader); - if(transmission_info.joints.size() != 1) + if (transmission_info.type != ("transmission_interface/" + type)) { - RCLCPP_FATAL(*logger_, "Invalid number of joints in SimpleTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in SimpleTransmission!"); // this should not happen! - } - - if(transmission_info.actuators.size() != 1) - { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in SimpleTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in SimpleTransmission!"); // this should not happen! + RCLCPP_FATAL(*logger_, "This is not %s!", type.c_str()); + throw transmission_interface::TransmissionInterfaceException("This is not " + type + "!"); // this should not happen! } + load_transmission_data(transmission_info, transmission, loader); - /* Find joint index for transmission handels by name*/ - std::vector::const_iterator joint_it = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[0].name); - int joint_index = std::distance(joint_names.begin(), joint_it); - - /* Joint handels */ - std::vector joint_handles; - append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index); - - /* Actuator handels */ - std::vector actuator_handles; - append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index); + int joint_for_transmission = 0; - try + if(type == "SimpleTransmission") { - transmission->configure(joint_handles, actuator_handles); + joint_for_transmission = 1; } - catch (const transmission_interface::TransmissionInterfaceException & exc) + else if(type == "FourBarLinkageTransmission" || type == "DifferentialTransmission") { - RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - throw; + joint_for_transmission = 2; } - transmissions_.push_back(transmission); - - RCLCPP_INFO(*logger_, "Simple transmissions initialized!"); -} - -void Pi3HatHardwareInterface::create_fbl_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::FourBarLinkageTransmissionLoader& loader, const std::vector& joint_names) -{ - RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialization starting!"); - - std::shared_ptr transmission; - if (transmission_info.type != "transmission_interface/FourBarLinkageTransmission") + if(transmission_info.joints.size() != joint_for_transmission) { - RCLCPP_FATAL(*logger_, "This is not FourBarLinkageTransmission!"); - throw transmission_interface::TransmissionInterfaceException("This is not FourBarLinkageTransmission!"); // this should not happen! + RCLCPP_FATAL(*logger_, "Invalid number of joints in %s!", type.c_str()); + throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in SimpleTransmission!"); // this should not happen! } - load_transmission_data(transmission_info, transmission, loader); - - if(transmission_info.joints.size() != 2) - { - RCLCPP_FATAL(*logger_, "Invalid number of joints in FourBarLinkageTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in FourBarLinkageTransmission!"); // this should not happen! - } - if(transmission_info.actuators.size() != 2) + if(transmission_info.actuators.size() != joint_for_transmission) { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in FourBarLinkageTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in FourBarLinkageTransmission!"); // this should not happen! + RCLCPP_FATAL(*logger_, "Invalid number of actuators in %s!", type.c_str()); + throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in SimpleTransmission!"); // this should not happen! } - /* Find joints indexes for transmission handels by name*/ - std::vector::const_iterator joint_it_1 = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[0].name); - int joint_index_1 = std::distance(joint_names.begin(), joint_it_1); - - std::vector::const_iterator joint_it_2 = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[1].name); - int joint_index_2 = std::distance(joint_names.begin(), joint_it_2); - - /* Joint handels */ std::vector joint_handles; - append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index_1); - append_joint_handles(joint_handles, transmission_info.joints[1].name, joint_index_2); - - /* Actuator handels */ std::vector actuator_handles; - append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index_1); - append_actuator_handles(actuator_handles, transmission_info.actuators[1].name, joint_index_2); - try + for(int i = 0; i < joint_for_transmission; ++i) { - transmission->configure(joint_handles, actuator_handles); - } - catch (const transmission_interface::TransmissionInterfaceException & exc) - { - RCLCPP_FATAL(*logger_, "Error while loading %s: %s", transmission_info.name.c_str(), exc.what()); - throw; - } + std::vector::const_iterator joint_it = std::find(joint_names.begin(), + joint_names.end(), transmission_info.joints[i].name); - transmissions_.push_back(transmission); - - RCLCPP_INFO(*logger_, "FourBarLinkage transmissions initialized!"); -} + int joint_index = std::distance(joint_names.begin(), joint_it); -void Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface::TransmissionInfo& transmission_info, - transmission_interface::DifferentialTransmissionLoader& loader, const std::vector& joint_names) -{ - RCLCPP_INFO(*logger_, "Differential transmissions initialization starting!"); - - std::shared_ptr transmission; - if (transmission_info.type != "transmission_interface/DifferentialTransmission") - { - RCLCPP_FATAL(*logger_, "This is not DifferentialTransmission!"); - throw transmission_interface::TransmissionInterfaceException("This is not DifferentialTransmission!"); // this should not happen! + append_joint_handles(joint_handles, transmission_info.joints[i].name, joint_index); + append_actuator_handles(actuator_handles, transmission_info.actuators[i].name, joint_index); } - - load_transmission_data(transmission_info, transmission, loader); - - if(transmission_info.joints.size() != 2) - { - RCLCPP_FATAL(*logger_, "Invalid number of joints in DifferentialTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of joints in DifferentialTransmission!");; // this should not happen! - } - if(transmission_info.actuators.size() != 2) - { - RCLCPP_FATAL(*logger_, "Invalid number of actuators in DifferentialTransmission!"); - throw transmission_interface::TransmissionInterfaceException("Invalid number of actuators in DifferentialTransmission!"); // this should not happen! - } - - /* Find joints indexes for transmission handels by name*/ - std::vector::const_iterator joint_it_1 = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[0].name); - int joint_index_1 = std::distance(joint_names.begin(), joint_it_1); - - std::vector::const_iterator joint_it_2 = std::find(joint_names.begin(), - joint_names.end(), transmission_info.joints[1].name); - int joint_index_2 = std::distance(joint_names.begin(), joint_it_2); - - /* Joint handels */ - std::vector joint_handles; - append_joint_handles(joint_handles, transmission_info.joints[0].name, joint_index_1); - append_joint_handles(joint_handles, transmission_info.joints[1].name, joint_index_2); - - /* Actuator handels */ - std::vector actuator_handles; - append_actuator_handles(actuator_handles, transmission_info.actuators[0].name, joint_index_1); - append_actuator_handles(actuator_handles, transmission_info.actuators[1].name, joint_index_2); try { @@ -764,33 +657,9 @@ void Pi3HatHardwareInterface::create_diff_transmission(const hardware_interface: transmissions_.push_back(transmission); - RCLCPP_INFO(*logger_, "Differential transmissions initialized!"); -} - -void Pi3HatHardwareInterface::add_controller_bridge(const ControllerParameters& params, const WrapperType type) -{ - std::unique_ptr wrapper_ptr; - switch(type) - { - case Moteus: - wrapper_ptr = make_moteus_wrapper(params); - break; - default: - throw std::invalid_argument("Motor type doesn't exist!"); - break; - } + RCLCPP_INFO(*logger_, "%s initialized!", type.c_str()); - controller_interface::ControllerBridge bridge = controller_interface::ControllerBridge(std::move(wrapper_ptr), params); - controller_bridges_.push_back(std::move(bridge)); -} -Pi3HatHardwareInterface::WrapperType Pi3HatHardwareInterface::choose_wrapper_type(const std::string& type) -{ - if(type == "moteus") - { - return WrapperType::Moteus; - } - return WrapperType::Moteus; } ControllerParameters Pi3HatHardwareInterface::get_controller_parameters(const hardware_interface::ComponentInfo& joint_info) From cc94e84baefcc6c6be15fc14a69d26e4c8d67b8e Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 16:48:30 +0200 Subject: [PATCH 133/137] Removed old code --- .../actuator_wrappers/ActuatorWrapperBase.hpp | 98 ------------------- .../actuator_wrappers/MoteusWrapper.hpp | 37 ------- .../old_code/JointActuatorTransform.hpp | 62 ------------ .../include/old_code/Transmissions.hpp | 60 ------------ .../src/actuator_wrappers/MoteusWrapper.cpp | 60 ------------ .../src/old_code/JointActuatorTransform.cpp | 91 ----------------- .../src/old_code/Transmissions.cpp | 40 -------- 7 files changed, 448 deletions(-) delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp delete mode 100644 src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp deleted file mode 100644 index 489ef54f..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/ActuatorWrapperBase.hpp +++ /dev/null @@ -1,98 +0,0 @@ -#ifndef _MOTOR_WRAPPER_BASE_ -#define _MOTOR_WRAPPER_BASE_ - -#include "3rd_libs/pi3hat/pi3hat.h" -#include -/* - Base Actuator Wrapper class, used for wrapping actuator API with simple CRTP interface - to create CAN frames for pi3hat Input structure. Note that it uses static polymorphism, - so remember to change instance of your derived class in pi3hat hardware interface files - (only for creation of an object). Converting values (like rotations to radians) should - be done in your wrapper. -*/ -namespace actuator_wrappers -{ - -/* Structure for basic actuator command */ -struct ActuatorCommand -{ - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ -}; - -/* Structure for basic actuator state */ -struct ActuatorState -{ - double position_; /* [radians] */ - double velocity_; /* [radians/s] */ - double torque_; /* [Nm] */ - int temperature_; /* [Celcius] */ - bool fault = false; -}; - -struct ActuatorParameters -{ - int id_; /* Usage in your wrapper (check moteus wrapper)*/ - int bus_; /* Usage in your wrapper (check moteus wrapper)*/ - double position_max_; - double position_min_; - double position_offset_; - double velocity_max_; - double torque_max_; - int direction_ = 1; -}; - -template -class ActuatorWrapperBase -{ - private: - - /* Used for CRTP interface */ - Derived& derived() - { - return static_cast(*this); - }; - - protected: - ActuatorParameters params_; - - /* pi3hat CAN frames */ - using CanFrame = mjbots::pi3hat::CanFrame; - - public: - - /* Constructor: takes CanFrame for later editing*/ - ActuatorWrapperBase(const ActuatorParameters& params): params_(params) {}; - - /* Static virtual method for starting actuators */ - void init(CanFrame& tx_frame) - { - derived().init(tx_frame); - }; - - /* Static virtual method for preparing TX CAN frame from ActuatorCommand */ - void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) - { - command.position_ = params_.direction_* std::clamp(command.position_, - params_.position_min_, params_.position_max_) + params_.position_offset_; - command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); - command.torque_ = params_.direction_* std::clamp(command.torque_, -params_.torque_max_, params_.torque_max_); - - derived().command_to_tx_frame(tx_frame, command); - }; - - /* Static virtual method for preparing ActuatorState form RX CAN frame */ - void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) - { - derived().rx_frame_to_state(rx_frame, state); - state.position_ = params_.direction_ * state.position_; - state.velocity_ = params_.direction_ * state.velocity_; - state.torque_ = params_.direction_ * state.torque_; - }; - -}; -}; - - -#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp deleted file mode 100644 index be24831a..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/actuator_wrappers/MoteusWrapper.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#ifndef _MOTEUS_ACTUATOR_WRAPPER_ -#define _MOTEUS_ACTUATOR_WRAPPER_ - - -#include "3rd_libs/moteus/moteus.h" -#include "ActuatorWrapperBase.hpp" - -namespace actuator_wrappers -{ -class MoteusWrapper: public ActuatorWrapperBase, protected mjbots::moteus::Controller -{ - private: - - /* Const coefficients for easy radians - rotations transform */ - static const double rotation_to_radians = 2 * M_PI; - static const double radians_to_rotation = 1 / (2 * M_PI); /* Multiplying is faster than dividing */ - - /* Command structure for moteus object*/ - mjbots::moteus::PositionMode::Command position_command_; - - public: - /* Create Moteus Wrapper from existing frames */ - MoteusWrapper(const ActuatorParameters& params, - mjbots::moteus::Controller::Options& options, - mjbots::moteus::PositionMode::Command& command); - - /* Static override */ - void init(CanFrame& tx_frame); - void command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command); - void rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state); -}; -}; - - - - -#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp deleted file mode 100644 index a0c54a2a..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/JointActuatorTransform.hpp +++ /dev/null @@ -1,62 +0,0 @@ -#ifndef _JOINT_ACTUATOR_TRANSFORM_ -#define _JOINT_ACTUATOR_TRANSFORM_ - -#include -#include -#include "Transmissions.hpp" - -namespace joint_actuator_transform -{ - /* - Class for transforming joint space to actuator space and actuator space to joint space. - It uses Transmission objects to create sparse matrixes used in transformations. - */ - class JointActuatorTransform - { - private: - - using vector = Eigen::Vector; - using TransmissionVector = std::vector>; - - /* vectors of joint states */ - vector& joint_positions_; - vector& joint_velocities_; - vector& joint_torques_; - - /* vetors of actuator states */ - vector& actuator_positions_; - vector& actuator_velocities_; - vector& actuator_torques_; - /* actuator -> joint transform matrixes */ - Eigen::SparseMatrix joint_position_velocity_matrix_; - Eigen::SparseMatrix joint_torque_matrix_; - /* joint -> actuator matrixes */ - Eigen::SparseMatrix actuator_position_velocity_matrix_; - Eigen::SparseMatrix actuator_torque_matrix_; - - vector joint_offsets_; - - public: - /* Constructor */ - JointActuatorTransform(vector joint_positions, vector& joint_velocities, vector& joint_torques, - vector& actuator_positions, vector& actuator_velocities, vector& actuator_torques); - - /* Create all transform matrixes */ - void make_transform_matrixes(const TransmissionVector& transmission_vector); - - /* Add joint offsets to transform - q_actuator = 0 and q_joint = q_joint_start - */ - void add_offsets(vector&& joint_offsets); - - /* joint <-> actuator transformations */ - void positions_to_joint_space(); - void positions_to_actuator_space(); - void velocities_to_joint_space(); - void velocities_to_actuator_space(); - void torques_to_joint_space(); - void torques_to_actuator_space(); - }; -}; - -#endif diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp deleted file mode 100644 index e3f69c61..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/old_code/Transmissions.hpp +++ /dev/null @@ -1,60 +0,0 @@ -#ifndef _TRANSMISSIONS_ -#define _TRANSMISSIONS_ - -#include -#include -#include - - -namespace joint_actuator_transform -{ - /* - Transmission interface class. Its job is to create triplets for sparse transformation matrixes. - - */ - class Transmission - { - public: - using T = Eigen::Triplet; - - /* Calculates new triplets for sparse transmission matrix */ - virtual void calculate_transmission_triplets(std::vector& triplet_vector) = 0; - virtual ~Transmission() = default; - }; - - /* Simple Transmission: (gearboxes, simple pulley systems etc.): - http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1SimpleTransmission.html - */ - class SimpleTransmission: Transmission - { - private: - size_t joint_index_; - double reduction_; - - public: - SimpleTransmission(size_t joint_index, double reduction); - void calculate_transmission_triplets(std::vector& triplet_vector) override; - - }; - - /* FourBarLinkage Transmission: more complicated systems where second - actuator is coupled with first one, like shown here: - http://docs.ros.org/en/jade/api/transmission_interface/html/c++/classtransmission__interface_1_1FourBarLinkageTransmission.html - */ - class FourBarLinkageTransmission: Transmission - { - private: - std::vector joint_indexes_; - std::vector actuator_reductions_; - std::vector joint_reductions_; - - public: - FourBarLinkageTransmission(const std::vector& joint_indexes, - std::vector actuator_reductions, std::vector joint_reductions); - void calculate_transmission_triplets(std::vector& triplet_vector) override; - - }; -}; - - -#endif \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp deleted file mode 100644 index f5c14ab3..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/actuator_wrappers/MoteusWrapper.cpp +++ /dev/null @@ -1,60 +0,0 @@ -#include "actuator_wrappers/MoteusWrapper.hpp" -//#include "../../include/actuator_wrappers/MoteusWrapper.hpp" - -/* - Moteus Actuator Wrapper. It uses API from moteus repository to communicate with pi3hat interface. - https://github.com/mjbots/moteus - -*/ - -using namespace actuator_wrappers; - -MoteusWrapper::MoteusWrapper(const ActuatorParameters& params, - mjbots::moteus::Controller::Options& options, - mjbots::moteus::PositionMode::Command& command): -ActuatorWrapperBase(params), mjbots::moteus::Controller(options), -position_command_(command){ } - - - -void MoteusWrapper::init(CanFrame& tx_frame) -{ - /* create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakeStop(); - - /* Copy data from CANFD frame to CAN frame*/ - tx_frame.id = can_fd_frame.arbitration_id; - tx_frame.bus = can_fd_frame.bus; - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); -} - -void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, ActuatorCommand& command) -{ - /* Change command values */ - position_command_.position = command.position_ * radians_to_rotation; - position_command_.velocity = command.velocity_ * radians_to_rotation; - position_command_.feedforward_torque = command.torque_; - - /* Create CANFD frame*/ - mjbots::moteus::CanFdFrame can_fd_frame = mjbots::moteus::Controller::MakePosition(position_command_); - - /* Copy data from CANFD frame to CAN frame*/ - tx_frame.id = can_fd_frame.arbitration_id; - tx_frame.bus = can_fd_frame.bus; - tx_frame.size = can_fd_frame.size; - std::memcpy(tx_frame.data, can_fd_frame.data, can_fd_frame.size); -} - -void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ActuatorState& state) -{ - /* Parse data from RX CAN frame to Result object */ - if(((rx_frame.id >> 8) & 0x7f) != params_.id_) return; /* This should not happen! (map frame to wrapper first) */ - - mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); - state.position_ = result.position * rotation_to_radians; - state.velocity_ = result.velocity * rotation_to_radians; - state.torque_ = result.torque; - state.temperature_ = result.temperature; - state.fault = result.fault; -} diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp deleted file mode 100644 index 642851f7..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/JointActuatorTransform.cpp +++ /dev/null @@ -1,91 +0,0 @@ -#include "../include/joint_actuator_transform/JointActuatorTransform.hpp" - - -using namespace joint_actuator_transform; - -JointActuatorTransform::JointActuatorTransform(vector joint_positions, vector& joint_velocities, vector& joint_torques, - vector& actuator_positions, vector& actuator_velocities, vector& actuator_torques): - joint_positions_(joint_positions), joint_velocities_(joint_velocities), - joint_torques_(joint_torques), actuator_positions_(actuator_positions), - actuator_velocities_(actuator_velocities), actuator_torques_(actuator_torques) {} - - -void JointActuatorTransform::make_transform_matrixes(const TransmissionVector& transmission_vector) -{ - /* Create sparse matrix for transforming joint position and velocities to actuator space: - - Q_actuator = A * Q_joint - - V_actuator = A * V_joint - */ - std::vector> transmission_triplets; - for(const auto& transmission: transmission_vector) - { - transmission->calculate_transmission_triplets(transmission_triplets); - } - actuator_position_velocity_matrix_.setFromTriplets(transmission_triplets.begin(), transmission_triplets.end()); - - /* Prepare solvers for calculating other sparse matrixes*/ - Eigen::SimplicialLDLT> solver; - Eigen::SparseMatrix I(actuator_position_velocity_matrix_.rows(),actuator_position_velocity_matrix_.cols()); - I.setIdentity(); - - /* Create sparse matrix for transforming actuator position and velocities to joint space: - - Q_joint = B * Q_actuator - - V_joint = B * V_actuator - - B = (A)^(-1) - */ - solver.compute(actuator_position_velocity_matrix_); - joint_position_velocity_matrix_ = solver.solve(I); - - /* Create sparse matrix for transforming actuator torques to joint space: - - Tau_joint = C * Tau_actuator - - C = (A)^transpose - */ - joint_torque_matrix_ = actuator_position_velocity_matrix_.transpose(); - - /* Create sparse matrix for transforming joint torques to actuator space: - - Tau_actuator = D * Tau_joint - - D = (C)^(-1) - */ - solver.compute(joint_torque_matrix_); - actuator_torque_matrix_ = solver.solve(I); - - /* Compressing all sparse matrixes */ - actuator_position_velocity_matrix_.makeCompressed(); - actuator_torque_matrix_.makeCompressed(); - joint_position_velocity_matrix_.makeCompressed(); - joint_torque_matrix_.makeCompressed(); -} - -void JointActuatorTransform::positions_to_joint_space() -{ - joint_positions_ = joint_position_velocity_matrix_ * actuator_positions_ + joint_offsets_; -} - -void JointActuatorTransform::positions_to_actuator_space() -{ - actuator_positions_ = actuator_position_velocity_matrix_ * (joint_positions_ - joint_offsets_); -} - -void JointActuatorTransform::velocities_to_joint_space() -{ - joint_velocities_ = joint_position_velocity_matrix_ * actuator_velocities_; -} - -void JointActuatorTransform::velocities_to_actuator_space() -{ - actuator_velocities_= actuator_position_velocity_matrix_ * joint_velocities_; -} - -void JointActuatorTransform::torques_to_joint_space() -{ - joint_torques_ = joint_torque_matrix_ * actuator_torques_; -} -void JointActuatorTransform::torques_to_actuator_space() -{ - actuator_torques_ = actuator_torque_matrix_ * joint_torques_; -} - -void JointActuatorTransform::add_offsets(vector&& joint_offsets) -{ - joint_offsets_ = std::move(joint_offsets); -} \ No newline at end of file diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp deleted file mode 100644 index 54571123..00000000 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/old_code/Transmissions.cpp +++ /dev/null @@ -1,40 +0,0 @@ -#include "../include/joint_actuator_transform/Transmissions.hpp" - -using namespace joint_actuator_transform; - -SimpleTransmission::SimpleTransmission(size_t joint_index, double reduction): -joint_index_(joint_index), reduction_(reduction) {} - -void SimpleTransmission::calculate_transmission_triplets(std::vector& triplet_vector) -{ - /* - Simple q_motor = reduction * q_joint - A[joint_index, joint_index] = reduction - */ - triplet_vector.push_back(T(joint_index_, joint_index_, reduction_)); -} - - -FourBarLinkageTransmission::FourBarLinkageTransmission(const std::vector& joint_indexes, - std::vector actuator_reductions, std::vector joint_reductions): - joint_indexes_(joint_indexes), actuator_reductions_(actuator_reductions), - joint_reductions_(joint_reductions) {} - -void FourBarLinkageTransmission::calculate_transmission_triplets(std::vector& triplet_vector) -{ - /* - q_motor_1 = a_reduction_1 * j_reduction_1 * q_joint_1 - q_motor_2 = a_reduction_2 * q_joint_1 + a_reduction_2 * j_reduction_2 * q_joint_2 - A[joint_index_1, joint_index_1] = a_reduction_1 * j_reduction_1 - A[joint_index_2, joint_index_1] = a_reduction_2 - A[joint_index_2, joint_index_2] = a_reduction_2 * j_reduction_2 - */ - triplet_vector.push_back(T(joint_indexes_[0], joint_indexes_[0], - actuator_reductions_[0]*joint_reductions_[0])); - - triplet_vector.push_back(T(joint_indexes_[1], joint_indexes_[0], - actuator_reductions_[1])); - - triplet_vector.push_back(T(joint_indexes_[1], joint_indexes_[1], - actuator_reductions_[1]*joint_reductions_[1])); -} \ No newline at end of file From bd8eec11da2162057e3f291438e0318517753422 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 17:25:07 +0200 Subject: [PATCH 134/137] Added license to files and documented code --- .../include/controllers/ControllerBridge.hpp | 16 +++++++ .../controllers/ControllerStructures.hpp | 46 ++++++++++++------- .../include/controllers/Controllers.hpp | 13 ++++++ .../wrappers/ControllerWrapper.hpp | 18 +++++++- .../controllers/wrappers/MoteusWrapper.hpp | 15 ++++++ .../controllers/wrappers/WrappersTypes.hpp | 16 +++++++ .../include/imu_transform/IMUTransform.hpp | 32 +++++++++---- .../pi3hat_hardware_interface.hpp | 15 +++++- .../src/controllers/ControllerBridge.cpp | 18 +++++++- .../wrappers/ControllerWrapper.cpp | 14 +++++- .../controllers/wrappers/MoteusWrapper.cpp | 31 +++++++++---- .../src/imu_transform/IMUTransform.cpp | 14 +++++- .../src/pi3hat_hardware_interface.cpp | 14 +++++- .../moteus/double_moteus_bridge_test.cpp | 14 +++++- .../controllers/moteus/simple_moteus_test.cpp | 13 ++++++ .../moteus/single_moteus_bridge_test.cpp | 12 ++++- 16 files changed, 259 insertions(+), 42 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp index e50f8e12..deff180b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerBridge.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _CONTROLLER_BRIDGE_HPP_ #define _CONTROLLER_BRIDGE_HPP_ @@ -10,6 +23,9 @@ namespace controller_interface { + +/* Class for abstracting communication with diffrent type of controllers. + Class makes basic transformations for command and state structures before using wrapper */ class ControllerBridge { private: diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp index c0efa1bd..8c2982e8 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/ControllerStructures.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _CONTROLLER_STRUCTURES_HPP_ #define _CONTROLLER_STRUCTURES_HPP_ @@ -7,31 +20,32 @@ namespace controller_interface /* Structure for basic actuator command */ struct ControllerCommand { - double position_ = 0; /* [radians] */ - double velocity_ = 0; /* [radians/s] */ - double torque_ = 0; /* [Nm] */ + double position_ = 0; /* [radians] */ + double velocity_ = 0; /* [radians/s] */ + double torque_ = 0; /* [Nm] */ }; /* Structure for basic actuator state */ struct ControllerState { - double position_ = 0; /* [radians] */ - double velocity_ = 0; /* [radians/s] */ - double torque_ = 0; /* [Nm] */ - double temperature_ = 0; /* [Celcius] */ - bool fault = false; + double position_ = 0; /* [radians] */ + double velocity_ = 0; /* [radians/s] */ + double torque_ = 0; /* [Nm] */ + double temperature_ = 0; /* [Celcius] */ + bool fault = false; }; +/* Structure for basic controller parameters */ struct ControllerParameters { - double position_max_ = 0; - double position_min_ = 0; - double position_offset_ = 0; - double velocity_max_ = 0; - double torque_max_ = 0; - int direction_ = 1; - int id_ = 0; /* Usage in your bridge (check moteus bridge)*/ - int bus_ = 0; /* Usage in your bridge (check moteus bridge)*/ + double position_max_ = 0; /* [radians] */ + double position_min_ = 0; /* [radians] */ + double position_offset_ = 0; /* [radians] */ + double velocity_max_ = 0; /* [radians/s] */ + double torque_max_ = 0; /* [Nm] */ + int direction_ = 1; /* 1 or -1 */ + int id_ = 0; /* Usage in your bridge (check moteus bridge) */ + int bus_ = 0; /* Usage in your bridge (check moteus bridge) */ }; } diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp index 5ee76e76..22eb0b87 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/Controllers.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _CONTROLLERS_HPP_ #define _CONTROLLERS_HPP_ diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp index cb449191..b951ecd1 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/ControllerWrapper.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _CONTROLLER_WRAPPER_HPP_ #define _CONTROLLER_WRAPPER_HPP_ @@ -7,6 +20,8 @@ namespace controller_interface { +/* Interface class for wrapper that connects 3rd party lib for controller to connector + bridge interface. Check out MoteusWrapper */ class ControllerWrapper { protected: @@ -19,7 +34,8 @@ class ControllerWrapper ControllerWrapper(ControllerWrapper&& other) = default; ControllerWrapper& operator=(const ControllerWrapper& other) = default; ControllerWrapper& operator=(ControllerWrapper&& other) = default; - + + virtual void command_to_tx_frame(CanFrame& tx_frame, const ControllerCommand& command) = 0; virtual void query_to_tx_frame(CanFrame& tx_frame) = 0; virtual void rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) = 0; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp index b0979897..84a500b9 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/MoteusWrapper.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _MOTEUS_WRAPPER_HPP_ #define _MOTEUS_WRAPPER_HPP_ @@ -34,6 +47,8 @@ class MoteusWrapper final: public ControllerWrapper }; +/* Copying for Moteus class is deleted, prevents from making constructor for MoteusWrapper + with only ControllerParameter as argument :/ */ std::unique_ptr make_moteus_wrapper(const ControllerParameters& params); }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp index bac6c6bc..f02c8e58 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/controllers/wrappers/WrappersTypes.hpp @@ -1,6 +1,22 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _WRAPPER_TYPES_HPP_ #define _WRAPPER_TYPES_HPP_ + +/* Here add header file for your wrapper */ + #include "ControllerWrapper.hpp" #include "MoteusWrapper.hpp" diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp index 4baf6c9c..def243dd 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/imu_transform/IMUTransform.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef _IMU_ #define _IMU_ @@ -7,17 +20,20 @@ #include namespace IMU +{ + +/* Class for basic transforming filtered IMU state from Pi3hat */ +class IMUTransform { - class IMUTransform - { - private: - constexpr static double degrees_to_radians = 2 * M_PI / 360.0; + private: + + constexpr static double degrees_to_radians = 2 * M_PI / 360.0; - public: + public: - /* Function for transforming transforming IMU data */ - void transform_attitude(mjbots::pi3hat::Attitude& attitude); - }; + /* Function for transforming IMU data */ + void transform_attitude(mjbots::pi3hat::Attitude& attitude); +}; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp index 1eb9f92b..8cfaa7e7 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #ifndef PI3HAT_HARDWARE_INTERFACE__PI3HAT_HARDWARE_INTERFACE_ #define PI3HAT_HARDWARE_INTERFACE__PI3HAT_HARDWARE_INTERFACE_ @@ -141,7 +154,6 @@ namespace pi3hat_hardware_interface /* FUNCTION FOR INITIALIZATION */ - controller_interface::ControllerParameters get_controller_parameters(const hardware_interface::ComponentInfo& joint_info); /* FUNCTION FOR CONTROLLERS */ @@ -189,7 +201,6 @@ namespace pi3hat_hardware_interface /* UTILITY FUNCTIONS */ - bool string_to_bool(const std::string& str); }; }; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp index 80fa8082..2b68e0df 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/ControllerBridge.cpp @@ -1,5 +1,17 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "controllers/ControllerBridge.hpp" -//#include "../../include/controllers/ControllerBridge.hpp" using namespace controller_interface; using mjbots::pi3hat::CanFrame; @@ -10,6 +22,7 @@ ControllerBridge::ControllerBridge( const ControllerParameters& params): wrapper_(nullptr), params_(params) { + /* Here add your wrapper type as std::string (use "else if" after this "if") */ if(wrapper_type == "moteus") { wrapper_ = make_moteus_wrapper(params); @@ -37,6 +50,7 @@ ControllerBridge& ControllerBridge::operator=(ControllerBridge&& other_controlle void ControllerBridge::make_command(CanFrame& tx_frame, ControllerCommand& command) const { + /* Basic transformations before sending data to wrapper */ command.position_ = params_.direction_* std::clamp(command.position_, params_.position_min_, params_.position_max_) + params_.position_offset_; command.velocity_ = params_.direction_* std::clamp(command.velocity_, -params_.velocity_max_, params_.velocity_max_); @@ -57,6 +71,8 @@ void ControllerBridge::make_query(CanFrame& tx_frame) const void ControllerBridge::get_state(const CanFrame& rx_frame, ControllerState& state) const { wrapper_->rx_frame_to_state(rx_frame, state); + + /* Basic transformations after getting data from wrapper */ state.position_ = params_.direction_ * (state.position_ - params_.position_offset_); state.velocity_ = params_.direction_ * state.velocity_; state.torque_ = params_.direction_ * state.torque_; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp index f9341694..181380cf 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/ControllerWrapper.cpp @@ -1,5 +1,17 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "controllers/wrappers/ControllerWrapper.hpp" -//#include "../../../include/controllers/wrappers/ControllerWrapper.hpp" using namespace controller_interface; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp index 8b69cc95..38c910fe 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/controllers/wrappers/MoteusWrapper.cpp @@ -1,5 +1,17 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "controllers/wrappers/MoteusWrapper.hpp" -//#include "../../../include/controllers/wrappers/MoteusWrapper.hpp" using namespace controller_interface; @@ -19,7 +31,7 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm position_command_.velocity = command.velocity_ * radians_to_rotation_; position_command_.feedforward_torque = command.torque_; - /* Create CANFD frame*/ + /* Create CANFD frame */ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakePosition(position_command_); /* Copy data from CANFD frame to CAN frame */ @@ -31,10 +43,10 @@ void MoteusWrapper::command_to_tx_frame(CanFrame& tx_frame, const ControllerComm void MoteusWrapper::query_to_tx_frame(CanFrame& tx_frame) { - /* Create CANFD frame*/ + /* Create CANFD frame */ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeQuery(); - /* Copy data from CANFD frame to CAN frame */ + /* Copy data from CANFD frame to TX CANFD Pi3hat frame */ tx_frame.id = can_fd_frame.arbitration_id; tx_frame.bus = can_fd_frame.bus; tx_frame.size = can_fd_frame.size; @@ -43,7 +55,7 @@ void MoteusWrapper::query_to_tx_frame(CanFrame& tx_frame) void MoteusWrapper::rx_frame_to_state(const CanFrame& rx_frame, ControllerState& state) { - /* Parse data from RX CAN frame to Result object */ + /* Parse data from RX CANFD Pi3hat frame to Result object */ if(((rx_frame.id >> 8) & 0x7f) != (uint32_t) moteus_controller_.options().id) return; /* This should not happen! (map frame to wrapper first) */ mjbots::moteus::Query::Result result = mjbots::moteus::Query::Parse(rx_frame.data, rx_frame.size); @@ -59,7 +71,7 @@ void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) /* create CANFD frame*/ mjbots::moteus::CanFdFrame can_fd_frame = moteus_controller_.MakeStop(); - /* Copy data from CANFD frame to CAN frame*/ + /* Copy data from CANFD frame to TX CAN Pi3hat frame */ tx_frame.id = can_fd_frame.arbitration_id; tx_frame.bus = can_fd_frame.bus; tx_frame.size = can_fd_frame.size; @@ -68,26 +80,27 @@ void MoteusWrapper::init_to_tx_frame(CanFrame& tx_frame) int MoteusWrapper::get_id_from_rx_frame(const CanFrame& rx_frame) { + /* Get real motor if from RX CAN Pi3hat frame */ return ((rx_frame.id>> 8) & 0x7f); } std::unique_ptr controller_interface::make_moteus_wrapper(const ControllerParameters& params) { - /* moteus options */ + /* Moteus options */ using mjbots::moteus::Controller; using controller_interface::MoteusWrapper; Controller::Options moteus_options; moteus_options.bus = params.bus_; moteus_options.id = params.id_; - /* moteus command format (it will be copied to wrapper) */ + /* Moteus command format (it will be copied to wrapper) */ mjbots::moteus::PositionMode::Format format; format.feedforward_torque = mjbots::moteus::kFloat; format.maximum_torque = mjbots::moteus::kFloat; format.velocity_limit= mjbots::moteus::kFloat; moteus_options.position_format = format; - /* moteus command (it will be copied to wrapper) */ + /* Moteus command (it will be copied to wrapper) */ mjbots::moteus::PositionMode::Command moteus_command; moteus_command.maximum_torque = params.torque_max_; //moteus_command.velocity_limit = params.velocity_max_; // Creates error, check later diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp index 5947f02f..d412e34b 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/imu_transform/IMUTransform.cpp @@ -1,5 +1,17 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "imu_transform/IMUTransform.hpp" -//#include "../../include/imu_transform/IMUTransform.hpp" using namespace IMU; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 76bea1e9..07d0d2ca 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -1,5 +1,17 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" -//#include "../include/pi3hat_hardware_interface/pi3hat_hardware_interface.hpp" using namespace pi3hat_hardware_interface; using namespace controller_interface; diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp index 9fbf2d25..fb7a5d51 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/double_moteus_bridge_test.cpp @@ -1,7 +1,19 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "controllers/Controllers.hpp" #include "3rd_libs/pi3hat/pi3hat.h" #include "3rd_libs/pi3hat/realtime.h" -//#include "../../include/3rd_libs/pi3hat/realtime.h" #include #include #include diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp index 10220b9f..c6d5167c 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/simple_moteus_test.cpp @@ -1,3 +1,16 @@ +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ + +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ + + #include "3rd_libs/pi3hat/pi3hat.h" #include "3rd_libs/pi3hat/realtime.h" #include "3rd_libs/moteus/moteus.h" diff --git a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp index e8cfa464..2a2383ca 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/test/controllers/moteus/single_moteus_bridge_test.cpp @@ -1,7 +1,17 @@ -#include "controllers/Controllers.hpp" +/* + * Pi3hat Hardware Interface for ROS2 control framework + * Copyright (C) 2024 KNR-Melson team + * + * Licensed under the GNU GENERAL PUBLIC LICENSE, Version 3.0 (the "License"); + * You may obtain a copy of the License at + * . + * + */ +/* Author: Bartłomiej Krajewski (https://github.com/BartlomiejK2) */ +#include "controllers/Controllers.hpp" #include "3rd_libs/pi3hat/pi3hat.h" #include "3rd_libs/pi3hat/realtime.h" #include From ea096aac361f08ba5b83a6006540c096dbb75e43 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 17:28:05 +0200 Subject: [PATCH 135/137] Removed commented code --- .../src/pi3hat_hardware_interface.cpp | 45 ------------------- 1 file changed, 45 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp index 07d0d2ca..e2e04e8e 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp +++ b/src/meldog_hardware/pi3hat_hardware_interface/src/pi3hat_hardware_interface.cpp @@ -185,40 +185,7 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_configure(const r hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_activate(const rclcpp_lifecycle::State &previous_state) { - /* Set all commands, states and transmission passthrough to start state */ - // for(int i = 0; i < joint_controller_number_; ++i) - // { - // controller_commands_[i].position_ = 0; - // controller_commands_[i].velocity_ = 0; - // controller_commands_[i].torque_ = 0; - - // controller_states_[i].position_ = 0; - // controller_states_[i].velocity_ = 0; - // controller_states_[i].torque_ = 0; - // controller_states_[i].fault = 0; - // controller_states_[i].temperature_ = 0; - - // controller_transmission_passthrough_[i].position_ = 0; - // controller_transmission_passthrough_[i].velocity_ = 0; - // controller_transmission_passthrough_[i].torque_ = 0; - - - // joint_commands_[i].position_ = 0; - // joint_commands_[i].velocity_ = 0; - // joint_commands_[i].torque_ = 0; - - // joint_states_[i].position_ = 0; - // joint_states_[i].velocity_ = 0; - // joint_states_[i].torque_ = 0; - // joint_states_[i].fault = 0; - // joint_states_[i].temperature_ = 0; - - // joint_transmission_passthrough_[i].position_ = 0; - // joint_transmission_passthrough_[i].velocity_ = 0; - // joint_transmission_passthrough_[i].torque_ = 0; - // } - /* Make start from actual motor position to 0.0 (offset included in controller bridges) */ RCLCPP_INFO(*logger_, "Motors reaching starting position!"); @@ -241,12 +208,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const controller_commands_[i].velocity_ = 0; controller_commands_[i].torque_ = 0; - controller_states_[i].position_ = 0; - controller_states_[i].velocity_ = 0; - controller_states_[i].torque_ = 0; - controller_states_[i].fault = 0; - controller_states_[i].temperature_ = 0; - controller_transmission_passthrough_[i].position_ = 0; controller_transmission_passthrough_[i].velocity_ = 0; controller_transmission_passthrough_[i].torque_ = 0; @@ -256,12 +217,6 @@ hardware_interface::CallbackReturn Pi3HatHardwareInterface::on_deactivate(const joint_commands_[i].velocity_ = 0; joint_commands_[i].torque_ = 0; - joint_states_[i].position_ = 0; - joint_states_[i].velocity_ = 0; - joint_states_[i].torque_ = 0; - joint_states_[i].fault = 0; - joint_states_[i].temperature_ = 0; - joint_transmission_passthrough_[i].position_ = 0; joint_transmission_passthrough_[i].velocity_ = 0; joint_transmission_passthrough_[i].torque_ = 0; From f6aa599f5c5606f45d8c0ca70ae9cd6907435c84 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 17:43:31 +0200 Subject: [PATCH 136/137] Changed LICENSE --- .../pi3hat_hardware_interface/LICENSE | 875 ++++++++++++++---- 1 file changed, 674 insertions(+), 201 deletions(-) diff --git a/src/meldog_hardware/pi3hat_hardware_interface/LICENSE b/src/meldog_hardware/pi3hat_hardware_interface/LICENSE index d6456956..1cfdbce8 100644 --- a/src/meldog_hardware/pi3hat_hardware_interface/LICENSE +++ b/src/meldog_hardware/pi3hat_hardware_interface/LICENSE @@ -1,202 +1,675 @@ - Apache License - Version 2.0, January 2004 - http://www.apache.org/licenses/ - - TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION - - 1. Definitions. - - "License" shall mean the terms and conditions for use, reproduction, - and distribution as defined by Sections 1 through 9 of this document. - - "Licensor" shall mean the copyright owner or entity authorized by - the copyright owner that is granting the License. - - "Legal Entity" shall mean the union of the acting entity and all - other entities that control, are controlled by, or are under common - control with that entity. For the purposes of this definition, - "control" means (i) the power, direct or indirect, to cause the - direction or management of such entity, whether by contract or - otherwise, or (ii) ownership of fifty percent (50%) or more of the - outstanding shares, or (iii) beneficial ownership of such entity. - - "You" (or "Your") shall mean an individual or Legal Entity - exercising permissions granted by this License. - - "Source" form shall mean the preferred form for making modifications, - including but not limited to software source code, documentation - source, and configuration files. - - "Object" form shall mean any form resulting from mechanical - transformation or translation of a Source form, including but - not limited to compiled object code, generated documentation, - and conversions to other media types. - - "Work" shall mean the work of authorship, whether in Source or - Object form, made available under the License, as indicated by a - copyright notice that is included in or attached to the work - (an example is provided in the Appendix below). - - "Derivative Works" shall mean any work, whether in Source or Object - form, that is based on (or derived from) the Work and for which the - editorial revisions, annotations, elaborations, or other modifications - represent, as a whole, an original work of authorship. For the purposes - of this License, Derivative Works shall not include works that remain - separable from, or merely link (or bind by name) to the interfaces of, - the Work and Derivative Works thereof. - - "Contribution" shall mean any work of authorship, including - the original version of the Work and any modifications or additions - to that Work or Derivative Works thereof, that is intentionally - submitted to Licensor for inclusion in the Work by the copyright owner - or by an individual or Legal Entity authorized to submit on behalf of - the copyright owner. For the purposes of this definition, "submitted" - means any form of electronic, verbal, or written communication sent - to the Licensor or its representatives, including but not limited to - communication on electronic mailing lists, source code control systems, - and issue tracking systems that are managed by, or on behalf of, the - Licensor for the purpose of discussing and improving the Work, but - excluding communication that is conspicuously marked or otherwise - designated in writing by the copyright owner as "Not a Contribution." - - "Contributor" shall mean Licensor and any individual or Legal Entity - on behalf of whom a Contribution has been received by Licensor and - subsequently incorporated within the Work. - - 2. Grant of Copyright License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - copyright license to reproduce, prepare Derivative Works of, - publicly display, publicly perform, sublicense, and distribute the - Work and such Derivative Works in Source or Object form. - - 3. Grant of Patent License. Subject to the terms and conditions of - this License, each Contributor hereby grants to You a perpetual, - worldwide, non-exclusive, no-charge, royalty-free, irrevocable - (except as stated in this section) patent license to make, have made, - use, offer to sell, sell, import, and otherwise transfer the Work, - where such license applies only to those patent claims licensable - by such Contributor that are necessarily infringed by their - Contribution(s) alone or by combination of their Contribution(s) - with the Work to which such Contribution(s) was submitted. If You - institute patent litigation against any entity (including a - cross-claim or counterclaim in a lawsuit) alleging that the Work - or a Contribution incorporated within the Work constitutes direct - or contributory patent infringement, then any patent licenses - granted to You under this License for that Work shall terminate - as of the date such litigation is filed. - - 4. Redistribution. You may reproduce and distribute copies of the - Work or Derivative Works thereof in any medium, with or without - modifications, and in Source or Object form, provided that You - meet the following conditions: - - (a) You must give any other recipients of the Work or - Derivative Works a copy of this License; and - - (b) You must cause any modified files to carry prominent notices - stating that You changed the files; and - - (c) You must retain, in the Source form of any Derivative Works - that You distribute, all copyright, patent, trademark, and - attribution notices from the Source form of the Work, - excluding those notices that do not pertain to any part of - the Derivative Works; and - - (d) If the Work includes a "NOTICE" text file as part of its - distribution, then any Derivative Works that You distribute must - include a readable copy of the attribution notices contained - within such NOTICE file, excluding those notices that do not - pertain to any part of the Derivative Works, in at least one - of the following places: within a NOTICE text file distributed - as part of the Derivative Works; within the Source form or - documentation, if provided along with the Derivative Works; or, - within a display generated by the Derivative Works, if and - wherever such third-party notices normally appear. The contents - of the NOTICE file are for informational purposes only and - do not modify the License. You may add Your own attribution - notices within Derivative Works that You distribute, alongside - or as an addendum to the NOTICE text from the Work, provided - that such additional attribution notices cannot be construed - as modifying the License. - - You may add Your own copyright statement to Your modifications and - may provide additional or different license terms and conditions - for use, reproduction, or distribution of Your modifications, or - for any such Derivative Works as a whole, provided Your use, - reproduction, and distribution of the Work otherwise complies with - the conditions stated in this License. - - 5. Submission of Contributions. Unless You explicitly state otherwise, - any Contribution intentionally submitted for inclusion in the Work - by You to the Licensor shall be under the terms and conditions of - this License, without any additional terms or conditions. - Notwithstanding the above, nothing herein shall supersede or modify - the terms of any separate license agreement you may have executed - with Licensor regarding such Contributions. - - 6. Trademarks. This License does not grant permission to use the trade - names, trademarks, service marks, or product names of the Licensor, - except as required for reasonable and customary use in describing the - origin of the Work and reproducing the content of the NOTICE file. - - 7. Disclaimer of Warranty. Unless required by applicable law or - agreed to in writing, Licensor provides the Work (and each - Contributor provides its Contributions) on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or - implied, including, without limitation, any warranties or conditions - of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A - PARTICULAR PURPOSE. You are solely responsible for determining the - appropriateness of using or redistributing the Work and assume any - risks associated with Your exercise of permissions under this License. - - 8. Limitation of Liability. In no event and under no legal theory, - whether in tort (including negligence), contract, or otherwise, - unless required by applicable law (such as deliberate and grossly - negligent acts) or agreed to in writing, shall any Contributor be - liable to You for damages, including any direct, indirect, special, - incidental, or consequential damages of any character arising as a - result of this License or out of the use or inability to use the - Work (including but not limited to damages for loss of goodwill, - work stoppage, computer failure or malfunction, or any and all - other commercial damages or losses), even if such Contributor - has been advised of the possibility of such damages. - - 9. Accepting Warranty or Additional Liability. While redistributing - the Work or Derivative Works thereof, You may choose to offer, - and charge a fee for, acceptance of support, warranty, indemnity, - or other liability obligations and/or rights consistent with this - License. However, in accepting such obligations, You may act only - on Your own behalf and on Your sole responsibility, not on behalf - of any other Contributor, and only if You agree to indemnify, - defend, and hold each Contributor harmless for any liability - incurred by, or claims asserted against, such Contributor by reason - of your accepting any such warranty or additional liability. - - END OF TERMS AND CONDITIONS - - APPENDIX: How to apply the Apache License to your work. - - To apply the Apache License to your work, attach the following - boilerplate notice, with the fields enclosed by brackets "[]" - replaced with your own identifying information. (Don't include - the brackets!) The text should be enclosed in the appropriate - comment syntax for the file format. We also recommend that a - file or class name and description of purpose be included on the - same "printed page" as the copyright notice for easier - identification within third-party archives. - - Copyright [yyyy] [name of copyright owner] - - Licensed under the Apache License, Version 2.0 (the "License"); - you may not use this file except in compliance with the License. - You may obtain a copy of the License at - - http://www.apache.org/licenses/LICENSE-2.0 - - Unless required by applicable law or agreed to in writing, software - distributed under the License is distributed on an "AS IS" BASIS, - WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. - See the License for the specific language governing permissions and - limitations under the License. + GNU GENERAL PUBLIC LICENSE + Version 3, 29 June 2007 + + Copyright (C) 2007 Free Software Foundation, Inc. + Everyone is permitted to copy and distribute verbatim copies + of this license document, but changing it is not allowed. + + Preamble + + The GNU General Public License is a free, copyleft license for +software and other kinds of works. + + The licenses for most software and other practical works are designed +to take away your freedom to share and change the works. By contrast, +the GNU General Public License is intended to guarantee your freedom to +share and change all versions of a program--to make sure it remains free +software for all its users. We, the Free Software Foundation, use the +GNU General Public License for most of our software; it applies also to +any other work released this way by its authors. You can apply it to +your programs, too. + + When we speak of free software, we are referring to freedom, not +price. Our General Public Licenses are designed to make sure that you +have the freedom to distribute copies of free software (and charge for +them if you wish), that you receive source code or can get it if you +want it, that you can change the software or use pieces of it in new +free programs, and that you know you can do these things. + + To protect your rights, we need to prevent others from denying you +these rights or asking you to surrender the rights. Therefore, you have +certain responsibilities if you distribute copies of the software, or if +you modify it: responsibilities to respect the freedom of others. + + For example, if you distribute copies of such a program, whether +gratis or for a fee, you must pass on to the recipients the same +freedoms that you received. You must make sure that they, too, receive +or can get the source code. And you must show them these terms so they +know their rights. + + Developers that use the GNU GPL protect your rights with two steps: +(1) assert copyright on the software, and (2) offer you this License +giving you legal permission to copy, distribute and/or modify it. + + For the developers' and authors' protection, the GPL clearly explains +that there is no warranty for this free software. For both users' and +authors' sake, the GPL requires that modified versions be marked as +changed, so that their problems will not be attributed erroneously to +authors of previous versions. + + Some devices are designed to deny users access to install or run +modified versions of the software inside them, although the manufacturer +can do so. This is fundamentally incompatible with the aim of +protecting users' freedom to change the software. The systematic +pattern of such abuse occurs in the area of products for individuals to +use, which is precisely where it is most unacceptable. Therefore, we +have designed this version of the GPL to prohibit the practice for those +products. If such problems arise substantially in other domains, we +stand ready to extend this provision to those domains in future versions +of the GPL, as needed to protect the freedom of users. + + Finally, every program is threatened constantly by software patents. +States should not allow patents to restrict development and use of +software on general-purpose computers, but in those that do, we wish to +avoid the special danger that patents applied to a free program could +make it effectively proprietary. To prevent this, the GPL assures that +patents cannot be used to render the program non-free. + + The precise terms and conditions for copying, distribution and +modification follow. + + TERMS AND CONDITIONS + + 0. Definitions. + + "This License" refers to version 3 of the GNU General Public License. + + "Copyright" also means copyright-like laws that apply to other kinds of +works, such as semiconductor masks. + + "The Program" refers to any copyrightable work licensed under this +License. Each licensee is addressed as "you". "Licensees" and +"recipients" may be individuals or organizations. + + To "modify" a work means to copy from or adapt all or part of the work +in a fashion requiring copyright permission, other than the making of an +exact copy. The resulting work is called a "modified version" of the +earlier work or a work "based on" the earlier work. + + A "covered work" means either the unmodified Program or a work based +on the Program. + + To "propagate" a work means to do anything with it that, without +permission, would make you directly or secondarily liable for +infringement under applicable copyright law, except executing it on a +computer or modifying a private copy. Propagation includes copying, +distribution (with or without modification), making available to the +public, and in some countries other activities as well. + + To "convey" a work means any kind of propagation that enables other +parties to make or receive copies. Mere interaction with a user through +a computer network, with no transfer of a copy, is not conveying. + + An interactive user interface displays "Appropriate Legal Notices" +to the extent that it includes a convenient and prominently visible +feature that (1) displays an appropriate copyright notice, and (2) +tells the user that there is no warranty for the work (except to the +extent that warranties are provided), that licensees may convey the +work under this License, and how to view a copy of this License. If +the interface presents a list of user commands or options, such as a +menu, a prominent item in the list meets this criterion. + + 1. Source Code. + + The "source code" for a work means the preferred form of the work +for making modifications to it. "Object code" means any non-source +form of a work. + + A "Standard Interface" means an interface that either is an official +standard defined by a recognized standards body, or, in the case of +interfaces specified for a particular programming language, one that +is widely used among developers working in that language. + + The "System Libraries" of an executable work include anything, other +than the work as a whole, that (a) is included in the normal form of +packaging a Major Component, but which is not part of that Major +Component, and (b) serves only to enable use of the work with that +Major Component, or to implement a Standard Interface for which an +implementation is available to the public in source code form. A +"Major Component", in this context, means a major essential component +(kernel, window system, and so on) of the specific operating system +(if any) on which the executable work runs, or a compiler used to +produce the work, or an object code interpreter used to run it. + + The "Corresponding Source" for a work in object code form means all +the source code needed to generate, install, and (for an executable +work) run the object code and to modify the work, including scripts to +control those activities. However, it does not include the work's +System Libraries, or general-purpose tools or generally available free +programs which are used unmodified in performing those activities but +which are not part of the work. For example, Corresponding Source +includes interface definition files associated with source files for +the work, and the source code for shared libraries and dynamically +linked subprograms that the work is specifically designed to require, +such as by intimate data communication or control flow between those +subprograms and other parts of the work. + + The Corresponding Source need not include anything that users +can regenerate automatically from other parts of the Corresponding +Source. + + The Corresponding Source for a work in source code form is that +same work. + + 2. Basic Permissions. + + All rights granted under this License are granted for the term of +copyright on the Program, and are irrevocable provided the stated +conditions are met. This License explicitly affirms your unlimited +permission to run the unmodified Program. The output from running a +covered work is covered by this License only if the output, given its +content, constitutes a covered work. This License acknowledges your +rights of fair use or other equivalent, as provided by copyright law. + + You may make, run and propagate covered works that you do not +convey, without conditions so long as your license otherwise remains +in force. You may convey covered works to others for the sole purpose +of having them make modifications exclusively for you, or provide you +with facilities for running those works, provided that you comply with +the terms of this License in conveying all material for which you do +not control copyright. Those thus making or running the covered works +for you must do so exclusively on your behalf, under your direction +and control, on terms that prohibit them from making any copies of +your copyrighted material outside their relationship with you. + + Conveying under any other circumstances is permitted solely under +the conditions stated below. Sublicensing is not allowed; section 10 +makes it unnecessary. + + 3. Protecting Users' Legal Rights From Anti-Circumvention Law. + + No covered work shall be deemed part of an effective technological +measure under any applicable law fulfilling obligations under article +11 of the WIPO copyright treaty adopted on 20 December 1996, or +similar laws prohibiting or restricting circumvention of such +measures. + + When you convey a covered work, you waive any legal power to forbid +circumvention of technological measures to the extent such circumvention +is effected by exercising rights under this License with respect to +the covered work, and you disclaim any intention to limit operation or +modification of the work as a means of enforcing, against the work's +users, your or third parties' legal rights to forbid circumvention of +technological measures. + + 4. Conveying Verbatim Copies. + + You may convey verbatim copies of the Program's source code as you +receive it, in any medium, provided that you conspicuously and +appropriately publish on each copy an appropriate copyright notice; +keep intact all notices stating that this License and any +non-permissive terms added in accord with section 7 apply to the code; +keep intact all notices of the absence of any warranty; and give all +recipients a copy of this License along with the Program. + + You may charge any price or no price for each copy that you convey, +and you may offer support or warranty protection for a fee. + + 5. Conveying Modified Source Versions. + + You may convey a work based on the Program, or the modifications to +produce it from the Program, in the form of source code under the +terms of section 4, provided that you also meet all of these conditions: + + a) The work must carry prominent notices stating that you modified + it, and giving a relevant date. + + b) The work must carry prominent notices stating that it is + released under this License and any conditions added under section + 7. This requirement modifies the requirement in section 4 to + "keep intact all notices". + + c) You must license the entire work, as a whole, under this + License to anyone who comes into possession of a copy. This + License will therefore apply, along with any applicable section 7 + additional terms, to the whole of the work, and all its parts, + regardless of how they are packaged. This License gives no + permission to license the work in any other way, but it does not + invalidate such permission if you have separately received it. + + d) If the work has interactive user interfaces, each must display + Appropriate Legal Notices; however, if the Program has interactive + interfaces that do not display Appropriate Legal Notices, your + work need not make them do so. + + A compilation of a covered work with other separate and independent +works, which are not by their nature extensions of the covered work, +and which are not combined with it such as to form a larger program, +in or on a volume of a storage or distribution medium, is called an +"aggregate" if the compilation and its resulting copyright are not +used to limit the access or legal rights of the compilation's users +beyond what the individual works permit. Inclusion of a covered work +in an aggregate does not cause this License to apply to the other +parts of the aggregate. + + 6. Conveying Non-Source Forms. + + You may convey a covered work in object code form under the terms +of sections 4 and 5, provided that you also convey the +machine-readable Corresponding Source under the terms of this License, +in one of these ways: + + a) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by the + Corresponding Source fixed on a durable physical medium + customarily used for software interchange. + + b) Convey the object code in, or embodied in, a physical product + (including a physical distribution medium), accompanied by a + written offer, valid for at least three years and valid for as + long as you offer spare parts or customer support for that product + model, to give anyone who possesses the object code either (1) a + copy of the Corresponding Source for all the software in the + product that is covered by this License, on a durable physical + medium customarily used for software interchange, for a price no + more than your reasonable cost of physically performing this + conveying of source, or (2) access to copy the + Corresponding Source from a network server at no charge. + + c) Convey individual copies of the object code with a copy of the + written offer to provide the Corresponding Source. This + alternative is allowed only occasionally and noncommercially, and + only if you received the object code with such an offer, in accord + with subsection 6b. + + d) Convey the object code by offering access from a designated + place (gratis or for a charge), and offer equivalent access to the + Corresponding Source in the same way through the same place at no + further charge. You need not require recipients to copy the + Corresponding Source along with the object code. If the place to + copy the object code is a network server, the Corresponding Source + may be on a different server (operated by you or a third party) + that supports equivalent copying facilities, provided you maintain + clear directions next to the object code saying where to find the + Corresponding Source. Regardless of what server hosts the + Corresponding Source, you remain obligated to ensure that it is + available for as long as needed to satisfy these requirements. + + e) Convey the object code using peer-to-peer transmission, provided + you inform other peers where the object code and Corresponding + Source of the work are being offered to the general public at no + charge under subsection 6d. + + A separable portion of the object code, whose source code is excluded +from the Corresponding Source as a System Library, need not be +included in conveying the object code work. + + A "User Product" is either (1) a "consumer product", which means any +tangible personal property which is normally used for personal, family, +or household purposes, or (2) anything designed or sold for incorporation +into a dwelling. In determining whether a product is a consumer product, +doubtful cases shall be resolved in favor of coverage. For a particular +product received by a particular user, "normally used" refers to a +typical or common use of that class of product, regardless of the status +of the particular user or of the way in which the particular user +actually uses, or expects or is expected to use, the product. A product +is a consumer product regardless of whether the product has substantial +commercial, industrial or non-consumer uses, unless such uses represent +the only significant mode of use of the product. + + "Installation Information" for a User Product means any methods, +procedures, authorization keys, or other information required to install +and execute modified versions of a covered work in that User Product from +a modified version of its Corresponding Source. The information must +suffice to ensure that the continued functioning of the modified object +code is in no case prevented or interfered with solely because +modification has been made. + + If you convey an object code work under this section in, or with, or +specifically for use in, a User Product, and the conveying occurs as +part of a transaction in which the right of possession and use of the +User Product is transferred to the recipient in perpetuity or for a +fixed term (regardless of how the transaction is characterized), the +Corresponding Source conveyed under this section must be accompanied +by the Installation Information. But this requirement does not apply +if neither you nor any third party retains the ability to install +modified object code on the User Product (for example, the work has +been installed in ROM). + + The requirement to provide Installation Information does not include a +requirement to continue to provide support service, warranty, or updates +for a work that has been modified or installed by the recipient, or for +the User Product in which it has been modified or installed. Access to a +network may be denied when the modification itself materially and +adversely affects the operation of the network or violates the rules and +protocols for communication across the network. + + Corresponding Source conveyed, and Installation Information provided, +in accord with this section must be in a format that is publicly +documented (and with an implementation available to the public in +source code form), and must require no special password or key for +unpacking, reading or copying. + + 7. Additional Terms. + + "Additional permissions" are terms that supplement the terms of this +License by making exceptions from one or more of its conditions. +Additional permissions that are applicable to the entire Program shall +be treated as though they were included in this License, to the extent +that they are valid under applicable law. If additional permissions +apply only to part of the Program, that part may be used separately +under those permissions, but the entire Program remains governed by +this License without regard to the additional permissions. + + When you convey a copy of a covered work, you may at your option +remove any additional permissions from that copy, or from any part of +it. (Additional permissions may be written to require their own +removal in certain cases when you modify the work.) You may place +additional permissions on material, added by you to a covered work, +for which you have or can give appropriate copyright permission. + + Notwithstanding any other provision of this License, for material you +add to a covered work, you may (if authorized by the copyright holders of +that material) supplement the terms of this License with terms: + + a) Disclaiming warranty or limiting liability differently from the + terms of sections 15 and 16 of this License; or + + b) Requiring preservation of specified reasonable legal notices or + author attributions in that material or in the Appropriate Legal + Notices displayed by works containing it; or + + c) Prohibiting misrepresentation of the origin of that material, or + requiring that modified versions of such material be marked in + reasonable ways as different from the original version; or + + d) Limiting the use for publicity purposes of names of licensors or + authors of the material; or + + e) Declining to grant rights under trademark law for use of some + trade names, trademarks, or service marks; or + + f) Requiring indemnification of licensors and authors of that + material by anyone who conveys the material (or modified versions of + it) with contractual assumptions of liability to the recipient, for + any liability that these contractual assumptions directly impose on + those licensors and authors. + + All other non-permissive additional terms are considered "further +restrictions" within the meaning of section 10. If the Program as you +received it, or any part of it, contains a notice stating that it is +governed by this License along with a term that is a further +restriction, you may remove that term. If a license document contains +a further restriction but permits relicensing or conveying under this +License, you may add to a covered work material governed by the terms +of that license document, provided that the further restriction does +not survive such relicensing or conveying. + + If you add terms to a covered work in accord with this section, you +must place, in the relevant source files, a statement of the +additional terms that apply to those files, or a notice indicating +where to find the applicable terms. + + Additional terms, permissive or non-permissive, may be stated in the +form of a separately written license, or stated as exceptions; +the above requirements apply either way. + + 8. Termination. + + You may not propagate or modify a covered work except as expressly +provided under this License. Any attempt otherwise to propagate or +modify it is void, and will automatically terminate your rights under +this License (including any patent licenses granted under the third +paragraph of section 11). + + However, if you cease all violation of this License, then your +license from a particular copyright holder is reinstated (a) +provisionally, unless and until the copyright holder explicitly and +finally terminates your license, and (b) permanently, if the copyright +holder fails to notify you of the violation by some reasonable means +prior to 60 days after the cessation. + + Moreover, your license from a particular copyright holder is +reinstated permanently if the copyright holder notifies you of the +violation by some reasonable means, this is the first time you have +received notice of violation of this License (for any work) from that +copyright holder, and you cure the violation prior to 30 days after +your receipt of the notice. + + Termination of your rights under this section does not terminate the +licenses of parties who have received copies or rights from you under +this License. If your rights have been terminated and not permanently +reinstated, you do not qualify to receive new licenses for the same +material under section 10. + + 9. Acceptance Not Required for Having Copies. + + You are not required to accept this License in order to receive or +run a copy of the Program. Ancillary propagation of a covered work +occurring solely as a consequence of using peer-to-peer transmission +to receive a copy likewise does not require acceptance. However, +nothing other than this License grants you permission to propagate or +modify any covered work. These actions infringe copyright if you do +not accept this License. Therefore, by modifying or propagating a +covered work, you indicate your acceptance of this License to do so. + + 10. Automatic Licensing of Downstream Recipients. + + Each time you convey a covered work, the recipient automatically +receives a license from the original licensors, to run, modify and +propagate that work, subject to this License. You are not responsible +for enforcing compliance by third parties with this License. + + An "entity transaction" is a transaction transferring control of an +organization, or substantially all assets of one, or subdividing an +organization, or merging organizations. If propagation of a covered +work results from an entity transaction, each party to that +transaction who receives a copy of the work also receives whatever +licenses to the work the party's predecessor in interest had or could +give under the previous paragraph, plus a right to possession of the +Corresponding Source of the work from the predecessor in interest, if +the predecessor has it or can get it with reasonable efforts. + + You may not impose any further restrictions on the exercise of the +rights granted or affirmed under this License. For example, you may +not impose a license fee, royalty, or other charge for exercise of +rights granted under this License, and you may not initiate litigation +(including a cross-claim or counterclaim in a lawsuit) alleging that +any patent claim is infringed by making, using, selling, offering for +sale, or importing the Program or any portion of it. + + 11. Patents. + + A "contributor" is a copyright holder who authorizes use under this +License of the Program or a work on which the Program is based. The +work thus licensed is called the contributor's "contributor version". + + A contributor's "essential patent claims" are all patent claims +owned or controlled by the contributor, whether already acquired or +hereafter acquired, that would be infringed by some manner, permitted +by this License, of making, using, or selling its contributor version, +but do not include claims that would be infringed only as a +consequence of further modification of the contributor version. For +purposes of this definition, "control" includes the right to grant +patent sublicenses in a manner consistent with the requirements of +this License. + + Each contributor grants you a non-exclusive, worldwide, royalty-free +patent license under the contributor's essential patent claims, to +make, use, sell, offer for sale, import and otherwise run, modify and +propagate the contents of its contributor version. + + In the following three paragraphs, a "patent license" is any express +agreement or commitment, however denominated, not to enforce a patent +(such as an express permission to practice a patent or covenant not to +sue for patent infringement). To "grant" such a patent license to a +party means to make such an agreement or commitment not to enforce a +patent against the party. + + If you convey a covered work, knowingly relying on a patent license, +and the Corresponding Source of the work is not available for anyone +to copy, free of charge and under the terms of this License, through a +publicly available network server or other readily accessible means, +then you must either (1) cause the Corresponding Source to be so +available, or (2) arrange to deprive yourself of the benefit of the +patent license for this particular work, or (3) arrange, in a manner +consistent with the requirements of this License, to extend the patent +license to downstream recipients. "Knowingly relying" means you have +actual knowledge that, but for the patent license, your conveying the +covered work in a country, or your recipient's use of the covered work +in a country, would infringe one or more identifiable patents in that +country that you have reason to believe are valid. + + If, pursuant to or in connection with a single transaction or +arrangement, you convey, or propagate by procuring conveyance of, a +covered work, and grant a patent license to some of the parties +receiving the covered work authorizing them to use, propagate, modify +or convey a specific copy of the covered work, then the patent license +you grant is automatically extended to all recipients of the covered +work and works based on it. + + A patent license is "discriminatory" if it does not include within +the scope of its coverage, prohibits the exercise of, or is +conditioned on the non-exercise of one or more of the rights that are +specifically granted under this License. You may not convey a covered +work if you are a party to an arrangement with a third party that is +in the business of distributing software, under which you make payment +to the third party based on the extent of your activity of conveying +the work, and under which the third party grants, to any of the +parties who would receive the covered work from you, a discriminatory +patent license (a) in connection with copies of the covered work +conveyed by you (or copies made from those copies), or (b) primarily +for and in connection with specific products or compilations that +contain the covered work, unless you entered into that arrangement, +or that patent license was granted, prior to 28 March 2007. + + Nothing in this License shall be construed as excluding or limiting +any implied license or other defenses to infringement that may +otherwise be available to you under applicable patent law. + + 12. No Surrender of Others' Freedom. + + If conditions are imposed on you (whether by court order, agreement or +otherwise) that contradict the conditions of this License, they do not +excuse you from the conditions of this License. If you cannot convey a +covered work so as to satisfy simultaneously your obligations under this +License and any other pertinent obligations, then as a consequence you may +not convey it at all. For example, if you agree to terms that obligate you +to collect a royalty for further conveying from those to whom you convey +the Program, the only way you could satisfy both those terms and this +License would be to refrain entirely from conveying the Program. + + 13. Use with the GNU Affero General Public License. + + Notwithstanding any other provision of this License, you have +permission to link or combine any covered work with a work licensed +under version 3 of the GNU Affero General Public License into a single +combined work, and to convey the resulting work. The terms of this +License will continue to apply to the part which is the covered work, +but the special requirements of the GNU Affero General Public License, +section 13, concerning interaction through a network will apply to the +combination as such. + + 14. Revised Versions of this License. + + The Free Software Foundation may publish revised and/or new versions of +the GNU General Public License from time to time. Such new versions will +be similar in spirit to the present version, but may differ in detail to +address new problems or concerns. + + Each version is given a distinguishing version number. If the +Program specifies that a certain numbered version of the GNU General +Public License "or any later version" applies to it, you have the +option of following the terms and conditions either of that numbered +version or of any later version published by the Free Software +Foundation. If the Program does not specify a version number of the +GNU General Public License, you may choose any version ever published +by the Free Software Foundation. + + If the Program specifies that a proxy can decide which future +versions of the GNU General Public License can be used, that proxy's +public statement of acceptance of a version permanently authorizes you +to choose that version for the Program. + + Later license versions may give you additional or different +permissions. However, no additional obligations are imposed on any +author or copyright holder as a result of your choosing to follow a +later version. + + 15. Disclaimer of Warranty. + + THERE IS NO WARRANTY FOR THE PROGRAM, TO THE EXTENT PERMITTED BY +APPLICABLE LAW. EXCEPT WHEN OTHERWISE STATED IN WRITING THE COPYRIGHT +HOLDERS AND/OR OTHER PARTIES PROVIDE THE PROGRAM "AS IS" WITHOUT WARRANTY +OF ANY KIND, EITHER EXPRESSED OR IMPLIED, INCLUDING, BUT NOT LIMITED TO, +THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR +PURPOSE. THE ENTIRE RISK AS TO THE QUALITY AND PERFORMANCE OF THE PROGRAM +IS WITH YOU. SHOULD THE PROGRAM PROVE DEFECTIVE, YOU ASSUME THE COST OF +ALL NECESSARY SERVICING, REPAIR OR CORRECTION. + + 16. Limitation of Liability. + + IN NO EVENT UNLESS REQUIRED BY APPLICABLE LAW OR AGREED TO IN WRITING +WILL ANY COPYRIGHT HOLDER, OR ANY OTHER PARTY WHO MODIFIES AND/OR CONVEYS +THE PROGRAM AS PERMITTED ABOVE, BE LIABLE TO YOU FOR DAMAGES, INCLUDING ANY +GENERAL, SPECIAL, INCIDENTAL OR CONSEQUENTIAL DAMAGES ARISING OUT OF THE +USE OR INABILITY TO USE THE PROGRAM (INCLUDING BUT NOT LIMITED TO LOSS OF +DATA OR DATA BEING RENDERED INACCURATE OR LOSSES SUSTAINED BY YOU OR THIRD +PARTIES OR A FAILURE OF THE PROGRAM TO OPERATE WITH ANY OTHER PROGRAMS), +EVEN IF SUCH HOLDER OR OTHER PARTY HAS BEEN ADVISED OF THE POSSIBILITY OF +SUCH DAMAGES. + + 17. Interpretation of Sections 15 and 16. + + If the disclaimer of warranty and limitation of liability provided +above cannot be given local legal effect according to their terms, +reviewing courts shall apply local law that most closely approximates +an absolute waiver of all civil liability in connection with the +Program, unless a warranty or assumption of liability accompanies a +copy of the Program in return for a fee. + + END OF TERMS AND CONDITIONS + + How to Apply These Terms to Your New Programs + + If you develop a new program, and you want it to be of the greatest +possible use to the public, the best way to achieve this is to make it +free software which everyone can redistribute and change under these terms. + + To do so, attach the following notices to the program. It is safest +to attach them to the start of each source file to most effectively +state the exclusion of warranty; and each file should have at least +the "copyright" line and a pointer to where the full notice is found. + + + Copyright (C) + + This program is free software: you can redistribute it and/or modify + it under the terms of the GNU General Public License as published by + the Free Software Foundation, either version 3 of the License, or + (at your option) any later version. + + This program is distributed in the hope that it will be useful, + but WITHOUT ANY WARRANTY; without even the implied warranty of + MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the + GNU General Public License for more details. + + You should have received a copy of the GNU General Public License + along with this program. If not, see . + +Also add information on how to contact you by electronic and paper mail. + + If the program does terminal interaction, make it output a short +notice like this when it starts in an interactive mode: + + Copyright (C) + This program comes with ABSOLUTELY NO WARRANTY; for details type `show w'. + This is free software, and you are welcome to redistribute it + under certain conditions; type `show c' for details. + +The hypothetical commands `show w' and `show c' should show the appropriate +parts of the General Public License. Of course, your program's commands +might be different; for a GUI interface, you would use an "about box". + + You should also get your employer (if you work as a programmer) or school, +if any, to sign a "copyright disclaimer" for the program, if necessary. +For more information on this, and how to apply and follow the GNU GPL, see +. + + The GNU General Public License does not permit incorporating your program +into proprietary programs. If your program is a subroutine library, you +may consider it more useful to permit linking proprietary applications with +the library. If this is what you want to do, use the GNU Lesser General +Public License instead of this License. But first, please read +. \ No newline at end of file From 2e9444adc95734c863367ac4bfd3055069b76bc8 Mon Sep 17 00:00:00 2001 From: Bartek Date: Fri, 9 Aug 2024 17:57:23 +0200 Subject: [PATCH 137/137] Added README.md --- src/meldog_hardware/pi3hat_hardware_interface/README.md | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 src/meldog_hardware/pi3hat_hardware_interface/README.md diff --git a/src/meldog_hardware/pi3hat_hardware_interface/README.md b/src/meldog_hardware/pi3hat_hardware_interface/README.md new file mode 100644 index 00000000..bbaac232 --- /dev/null +++ b/src/meldog_hardware/pi3hat_hardware_interface/README.md @@ -0,0 +1,3 @@ +# pi3hat_hardware_interface + +ROS2 control hardware interface for mjbots [pi3hat](https://mjbots.com/products/mjbots-pi3hat-r4-5). More information [here](https://github.com/BartlomiejK2/pi3hat_hardware_interface). \ No newline at end of file