diff --git a/.clang-format b/.clang-format new file mode 100644 index 0000000..191520d --- /dev/null +++ b/.clang-format @@ -0,0 +1,65 @@ +--- +BasedOnStyle: Google +AccessModifierOffset: -2 +ConstructorInitializerIndentWidth: 2 +AlignEscapedNewlinesLeft: false +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: false +AllowShortIfStatementsOnASingleLine: false +AllowShortLoopsOnASingleLine: false +AllowShortFunctionsOnASingleLine: None +AlwaysBreakTemplateDeclarations: true +AlwaysBreakBeforeMultilineStrings: false +BreakBeforeBinaryOperators: false +BreakBeforeTernaryOperators: false +BreakConstructorInitializersBeforeComma: true +BinPackParameters: true +ColumnLimit: 120 +ConstructorInitializerAllOnOneLineOrOnePerLine: true +DerivePointerBinding: false +PointerBindsToType: true +ExperimentalAutoDetectBinPacking: false +IndentCaseLabels: true +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCSpaceBeforeProtocolList: true +PenaltyBreakBeforeFirstCallParameter: 19 +PenaltyBreakComment: 60 +PenaltyBreakString: 1 +PenaltyBreakFirstLessLess: 1000 +PenaltyExcessCharacter: 1000 +PenaltyReturnTypeOnItsOwnLine: 90 +SpacesBeforeTrailingComments: 2 +Cpp11BracedListStyle: false +Standard: Auto +IndentWidth: 2 +TabWidth: 2 +UseTab: Never +IndentFunctionDeclarationAfterType: false +SpacesInParentheses: false +SpacesInAngles: false +SpaceInEmptyParentheses: false +SpacesInCStyleCastParentheses: false +SpaceAfterControlStatementKeyword: true +SpaceBeforeAssignmentOperators: true +ContinuationIndentWidth: 4 +SortIncludes: false +SpaceAfterCStyleCast: false + +# Configure each individual brace in BraceWrapping +BreakBeforeBraces: Custom + +# Control of individual brace wrapping cases +BraceWrapping: + AfterCaseLabel: 'true' + AfterClass: 'true' + AfterControlStatement: Never + AfterEnum : 'true' + AfterFunction : 'true' + AfterNamespace : 'true' + AfterStruct : 'true' + AfterUnion : 'true' + BeforeCatch : 'false' + BeforeElse : 'false' + IndentBraces : 'false' +... diff --git a/.pre-commit-config.yaml b/.pre-commit-config.yaml new file mode 100644 index 0000000..30c3e6c --- /dev/null +++ b/.pre-commit-config.yaml @@ -0,0 +1,128 @@ +# To use: +# +# pre-commit run -a +# +# Or: +# +# pre-commit install # (runs every time you commit in git) +# +# To update this file: +# +# pre-commit autoupdate +# +# See https://github.com/pre-commit/pre-commit + +repos: + # Standard hooks + - repo: https://github.com/pre-commit/pre-commit-hooks + rev: v5.0.0 + hooks: + - id: check-added-large-files + - id: check-ast + - id: check-case-conflict + - id: check-docstring-first + - id: check-merge-conflict + - id: check-symlinks + - id: check-xml + - id: check-yaml + - id: debug-statements + - id: end-of-file-fixer + - id: mixed-line-ending + - id: trailing-whitespace + - id: check-byte-order-marker # Forbid UTF-8 byte-order markers + + # Python hooks + - repo: https://github.com/asottile/pyupgrade + rev: v3.17.0 + hooks: + - id: pyupgrade + args: [--py36-plus] + + - repo: https://github.com/psf/black + rev: 24.8.0 + hooks: + - id: black + args: ["--line-length=100"] + + - repo: https://github.com/pycqa/pydocstyle + rev: 6.3.0 + hooks: + - id: pydocstyle + args: ["--ignore=D100,D101,D102,D103,D104,D105,D106,D107,D203,D212,D401,D404"] + + - repo: https://github.com/pycqa/flake8 + rev: 7.1.1 + hooks: + - id: flake8 + args: ["--ignore=E501,W503"] + + # CPP hooks + - repo: local + hooks: + - id: ament_cppcheck + name: ament_cppcheck + description: Static code analysis of C/C++ files. + stages: [pre-commit] + entry: env AMENT_CPPCHECK_ALLOW_SLOW_VERSIONS=1 ament_cppcheck + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + + + - repo: local + hooks: + - id: clang-format + name: clang-format + description: Format files with ClangFormat. + entry: clang-format-14 + language: system + files: \.(c|cc|cxx|cpp|frag|glsl|h|hpp|hxx|ih|ispc|ipp|java|js|m|proto|vert)$ + args: ['-fallback-style=none', '-i'] + - repo: local + hooks: + - id: ament_cpplint + name: ament_cpplint + description: Static code analysis of C/C++ files. + stages: [pre-commit] + entry: ament_cpplint + language: system + files: \.(h\+\+|h|hh|hxx|hpp|cuh|c|cc|cpp|cu|c\+\+|cxx|tpp|txx)$ + args: ["--linelength=120"] + + # Cmake hooks + - repo: local + hooks: + - id: ament_lint_cmake + name: ament_lint_cmake + description: Check format of CMakeLists.txt files. + stages: [pre-commit] + entry: ament_lint_cmake + language: system + files: CMakeLists.txt$ + + # Copyright - ament_copyright is somewhat broken for some licenses. We therefore disable it + #- repo: local + # hooks: + # - id: ament_copyright + # name: ament_copyright + # description: Check if copyright notice is available in all files. + # stages: [pre-commit] + # entry: ament_copyright + # language: system + + + # Docs - RestructuredText hooks + - repo: https://github.com/PyCQA/doc8 + rev: v1.1.2 + hooks: + - id: doc8 + args: ['--max-line-length=100', '--ignore=D001'] + + + # Spellcheck in comments and docs + # skipping of *.svg files is not working... + - repo: https://github.com/codespell-project/codespell + rev: v2.3.0 + hooks: + - id: codespell + args: ['--write-changes', '-L bootup,assertIn'] + exclude: \.(svg|pyc|drawio)$ diff --git a/CMakeLists.txt b/CMakeLists.txt index 935208a..4038425 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -29,11 +29,11 @@ target_compile_features(${PROJECT_NAME} PUBLIC cxx_std_17) ament_target_dependencies(${PROJECT_NAME} ethercat_sdk_master - + ) target_link_libraries(${PROJECT_NAME} - yaml-cpp + yaml-cpp ) diff --git a/README.md b/README.md index da0effa..b00204d 100644 --- a/README.md +++ b/README.md @@ -1,4 +1,4 @@ -# rsl_drive_sdk +# rsl_drive_sdk Software Development Kit for all RSL drives.\ The source code is released under [BSD 3](LICENSE). @@ -9,7 +9,7 @@ The source code is released under [BSD 3](LICENSE). | ---| --- | | Dynadrive | Fully supported, including calibration | | Halodi drives | Fully supported, including calibration | -| Other | There are additional, but not officially supported drives | +| Other | There are additional, but not officially supported drives | ## Dependencies @@ -18,15 +18,15 @@ The source code is released under [BSD 3](LICENSE). | Repository | URL | License | Content | | --- | - | --- | --- | -| ethercat_sdk_master | https://github.com/leggedrobotics/ethercat_sdk_master/tree/master/ethercat_sdk_master | BSD 3-Clause | Ethercat master implementation | -| soem_interface | https://github.com/leggedrobotics/soem_interface | GPLv3 | Wrapper around [SOEM](https://github.com/OpenEtherCATsociety/soem) | +| ethercat_sdk_master | https://github.com/leggedrobotics/ethercat_sdk_master/tree/master/ethercat_sdk_master | BSD 3-Clause | Ethercat master implementation | +| soem_interface | https://github.com/leggedrobotics/soem_interface | GPLv3 | Wrapper around [SOME](https://github.com/OpenEtherCATsociety/some) | | message_logger | https://github.com/leggedrobotics/message_logger.git | BSD 3-Clause | Simple logger | #### Installable via rosdep: | Repository | URL | License | Content | | --- | - | --- | --- | -| yaml_cpp_vendor |https://github.com/ros2/yaml_cpp_vendor | Apache 2.0 / MIT | yaml library | +| yaml_cpp_vendor |https://github.com/ros2/yaml_cpp_vendor | Apache 2.0 / MIT | yaml library | ## Usage diff --git a/include/rsl_drive_sdk/Command.hpp b/include/rsl_drive_sdk/Command.hpp index 5100c4a..18b3a26 100644 --- a/include/rsl_drive_sdk/Command.hpp +++ b/include/rsl_drive_sdk/Command.hpp @@ -2,29 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include +#include // any measurements #include @@ -32,11 +34,9 @@ #include "rsl_drive_sdk/mode/ModeEnum.hpp" #include "rsl_drive_sdk/mode/PidGains.hpp" - namespace rsl_drive_sdk { - //! Drive command. class Command { @@ -68,8 +68,8 @@ class Command Command(); virtual ~Command(); - const std::chrono::high_resolution_clock::time_point & getStamp() const; - void setStamp(const std::chrono::high_resolution_clock::time_point & stamp); + const std::chrono::high_resolution_clock::time_point& getStamp() const; + void setStamp(const std::chrono::high_resolution_clock::time_point& stamp); mode::ModeEnum getModeEnum() const; void setModeEnum(const mode::ModeEnum modeEnum); @@ -98,9 +98,9 @@ class Command double getJointTorque() const; void setJointTorque(const double jointTorque); - mode::PidGainsF & getPidGains(); - const mode::PidGainsF & getPidGains() const; - void setPidGains(const mode::PidGainsF & pidGains); + mode::PidGainsF& getPidGains(); + const mode::PidGainsF& getPidGains() const; + void setPidGains(const mode::PidGainsF& pidGains); /*! * Check if the command is valid: @@ -111,10 +111,9 @@ class Command */ bool isValid() const; - virtual std::string asString(const std::string & prefix = "") const; + virtual std::string asString(const std::string& prefix = "") const; }; -std::ostream & operator<<(std::ostream & out, const Command & command); - +std::ostream& operator<<(std::ostream& out, const Command& command); -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/Drive.hpp b/include/rsl_drive_sdk/Drive.hpp index a45a84d..9f5ff31 100644 --- a/include/rsl_drive_sdk/Drive.hpp +++ b/include/rsl_drive_sdk/Drive.hpp @@ -2,39 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include #include +#include +#include +#include // rsl_drive_sdk // soem_interface #include - // rsl_drive_sdk ethercat #include "rsl_drive_sdk/PdoTypeEnum.hpp" #include "rsl_drive_sdk/Statusword.hpp" @@ -53,11 +56,11 @@ #include "rsl_drive_sdk/fsm/StateMachine.hpp" #include "rsl_drive_sdk/usings.hpp" - namespace rsl_drive_sdk { -class DriveEthercatDevice : public ecat_master::EthercatDevice { +class DriveEthercatDevice : public ecat_master::EthercatDevice +{ protected: using RecMutex = std::recursive_mutex; using RecLock = std::lock_guard; @@ -67,19 +70,19 @@ class DriveEthercatDevice : public ecat_master::EthercatDevice { static constexpr double milliScaling_ = 1000.0; static constexpr double kiloScaling_ = 0.001; - static constexpr double timeScaling_ = milliScaling_; // ms + static constexpr double timeScaling_ = milliScaling_; // ms static constexpr double gainScaling_ = 1.0; - static constexpr double temperatureScaling_ = 100.0; // °cC (and offset) - static constexpr double motorVoltageScaling_ = 100.0; // cV - static constexpr double motorCurrentScaling_ = 1.0; // A - static constexpr double motorPositionScaling_ = 1.0; // rad - static constexpr double motorVelocityScaling_ = kiloScaling_ * rpmPerRadS_; // krpm - static constexpr double gearPositionScaling_ = 1.0; // W - static constexpr double gearVelocityScaling_ = 1.0; // W - static constexpr double jointPositionScaling_ = 1.0; // rad - static constexpr double jointVelocityScaling_ = rpmPerRadS_; // rpm - static constexpr double jointAccelerationScaling_ = rpmPerRadS_; // rpm/s - static constexpr double jointTorqueScaling_ = 1.0; // Nm + static constexpr double temperatureScaling_ = 100.0; // °cC (and offset) + static constexpr double motorVoltageScaling_ = 100.0; // cV + static constexpr double motorCurrentScaling_ = 1.0; // A + static constexpr double motorPositionScaling_ = 1.0; // rad + static constexpr double motorVelocityScaling_ = kiloScaling_ * rpmPerRadS_; // krpm + static constexpr double gearPositionScaling_ = 1.0; // W + static constexpr double gearVelocityScaling_ = 1.0; // W + static constexpr double jointPositionScaling_ = 1.0; // rad + static constexpr double jointVelocityScaling_ = rpmPerRadS_; // rpm + static constexpr double jointAccelerationScaling_ = rpmPerRadS_; // rpm/s + static constexpr double jointTorqueScaling_ = 1.0; // Nm static constexpr double timeScalingInv_ = 1.0 / timeScaling_; static constexpr double gainScalingInv_ = 1.0 / gainScaling_; @@ -97,7 +100,6 @@ class DriveEthercatDevice : public ecat_master::EthercatDevice { static constexpr double temperatureOffset_ = -55.0; - using PdoInfos = std::map; static std::map modeEnumToOdIndex_; @@ -121,45 +123,44 @@ class DriveEthercatDevice : public ecat_master::EthercatDevice { rsl_drive_sdk::configuration::Configuration config_; - - //! State machine of the device. + //! State machine of the device. fsm::StateMachine stateMachine_; void updateProcessReading(); bool statuswordRequested_ = false; void requestAndSetStatusword(); - void setStatusword(Statusword & statusword); + void setStatusword(Statusword& statusword); Statusword statusword_; std::multimap> readingCbs_; - //! Prioritized callbacks called when an error appeared. + //! Prioritized callbacks called when an error appeared. std::multimap> errorCbs_; - //! Prioritized callbacks called when an error disappeared. + //! Prioritized callbacks called when an error disappeared. std::multimap> errorRecoveredCbs_; - //! Prioritized callbacks called when an fatal appeared. + //! Prioritized callbacks called when an fatal appeared. std::multimap> fatalCbs_; - //! Prioritized callbacks called when an fatal disappeared. + //! Prioritized callbacks called when an fatal disappeared. std::multimap> fatalRecoveredCbs_; - //! Prioritized callbacks called when the device disconnected. + //! Prioritized callbacks called when the device disconnected. std::multimap> deviceDisconnectedCbs_; - //! Prioritized callbacks called when the device reconnected. + //! Prioritized callbacks called when the device reconnected. std::multimap> deviceReconnectedCbs_; public: typedef std::shared_ptr SharedPtr; - static SharedPtr deviceFromFile( - const std::string & fileName, const std::string & name, - const uint32_t address, const PdoTypeEnum pdoTypeEnum); - DriveEthercatDevice( - const uint32_t address, const std::string & name, - const PdoTypeEnum pdoTypeEnum); + static SharedPtr deviceFromFile(const std::string& fileName, const std::string& name, const uint32_t address, + const PdoTypeEnum pdoTypeEnum); + DriveEthercatDevice(const uint32_t address, const std::string& name, const PdoTypeEnum pdoTypeEnum); virtual ~DriveEthercatDevice() = default; uint32_t getAddress() const; - configuration::Configuration & getConfiguration() {return config_;} - void applyConfiguration(const configuration::Configuration & config); + configuration::Configuration& getConfiguration() + { + return config_; + } + void applyConfiguration(const configuration::Configuration& config); bool startup() override; void updateRead() override; void updateWrite() override; @@ -172,330 +173,308 @@ class DriveEthercatDevice : public ecat_master::EthercatDevice { void setState(const uint16_t state); bool waitForState(const uint16_t state); - PdoTypeEnum getCurrentPdoTypeEnum() const; PdoInfo getCurrentPdoInfo() const override; bool receivesGearAndJointEncoderTicks() const; std::string getName() const override; - //State machine. + // State machine. fsm::StateEnum getActiveStateEnum() const; fsm::StateEnum getGoalStateEnum() const; bool goalStateHasBeenReached() const; - bool setFSMGoalState( - const fsm::StateEnum goalStateEnum, const bool reachState, - const double timeout, const double checkingFrequency); + bool setFSMGoalState(const fsm::StateEnum goalStateEnum, const bool reachState, const double timeout, + const double checkingFrequency); void clearGoalStateEnum(); - // Drive info. - bool getDriveModel(std::string & model); - bool getDriveInfoSerialNumber(std::string & serialNumber); - bool setDriveInfoSerialNumber(const std::string & serialNumber); - bool getDriveInfoName(std::string & name); - bool setDriveInfoName(const std::string & name); - bool getDriveInfoId(uint16_t & id); + // Drive info. + bool getDriveModel(std::string& model); + bool getDriveInfoSerialNumber(std::string& serialNumber); + bool setDriveInfoSerialNumber(const std::string& serialNumber); + bool getDriveInfoName(std::string& name); + bool setDriveInfoName(const std::string& name); + bool getDriveInfoId(uint16_t& id); bool setDriveInfoId(const uint16_t id); - bool getDriveInfoBootloaderVersion(rsl_drive_sdk::common::Version & bootloaderVersion); - bool setDriveInfoBootloaderVersion(const rsl_drive_sdk::common::Version & bootloaderVersion); - bool getDriveInfoFirmwareVersion(rsl_drive_sdk::common::Version & firmwareVersion); - bool getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareInfo & firmwareInfo); - bool getGearboxRatio(float & ratio); - bool getBuildInfo(rsl_drive_sdk::common::BuildInfo & buildInfo); - bool getDriveType(std::string & type); - - // Flash storage. + bool getDriveInfoBootloaderVersion(rsl_drive_sdk::common::Version& bootloaderVersion); + bool setDriveInfoBootloaderVersion(const rsl_drive_sdk::common::Version& bootloaderVersion); + bool getDriveInfoFirmwareVersion(rsl_drive_sdk::common::Version& firmwareVersion); + bool getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareInfo& firmwareInfo); + bool getGearboxRatio(float& ratio); + bool getBuildInfo(rsl_drive_sdk::common::BuildInfo& buildInfo); + bool getDriveType(std::string& type); + + // Flash storage. bool eraseFlashStorage(); bool resetFlashStorageSections(const uint16_t flashStorageSections); - // Calibration. - bool getCalibrationState( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::CalibrationState & calibrationState); + // Calibration. + bool getCalibrationState(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::CalibrationState& calibrationState); bool sendCalibrationGearAndJointEncoderHomingNewJointPosition(const double newJointPosition); bool startCalibration(const rsl_drive_sdk::calibration::CalibrationModeEnum calibrationModeEnum); - bool calibrationIsRunning(bool & running); - bool getCalibrationMotorEncoderOffset( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::MotorEncoderOffset & motorEncoderOffset); + bool calibrationIsRunning(bool& running); + bool getCalibrationMotorEncoderOffset(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::MotorEncoderOffset& motorEncoderOffset); bool getCalibrationMotorEncoderParameters( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::MotorEncoderParameters & motorEncoderParameters); + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::MotorEncoderParameters& motorEncoderParameters); bool getCalibrationGearJointEncoderOffset( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::GearJointEncoderOffset & gearJointEncoderOffset); + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::GearJointEncoderOffset& gearJointEncoderOffset); bool setCalibrationGearAndJointEncoderHoming( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - const rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming & - gearAndJointEncoderHoming); - bool getCalibrationGearJointEncoderOffsetConstant(int32_t & constant); + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + const rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming& gearAndJointEncoderHoming); + bool getCalibrationGearJointEncoderOffsetConstant(int32_t& constant); bool setCalibrationGearJointEncoderOffsetConstant(const int32_t constant); bool getCalibrationGearAndJointEncoderHoming( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming & gearAndJointEncoderHoming); - bool getCalibrationImuGyroscopeDcBias( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::ImuGyroscopeDcBias & imuGyroscopeDcBias); - bool setCalibrationSpringStiffness( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - const rsl_drive_sdk::calibration::parameter::SpringStiffness & springStiffness); - bool getCalibrationSpringStiffness( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::SpringStiffness & springStiffness); - bool setCalibrationFrictionEstimation( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - const rsl_drive_sdk::calibration::parameter::FrictionEstimation & frictionEstimation); - bool getCalibrationFrictionEstimation( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::FrictionEstimation & frictionEstimation); - bool resetCustomCalibrationsToFactory( - const rsl_drive_sdk::calibration::CalibrationState calibrationState); + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming& gearAndJointEncoderHoming); + bool getCalibrationImuGyroscopeDcBias(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::ImuGyroscopeDcBias& imuGyroscopeDcBias); + bool setCalibrationSpringStiffness(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + const rsl_drive_sdk::calibration::parameter::SpringStiffness& springStiffness); + bool getCalibrationSpringStiffness(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::SpringStiffness& springStiffness); + bool + setCalibrationFrictionEstimation(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + const rsl_drive_sdk::calibration::parameter::FrictionEstimation& frictionEstimation); + bool getCalibrationFrictionEstimation(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::FrictionEstimation& frictionEstimation); + bool resetCustomCalibrationsToFactory(const rsl_drive_sdk::calibration::CalibrationState calibrationState); bool writeFactoryCalibration(); - // Configuration. - bool getMaxCurrent(double & maxCurrent); + // Configuration. + bool getMaxCurrent(double& maxCurrent); bool setMaxCurrent(const double maxCurrent); - bool getMaxFreezeCurrent(double & current); + bool getMaxFreezeCurrent(double& current); bool setMaxFreezeCurrent(const double current); - bool getMaxMotorVelocity(double & maxMotorVelocity); + bool getMaxMotorVelocity(double& maxMotorVelocity); bool setMaxMotorVelocity(const double maxMotorVelocity); - bool getMaxJointTorque(double & maxJointTorque); + bool getMaxJointTorque(double& maxJointTorque); bool setMaxJointTorque(const double maxJointTorque); - bool getCurrentIntegratorSaturation(double & saturation); + bool getCurrentIntegratorSaturation(double& saturation); bool setCurrentIntegratorSaturation(const double saturation); - bool getJointTorqueIntegratorSaturation(double & saturation); + bool getJointTorqueIntegratorSaturation(double& saturation); bool setJointTorqueIntegratorSaturation(const double saturation); - bool getDirection(int16_t & direction); + bool getDirection(int16_t& direction); bool setDirection(const int16_t direction); - bool getJointPositionLimitsSoft(rsl_drive_sdk::common::Limits & limits); - bool setJointPositionLimitsSoft(const rsl_drive_sdk::common::Limits & limits); - bool getJointPositionLimitsHard(rsl_drive_sdk::common::Limits & limits); - bool setJointPositionLimitsHard(const rsl_drive_sdk::common::Limits & limits); - bool getControlGains( - const rsl_drive_sdk::mode::ModeEnum mode, - rsl_drive_sdk::mode::PidGainsF & pidGains); - bool setControlGains( - const rsl_drive_sdk::mode::ModeEnum mode, - const rsl_drive_sdk::mode::PidGainsF & pidGains); - bool getErrorStateBehavior(uint16_t & errorStateBehavior); + bool getJointPositionLimitsSoft(rsl_drive_sdk::common::Limits& limits); + bool setJointPositionLimitsSoft(const rsl_drive_sdk::common::Limits& limits); + bool getJointPositionLimitsHard(rsl_drive_sdk::common::Limits& limits); + bool setJointPositionLimitsHard(const rsl_drive_sdk::common::Limits& limits); + bool getControlGains(const rsl_drive_sdk::mode::ModeEnum mode, rsl_drive_sdk::mode::PidGainsF& pidGains); + bool setControlGains(const rsl_drive_sdk::mode::ModeEnum mode, const rsl_drive_sdk::mode::PidGainsF& pidGains); + bool getErrorStateBehavior(uint16_t& errorStateBehavior); bool setErrorStateBehavior(const uint16_t errorStateBehavior); - bool getImuEnable(bool & enabled); + bool getImuEnable(bool& enabled); bool setImuEnable(const bool enable); - bool getImuAccelerometerRange(uint32_t & range); + bool getImuAccelerometerRange(uint32_t& range); bool setImuAccelerometerRange(const uint32_t range); - bool getImuGyroscopeRange(uint32_t & range); + bool getImuGyroscopeRange(uint32_t& range); bool setImuGyroscopeRange(const uint32_t range); - bool getFanMode(uint32_t & mode); + bool getFanMode(uint32_t& mode); bool setFanMode(const uint32_t mode); - bool getFanIntensity(uint32_t & intensity); + bool getFanIntensity(uint32_t& intensity); bool setFanIntensity(const uint32_t intensity); - bool getFanLowerTemperature(float & temperature); + bool getFanLowerTemperature(float& temperature); bool setFanLowerTemperature(const float temperature); - bool getFanUpperTemperature(float & temperature); + bool getFanUpperTemperature(float& temperature); bool setFanUpperTemperature(const float temperature); bool setBrakeMode(const bool mode); - bool getBrakeMode(bool & mode); + bool getBrakeMode(bool& mode); bool setBrakeDuty(const float d); - bool getBrakeDuty(float & d); - bool getGearJointVelocityFilterType(uint32_t & type); + bool getBrakeDuty(float& d); + bool getGearJointVelocityFilterType(uint32_t& type); bool setGearJointVelocityFilterType(const uint32_t type); - bool getGearJointVelocityKfNoiseVariance(float & variance); + bool getGearJointVelocityKfNoiseVariance(float& variance); bool setGearJointVelocityKfNoiseVariance(const float variance); - bool getGearJointVelocityKfLambda2(float & lambda); + bool getGearJointVelocityKfLambda2(float& lambda); bool setGearJointVelocityKfLambda2(const float lambda); - bool getGearJointVelocityKfGamma(float & gamma); + bool getGearJointVelocityKfGamma(float& gamma); bool setGearJointVelocityKfGamma(const float gamma); - bool getGearJointVelocityIirAlpha(float & alpha); + bool getGearJointVelocityIirAlpha(float& alpha); bool setGearJointVelocityIirAlpha(const float alpha); - bool getGearJointVelocityEmaAlpha(float & alpha); + bool getGearJointVelocityEmaAlpha(float& alpha); bool setGearJointVelocityEmaAlpha(const float alpha); - bool getJointVelocityForAccelerationFilterType(uint32_t & type); + bool getJointVelocityForAccelerationFilterType(uint32_t& type); bool setJointVelocityForAccelerationFilterType(const uint32_t type); - bool getJointVelocityForAccelerationKfNoiseVariance(float & variance); + bool getJointVelocityForAccelerationKfNoiseVariance(float& variance); bool setJointVelocityForAccelerationKfNoiseVariance(const float variance); - bool getJointVelocityForAccelerationKfLambda2(float & lambda); + bool getJointVelocityForAccelerationKfLambda2(float& lambda); bool setJointVelocityForAccelerationKfLambda2(const float lambda); - bool getJointVelocityForAccelerationKfGamma(float & gamma); + bool getJointVelocityForAccelerationKfGamma(float& gamma); bool setJointVelocityForAccelerationKfGamma(const float gamma); - bool getJointVelocityForAccelerationIirAlpha(float & alpha); + bool getJointVelocityForAccelerationIirAlpha(float& alpha); bool setJointVelocityForAccelerationIirAlpha(const float alpha); - bool getJointVelocityForAccelerationEmaAlpha(float & alpha); + bool getJointVelocityForAccelerationEmaAlpha(float& alpha); bool setJointVelocityForAccelerationEmaAlpha(const float alpha); - bool getJointAccelerationFilterType(uint32_t & type); + bool getJointAccelerationFilterType(uint32_t& type); bool setJointAccelerationFilterType(const uint32_t type); - bool getJointAccelerationKfNoiseVariance(float & variance); + bool getJointAccelerationKfNoiseVariance(float& variance); bool setJointAccelerationKfNoiseVariance(const float variance); - bool getJointAccelerationKfLambda2(float & lambda); + bool getJointAccelerationKfLambda2(float& lambda); bool setJointAccelerationKfLambda2(const float lambda); - bool getJointAccelerationKfGamma(float & gamma); + bool getJointAccelerationKfGamma(float& gamma); bool setJointAccelerationKfGamma(const float gamma); - bool getJointAccelerationIirAlpha(float & alpha); + bool getJointAccelerationIirAlpha(float& alpha); bool setJointAccelerationIirAlpha(const float alpha); - bool getJointAccelerationEmaAlpha(float & alpha); + bool getJointAccelerationEmaAlpha(float& alpha); bool setJointAccelerationEmaAlpha(const float alpha); bool writeConfiguration(); bool setDGainFilterCutoffFrequency(const float freq); - bool getDGainFilterCutoffFrequency(float & freq); + bool getDGainFilterCutoffFrequency(float& freq); - // Status. - bool getStatuswordSdo(rsl_drive_sdk::Statusword & statusword); + // Status. + bool getStatuswordSdo(rsl_drive_sdk::Statusword& statusword); rsl_drive_sdk::Statusword getStatusword(); - // Control. + // Control. void setControlword(const uint16_t controlwordId); void resetControlword(); - void setCommand(const rsl_drive_sdk::Command & command); - void getReading(rsl_drive_sdk::ReadingExtended & reading); + void setCommand(const rsl_drive_sdk::Command& command); + void getReading(rsl_drive_sdk::ReadingExtended& reading); - // RTDL (Real time data logging). - bool getRtdlEnabled(bool & enabled); + // RTDL (Real time data logging). + bool getRtdlEnabled(bool& enabled); bool setRtdlEnable(const bool enable); bool setRtdlCommand(const uint16_t command); - bool getRtdlStatus(uint16_t & status); - bool getRtdlLoggingFrequency(uint16_t & frequency); + bool getRtdlStatus(uint16_t& status); + bool getRtdlLoggingFrequency(uint16_t& frequency); bool setRtdlLoggingFrequency(const uint16_t frequency); - bool getRtdlStreamingFrequency(uint16_t & frequency); + bool getRtdlStreamingFrequency(uint16_t& frequency); bool setRtdlStreamingFrequency(const uint16_t frequency); - bool getRtdlLastTimestamp(uint64_t & timestamp); + bool getRtdlLastTimestamp(uint64_t& timestamp); - // Data logging: + // Data logging: bool clearLoggedData(); bool refreshLoggedData(); - bool readOperatingTimes(uint64_t & timeTotal, uint64_t & timeActive); - bool readJointTravel(double & jointTravel); + bool readOperatingTimes(uint64_t& timeTotal, uint64_t& timeActive); + bool readJointTravel(double& jointTravel); - - bool getCalibrationTypeId(uint16_t & calibrationTypeId); + bool getCalibrationTypeId(uint16_t& calibrationTypeId); bool setCalibrationTypeId(const uint16_t calibrationTypeId); - bool getLockStatus(uint32_t & status); - bool sendPassword(const std::string & password); - - bool getCalibrationTypeEnum( - rsl_drive_sdk::calibration::CalibrationTypeEnum & calibrationTypeEnum); - bool setCalibrationTypeEnum( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum); - - /*! - * Add a callback called when a reading has been received. - * @param readingCb Callback. - * @param priority Priority. - */ - void addReadingCb(const ReadingCb & readingCb, const int priority = 0); - - /*! - * Add a callback called when an error appeared. - * @param readingCb Callback. - * @param priority Priority. - */ - void addErrorCb(const ErrorCb & errorCb, const int priority = 0); - - /*! - * Add a callback called when an error disappeared. - * @param readingCb Callback. - * @param priority Priority. - */ - void addErrorRecoveredCb(const ErrorRecoveredCb & errorRecoveredCb, const int priority = 0); - - /*! - * Add a callback called when a fatal appeared. - * @param readingCb Callback. - * @param priority Priority. - */ - void addFatalCb(const FatalCb & fatalCb, const int priority = 0); - - /*! - * Add a callback called when a fatal disappeared. - * @param readingCb Callback. - * @param priority Priority. - */ - void addFatalRecoveredCb(const FatalRecoveredCb & fatalRecoveredCb, const int priority = 0); - - /*! - * Add a callback called when the device disconnected. - * @param readingCb Callback. - * @param priority Priority. - */ - void addDeviceDisconnectedCb( - const DeviceDisconnectedCb & deviceDisconnectedCb, - const int priority = 0); - - /*! - * Add a callback called when a the device reconnected. - * @param readingCb Callback. - * @param priority Priority. - */ - void addDeviceReconnectedCb( - const DeviceReconnectedCb & deviceReconnectedCb, - const int priority = 0); - - /*! - * Call the prioritized error callbacks. - */ + bool getLockStatus(uint32_t& status); + bool sendPassword(const std::string& password); + + bool getCalibrationTypeEnum(rsl_drive_sdk::calibration::CalibrationTypeEnum& calibrationTypeEnum); + bool setCalibrationTypeEnum(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum); + + /*! + * Add a callback called when a reading has been received. + * @param readingCb Callback. + * @param priority Priority. + */ + void addReadingCb(const ReadingCb& readingCb, const int priority = 0); + + /*! + * Add a callback called when an error appeared. + * @param readingCb Callback. + * @param priority Priority. + */ + void addErrorCb(const ErrorCb& errorCb, const int priority = 0); + + /*! + * Add a callback called when an error disappeared. + * @param readingCb Callback. + * @param priority Priority. + */ + void addErrorRecoveredCb(const ErrorRecoveredCb& errorRecoveredCb, const int priority = 0); + + /*! + * Add a callback called when a fatal appeared. + * @param readingCb Callback. + * @param priority Priority. + */ + void addFatalCb(const FatalCb& fatalCb, const int priority = 0); + + /*! + * Add a callback called when a fatal disappeared. + * @param readingCb Callback. + * @param priority Priority. + */ + void addFatalRecoveredCb(const FatalRecoveredCb& fatalRecoveredCb, const int priority = 0); + + /*! + * Add a callback called when the device disconnected. + * @param readingCb Callback. + * @param priority Priority. + */ + void addDeviceDisconnectedCb(const DeviceDisconnectedCb& deviceDisconnectedCb, const int priority = 0); + + /*! + * Add a callback called when a the device reconnected. + * @param readingCb Callback. + * @param priority Priority. + */ + void addDeviceReconnectedCb(const DeviceReconnectedCb& deviceReconnectedCb, const int priority = 0); + + /*! + * Call the prioritized error callbacks. + */ void errorCb(); - /*! - * Call the prioritized error recovered callbacks. - */ + /*! + * Call the prioritized error recovered callbacks. + */ void errorRecoveredCb(); - /*! - * Check if the device is in the error state. - * @return True if the device is in the error state. - */ + /*! + * Check if the device is in the error state. + * @return True if the device is in the error state. + */ bool deviceIsInErrorState(); - /*! - * Call the prioritized fatal callbacks. - */ + /*! + * Call the prioritized fatal callbacks. + */ void fatalCb(); - /*! - * Call the prioritized fatal recovered callbacks. - */ + /*! + * Call the prioritized fatal recovered callbacks. + */ void fatalRecoveredCb(); - /*! - * Check if the device is in the fatal state. - * @return True if the device is in the fatal state. - */ + /*! + * Check if the device is in the fatal state. + * @return True if the device is in the fatal state. + */ bool deviceIsInFatalState(); - /*! - * Call the device disconnected callbacks. - */ + /*! + * Call the device disconnected callbacks. + */ void deviceDisconnectedCb(); - /*! - * Call the device reconnected callbacks. - */ + /*! + * Call the device reconnected callbacks. + */ void deviceReconnectedCb(); - /*! - * Check whether the joint position is within the soft limits. - * @True if the device is within the soft limits. - */ + /*! + * Check whether the joint position is within the soft limits. + * @True if the device is within the soft limits. + */ bool isWithinJointPositionLimitsSoft() const; - /*! - * Check whether the joint position is within the hard limits. - * @True if the device is within the hard limits. - */ + /*! + * Check whether the joint position is within the hard limits. + * @True if the device is within the hard limits. + */ bool isWithinJointPositionLimitsHard() const; - //@} + //@} protected: bool configurePdo(const PdoTypeEnum pdoTypeEnum); - - template - bool read_and_apply_configuration_object( - std::function(void)> config_getter, std::function config_setter, - std::function drive_getter, std::function drive_setter) + template + bool read_and_apply_configuration_object(std::function(void)> config_getter, + std::function config_setter, std::function drive_getter, + std::function drive_setter) { bool success = false; - if(config_getter().isSet()) { + if (config_getter().isSet()) { success &= drive_setter(config_getter()); } else { T val; @@ -506,10 +485,9 @@ class DriveEthercatDevice : public ecat_master::EthercatDevice { return success; } - // custom handling of string SDOs - bool sendSdoReadStringCustom(const uint16_t index, std::string & value); - bool sendSdoWriteStringCustom(const uint16_t index, const std::string & value); + // custom handling of string SDOs + bool sendSdoReadStringCustom(const uint16_t index, std::string& value); + bool sendSdoWriteStringCustom(const uint16_t index, const std::string& value); }; - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/DriveCalibrator.hpp b/include/rsl_drive_sdk/DriveCalibrator.hpp index 19c0483..e3ddc26 100644 --- a/include/rsl_drive_sdk/DriveCalibrator.hpp +++ b/include/rsl_drive_sdk/DriveCalibrator.hpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once @@ -26,37 +28,35 @@ namespace rsl_drive_sdk { - class DriveCalibrator - { - public: - DriveCalibrator(DriveEthercatDevice::SharedPtr drive); +class DriveCalibrator +{ +public: + explicit DriveCalibrator(DriveEthercatDevice::SharedPtr drive); - bool calibrate( - const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute = true, - const double gearAndJointEncoderHomingNewJointPosition = 0.0); + bool calibrate(const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute = true, + const double gearAndJointEncoderHomingNewJointPosition = 0.0); - /** - * @brief start_calibration - run calibration non blocking - * @return false als long as calibration is running - * @throws on error - */ - void start_calibration( - const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute = true, - const double gearAndJointEncoderHomingNewJointPosition = 0.0); + /** + * @brief start_calibration - run calibration non blocking + * @return false also long as calibration is running + * @throws on error + */ + void start_calibration(const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute = true, + const double gearAndJointEncoderHomingNewJointPosition = 0.0); - /** - * @brief is_calibration_running - check if there is currently a calibration running - */ - bool is_calibration_running() - { - bool calib_running = true; - _drive->calibrationIsRunning(calib_running); - return calib_running; - } + /** + * @brief is_calibration_running - check if there is currently a calibration running + */ + bool is_calibration_running() + { + bool calib_running = true; + _drive->calibrationIsRunning(calib_running); + return calib_running; + } - private: - DriveEthercatDevice::SharedPtr _drive; - }; -} +private: + DriveEthercatDevice::SharedPtr _drive; +}; +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/DriveCollection.hpp b/include/rsl_drive_sdk/DriveCollection.hpp index e1c50db..705d72c 100644 --- a/include/rsl_drive_sdk/DriveCollection.hpp +++ b/include/rsl_drive_sdk/DriveCollection.hpp @@ -2,32 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include #include +#include +#include +#include // rsl_drive_sdk #include "rsl_drive_sdk/Drive.hpp" @@ -36,7 +40,6 @@ // message logger #include - namespace rsl_drive_sdk { @@ -64,34 +67,33 @@ class DriveCollection /*! * Constructor. */ - DriveCollection(const DrivesVector & drives); + explicit DriveCollection(const DrivesVector& drives); /*! * Destructor. */ virtual ~DriveCollection(); - /*! * Add an rsl_drive_sdk to the vector of rsl_drive_sdks. * @param pointer to the new rsl_drive_sdk * @return True if successful. */ - bool addDrive(const DriveEthercatDevice::SharedPtr & drive); + bool addDrive(const DriveEthercatDevice::SharedPtr& drive); /*! * Check if a Drive with a given name exists. * @param name Name of the Frive. * @return True if existing. */ - bool driveExists(const std::string & name) const; + bool driveExists(const std::string& name) const; /*! * Get an Drive by name. If unsure, first check if the Drive exists with rsl_drive_sdkExists(..). * @param name Name of the Drive. * @return Pointer to the Drive. */ - DriveEthercatDevice::SharedPtr getDrive(const std::string & name) const; + DriveEthercatDevice::SharedPtr getDrive(const std::string& name) const; /*! * Get all Drives. @@ -124,16 +126,14 @@ class DriveCollection /*! * Set the goal state of the state machines of all devices. * @param goalStateEnum Goal state. - * @param reachState True: Block until the devices are in the goal state, False: return after setting the goal state. + * @param reachState True: Block until the devices are in the goal state, False: return after setting the goal + * state. * @param timeout Maximal blocking time. * @param checkingFrequency Frequency at which is checked, whether the devices have reached the goal state. * @return True if non-blocking, or blocking and the devices reached the goal state within the timeout. */ - bool setGoalStatesEnum( - const fsm::StateEnum goalStateEnum, - const bool reachStates = false, - const double timeout = 5.0, - const double checkingFrequency = 100.0); + bool setGoalStatesEnum(const fsm::StateEnum goalStateEnum, const bool reachStates = false, const double timeout = 5.0, + const double checkingFrequency = 100.0); /*! * Clear the current goal state of the state machines of all devices. @@ -158,7 +158,7 @@ class DriveCollection * @param stateEnum Return argument, will contain the common state. NA if not all devices are in the same state. * @return True if all devices are in the same state. */ - bool allDevicesAreInTheSameState(fsm::StateEnum & stateEnum) const; + bool allDevicesAreInTheSameState(fsm::StateEnum& stateEnum) const; /*! * Check if all devices are in a given mode. @@ -172,7 +172,7 @@ class DriveCollection * @param modeEnum Return argument, will contain the common mode. NA if not all devices are in the same mode. * @return True if all devices are in the same mode. */ - bool allDevicesAreInTheSameMode(mode::ModeEnum & modeEnum) const; + bool allDevicesAreInTheSameMode(mode::ModeEnum& modeEnum) const; /*! * Check if no device is in the Error state. @@ -211,14 +211,14 @@ class DriveCollection * Run a calibration for a given device. * @param deviceName Name of the device to run the calibration. * @param calibrationModeEnum Mode of the calibration. - * @param gearAndJointEncoderHomingAbsolute In case of the gear and joint encoder homing calibration, this flag indicates whether it shall be done absolute or relative. - * @param gearAndJointEncoderHomingNewJointPosition In case of the gear and joint encoder homing calibration, this is the new joint position. + * @param gearAndJointEncoderHomingAbsolute In case of the gear and joint encoder homing calibration, this + * flag indicates whether it shall be done absolute or relative. + * @param gearAndJointEncoderHomingNewJointPosition In case of the gear and joint encoder homing calibration, this is + * the new joint position. */ - virtual bool calibrate( - const std::string & deviceName, - const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute = true, - const double gearAndJointEncoderHomingNewJointPosition = 0.0); + virtual bool calibrate(const std::string& deviceName, const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute = true, + const double gearAndJointEncoderHomingNewJointPosition = 0.0); //@} @@ -254,7 +254,7 @@ class DriveCollection * Calls Drive::setCommand for all drives. * @param commands Commands to stage for each Drive. Order and size must match the the list of Drives. */ - void setCommands(const std::vector & commands); + void setCommands(const std::vector& commands); //@} @@ -271,8 +271,6 @@ class DriveCollection void printInfo() const; //@} - }; - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/RxPdo.hpp b/include/rsl_drive_sdk/PDO/RxPdo.hpp index 50d1e62..b474eb6 100644 --- a/include/rsl_drive_sdk/PDO/RxPdo.hpp +++ b/include/rsl_drive_sdk/PDO/RxPdo.hpp @@ -2,37 +2,35 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ - #pragma once - // std #include - namespace rsl_drive_sdk { - struct RxPdo { uint16_t controlword_ = 0; @@ -47,5 +45,4 @@ struct RxPdo float controlParameterD_ = 0.0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/TxPdoA.hpp b/include/rsl_drive_sdk/PDO/TxPdoA.hpp index ed8c71e..1913c1c 100644 --- a/include/rsl_drive_sdk/PDO/TxPdoA.hpp +++ b/include/rsl_drive_sdk/PDO/TxPdoA.hpp @@ -2,35 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { - struct TxPdoA { uint32_t statusword_ = 0; @@ -47,5 +46,4 @@ struct TxPdoA float measuredJointTorque_ = 0.0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/TxPdoB.hpp b/include/rsl_drive_sdk/PDO/TxPdoB.hpp index 41e8c9f..5431413 100644 --- a/include/rsl_drive_sdk/PDO/TxPdoB.hpp +++ b/include/rsl_drive_sdk/PDO/TxPdoB.hpp @@ -2,35 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { - struct TxPdoB { uint32_t statusword_ = 0; @@ -54,5 +53,4 @@ struct TxPdoB float measuredImuAngularVelocityW_ = 0.0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/TxPdoC.hpp b/include/rsl_drive_sdk/PDO/TxPdoC.hpp index 2be056f..ca6d5dd 100644 --- a/include/rsl_drive_sdk/PDO/TxPdoC.hpp +++ b/include/rsl_drive_sdk/PDO/TxPdoC.hpp @@ -2,35 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { - struct TxPdoC { uint32_t statusword_ = 0; @@ -78,5 +77,4 @@ struct TxPdoC float measuredImuAngularVelocityW_ = 0.0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/TxPdoD.hpp b/include/rsl_drive_sdk/PDO/TxPdoD.hpp index 0ca4858..07285d1 100644 --- a/include/rsl_drive_sdk/PDO/TxPdoD.hpp +++ b/include/rsl_drive_sdk/PDO/TxPdoD.hpp @@ -2,35 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { - struct TxPdoD { uint32_t statusword_ = 0; @@ -66,5 +65,4 @@ struct TxPdoD int32_t value6_ = 0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PDO/TxPdoE.hpp b/include/rsl_drive_sdk/PDO/TxPdoE.hpp index 16e3e75..6f287c9 100644 --- a/include/rsl_drive_sdk/PDO/TxPdoE.hpp +++ b/include/rsl_drive_sdk/PDO/TxPdoE.hpp @@ -2,35 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { - struct TxPdoE { uint32_t statusword_ = 0; @@ -81,5 +80,4 @@ struct TxPdoE int32_t measuredCoilTemp3_ = 0.0; } __attribute__((packed)); - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/PdoTypeEnum.hpp b/include/rsl_drive_sdk/PdoTypeEnum.hpp index e619885..7179bf1 100644 --- a/include/rsl_drive_sdk/PdoTypeEnum.hpp +++ b/include/rsl_drive_sdk/PdoTypeEnum.hpp @@ -2,40 +2,39 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - namespace rsl_drive_sdk { - enum class PdoTypeEnum { NA, - A, // Smallest PDO without IMU. - B, // Smallest PDO with IMU. - C, // Full PDO with support for Real Time Data Logging (RTDL). - D, // [DEPRECATED] Full PDO. - E //Testbench + A, // Smallest PDO without IMU. + B, // Smallest PDO with IMU. + C, // Full PDO with support for Real Time Data Logging (RTDL). + D, // [DEPRECATED] Full PDO. + E // Testbench }; - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/Reading.hpp b/include/rsl_drive_sdk/Reading.hpp index 3ec955c..a5f3ff3 100644 --- a/include/rsl_drive_sdk/Reading.hpp +++ b/include/rsl_drive_sdk/Reading.hpp @@ -2,39 +2,40 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once +#include // rsl_drive_sdk #include "rsl_drive_sdk/Command.hpp" #include "rsl_drive_sdk/State.hpp" #include "rsl_drive_sdk/StateExtended.hpp" - namespace rsl_drive_sdk { - //! Reading of the Drive containing arbitrary command and state information. -template +template class ReadingT { public: @@ -48,40 +49,44 @@ class ReadingT State state_; public: - ReadingT() {} - virtual ~ReadingT() {} + ReadingT() + { + } + virtual ~ReadingT() + { + } - const Command & getCommanded() const + const Command& getCommanded() const { return commanded_; } - Command & getCommanded() + Command& getCommanded() { return commanded_; } - void setCommanded(const Command & commanded) + void setCommanded(const Command& commanded) { commanded_ = commanded; } - const State & getState() const + const State& getState() const { return state_; } - State & getState() + State& getState() { return state_; } - void setState(const State & state) + void setState(const State& state) { state_ = state; } - std::string asString(const std::string & prefix = "") const + std::string asString(const std::string& prefix = "") const { std::stringstream ss; ss << prefix << commanded_.asString(" ") << std::endl; @@ -90,8 +95,8 @@ class ReadingT } }; -template -std::ostream & operator<<(std::ostream & out, const ReadingT & reading) +template +std::ostream& operator<<(std::ostream& out, const ReadingT& reading) { out << "Reading:" << std::endl; out << reading.asString(" ") << std::endl; @@ -101,5 +106,4 @@ std::ostream & operator<<(std::ostream & out, const ReadingT & //! Reading of the Drive containing the command and the state. using Reading = ReadingT; - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/ReadingExtended.hpp b/include/rsl_drive_sdk/ReadingExtended.hpp index 13411b6..a65a9e9 100644 --- a/include/rsl_drive_sdk/ReadingExtended.hpp +++ b/include/rsl_drive_sdk/ReadingExtended.hpp @@ -2,37 +2,35 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/Reading.hpp" - namespace rsl_drive_sdk { - //! Extended reading of the Drive containing the command and the extended state. using ReadingExtended = ReadingT; - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/State.hpp b/include/rsl_drive_sdk/State.hpp index 8a64d8a..90e9246 100644 --- a/include/rsl_drive_sdk/State.hpp +++ b/include/rsl_drive_sdk/State.hpp @@ -2,29 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include +#include // any measurements @@ -33,7 +35,6 @@ // rsl_drive_sdk #include "rsl_drive_sdk/Statusword.hpp" - namespace rsl_drive_sdk { @@ -46,10 +47,8 @@ struct Imu double angle_velocity_x; double angle_velocity_y; double angle_velocity_z; - }; - //! State of the Drive. class State { @@ -57,7 +56,7 @@ class State //! Time when the state was read out. std::chrono::high_resolution_clock::time_point stamp_; //! Statusword. - Statusword statusword_ {}; + Statusword statusword_{}; //! Current [A]. double current_ = 0.0; //! Gear position [rad]. @@ -73,17 +72,17 @@ class State //! Joint torque [Nm]. double jointTorque_ = 0.0; //! IMU. - Imu imu_ {}; + Imu imu_{}; public: State(); virtual ~State(); - const std::chrono::high_resolution_clock::time_point & getStamp() const; - void setStamp(const std::chrono::high_resolution_clock::time_point & stamp); + const std::chrono::high_resolution_clock::time_point& getStamp() const; + void setStamp(const std::chrono::high_resolution_clock::time_point& stamp); - const Statusword & getStatusword() const; - void setStatusword(const Statusword & statusword); + const Statusword& getStatusword() const; + void setStatusword(const Statusword& statusword); double getCurrent() const; void setCurrent(const double current); @@ -106,8 +105,8 @@ class State double getJointTorque() const; void setJointTorque(const double jointTorque); - const Imu & getImu() const; - void setImu(const Imu & imu); + const Imu& getImu() const; + void setImu(const Imu& imu); /*! * Check if the state is valid: @@ -118,10 +117,9 @@ class State */ bool isValid() const; - virtual std::string asString(const std::string & prefix = "") const; + virtual std::string asString(const std::string& prefix = "") const; }; -std::ostream & operator<<(std::ostream & out, const State & state); - +std::ostream& operator<<(std::ostream& out, const State& state); -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/StateExtended.hpp b/include/rsl_drive_sdk/StateExtended.hpp index 1a06ead..4b6361b 100644 --- a/include/rsl_drive_sdk/StateExtended.hpp +++ b/include/rsl_drive_sdk/StateExtended.hpp @@ -2,35 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once +#include // rsl_drive_sdk #include "rsl_drive_sdk/State.hpp" - namespace rsl_drive_sdk { - //! Extended state of the Drive, containing additional information to the state. class StateExtended : public State { @@ -84,9 +85,9 @@ class StateExtended : public State float coilTemp2_ = 0.0; float coilTemp3_ = 0.0; - //Apparent power in [W] + // Apparent power in [W] double measuredApparentPower = 0.0; - //Apparent input current [A] + // Apparent input current [A] double measuredInputCurrent = 0.0; public: @@ -168,15 +169,13 @@ class StateExtended : public State double getCoilTemp3() const; void setCoilTemp3(const double coilTemp); - - virtual std::string asString(const std::string & prefix = "") const; + virtual std::string asString(const std::string& prefix = "") const; double getMeasuredApparentPower() const; void setMeasuredApparentPower(double value); double getMeasuredInputCurrent() const; void setMeasuredInputCurrent(double value); }; -std::ostream & operator<<(std::ostream & out, const StateExtended & stateExtended); - +std::ostream& operator<<(std::ostream& out, const StateExtended& stateExtended); -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/Statusword.hpp b/include/rsl_drive_sdk/Statusword.hpp index 2503e14..896435b 100644 --- a/include/rsl_drive_sdk/Statusword.hpp +++ b/include/rsl_drive_sdk/Statusword.hpp @@ -2,49 +2,49 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include +#include // rsl_drive_sdk #include "message_logger/message_logger.hpp" #include "rsl_drive_sdk/mode/ModeEnum.hpp" #include "rsl_drive_sdk/fsm/StateEnum.hpp" - namespace rsl_drive_sdk { - class Statusword { public: - struct DataBits // See drive firmware: motordrive_def.h + struct DataBits // See drive firmware: motordrive_def.h { - uint32_t stateId_ : 4; // FSM Status: Bits 0-3 - uint32_t modeId_ : 4; // Operation Mode: 4-7 + uint32_t stateId_ : 4; // FSM Status: Bits 0-3 + uint32_t modeId_ : 4; // Operation Mode: 4-7 uint32_t warningOvertemperatureBridge_ : 1; uint32_t warningOvertemperatureStator_ : 1; @@ -63,7 +63,6 @@ class Statusword uint32_t error_direct_drive_elec_zero_offset_invalid : 1; uint32_t error_max_velocity_exceeded : 1; - uint32_t errorJointPositionLimitsSoft_ : 1; uint32_t errorJointPositionLimitsHard_ : 1; @@ -83,15 +82,16 @@ class Statusword uint32_t fatal_joint_motor_encoder : 1; }; - union Data{ + union Data + { DataBits bits_; uint32_t all_ = 0; Data(); Data(const uint32_t data); - bool operator==(const Data & other); - bool operator!=(const Data & other); + bool operator==(const Data& other); + bool operator!=(const Data& other); }; protected: @@ -104,11 +104,11 @@ class Statusword public: Statusword(); - Statusword(const Statusword & statusword); + Statusword(const Statusword& statusword); explicit Statusword(const uint32_t data); virtual ~Statusword(); - Statusword & operator=(const Statusword & statusword); + Statusword& operator=(const Statusword& statusword); bool isEmpty() const; @@ -118,7 +118,7 @@ class Statusword void setData(const uint32_t data); uint32_t getData() const; - Data & getDataField(); + Data& getDataField(); fsm::StateEnum getStateEnum() const; void setStateEnum(const fsm::StateEnum stateEnum); @@ -126,17 +126,11 @@ class Statusword mode::ModeEnum getModeEnum() const; void setModeEnum(const mode::ModeEnum modeEnum); - void getMessages( - std::vector & infos, - std::vector & warnings, - std::vector & errors, - std::vector & fatals) const; - void getMessagesDiff( - Statusword & previousStatusword, - std::vector & infos, - std::vector & warnings, - std::vector & errors, - std::vector & fatals) const; + void getMessages(std::vector& infos, std::vector& warnings, + std::vector& errors, std::vector& fatals) const; + void getMessagesDiff(Statusword& previousStatusword, std::vector& infos, + std::vector& warnings, std::vector& errors, + std::vector& fatals) const; bool hasWarningHighTemperatureBridge() const; bool hasWarningHighTemperatureStator() const; @@ -163,7 +157,6 @@ class Statusword bool hasFatalJointMotorEncoder() const; }; -std::ostream & operator<<(std::ostream & os, const Statusword & statusword); - +std::ostream& operator<<(std::ostream& os, const Statusword& statusword); -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/CalibrationModeEnum.hpp b/include/rsl_drive_sdk/calibration/CalibrationModeEnum.hpp index c59f40c..6dc5004 100644 --- a/include/rsl_drive_sdk/calibration/CalibrationModeEnum.hpp +++ b/include/rsl_drive_sdk/calibration/CalibrationModeEnum.hpp @@ -2,50 +2,49 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include - namespace rsl_drive_sdk { namespace calibration { - //! Calibration mode short names. -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_FRICTION_ESTIMATION ("FricEst") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GEAR_AND_JOINT_ENC_HOMING ("GJEHoming") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GEAR_JOINT_ENC_OFFSET ("GJEOffset") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GRAVITY_COMPENSATION ("GravComp") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_IMU_GYRO_DC_BIAS ("GyroBias") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_MOTOR_ENC_OFFSET ("MEOffset") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_MOTOR_ENC_PARAMS ("MEParams") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_NA ("N/A") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_SAFE_JOINT_VEL ("SafeJVel") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_SPRING_STIFFNESS ("SprStiff") -#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_AKSIM_SELF_CALIB ("AksimSelfCal") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_FRICTION_ESTIMATION ("FricEst") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GEAR_AND_JOINT_ENC_HOMING ("GJEHoming") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GEAR_JOINT_ENC_OFFSET ("GJEOffset") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_GRAVITY_COMPENSATION ("GravComp") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_IMU_GYRO_DC_BIAS ("GyroBias") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_MOTOR_ENC_OFFSET ("MEOffset") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_MOTOR_ENC_PARAMS ("MEParams") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_NA ("N/A") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_SAFE_JOINT_VEL ("SafeJVel") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_SPRING_STIFFNESS ("SprStiff") +#define RSL_DRIVE_CALIB_MODE_NAME_SHORT_AKSIM_SELF_CALIB ("AksimSelfCal") //! Calibration mode enumerators for type safe usage. enum class CalibrationModeEnum @@ -60,9 +59,9 @@ enum class CalibrationModeEnum NA, SafeJointVelocity, SpringStiffness, - AksimSelfCalibMode, // For initializing the overall calibration - AksimGearCalibState, // For reading the gear-Aksim's calibration state - AksimJointCalibState // For reading the joint-Aksim's calibration state + AksimSelfCalibMode, // For initializing the overall calibration + AksimGearCalibState, // For reading the gear-Aksim's calibration state + AksimJointCalibState // For reading the joint-Aksim's calibration state }; /*! @@ -91,10 +90,9 @@ std::string calibrationModeEnumToName(const CalibrationModeEnum calibrationModeE * @param calibrationModeName Human readable string. * @return Calibration mode enumerator. */ -CalibrationModeEnum calibrationModeNameToEnum(const std::string & calibrationModeName); - -std::ostream & operator<<(std::ostream & out, const CalibrationModeEnum calibrationModeEnum); +CalibrationModeEnum calibrationModeNameToEnum(const std::string& calibrationModeName); +std::ostream& operator<<(std::ostream& out, const CalibrationModeEnum calibrationModeEnum); -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/CalibrationState.hpp b/include/rsl_drive_sdk/calibration/CalibrationState.hpp index 137f9f5..0fb9380 100644 --- a/include/rsl_drive_sdk/calibration/CalibrationState.hpp +++ b/include/rsl_drive_sdk/calibration/CalibrationState.hpp @@ -2,40 +2,39 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include // rsl_drive_sdk #include "rsl_drive_sdk/calibration/CalibrationModeEnum.hpp" - namespace rsl_drive_sdk { namespace calibration { - /*! * Calibration state bits. Can be used to indicate e.g. which calibrations have been * done or which calibrations should be reset. @@ -57,7 +56,8 @@ struct CalibrationStateBits }; //! Calibration state union. -union CalibrationState{ +union CalibrationState +{ //! Bitwise access. CalibrationStateBits single_; //! Complete access. @@ -75,10 +75,8 @@ union CalibrationState{ * @param calibrationModeEnum Calibration mode enumerator. * @param isCalibrated The new state of the calibration. */ - void setSingleCalibrationState(const CalibrationModeEnum calibrationModeEnum, - const bool isCalibrated); + void setSingleCalibrationState(const CalibrationModeEnum calibrationModeEnum, const bool isCalibrated); }; - -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/CalibrationTypeEnum.hpp b/include/rsl_drive_sdk/calibration/CalibrationTypeEnum.hpp index 000e823..9afc565 100644 --- a/include/rsl_drive_sdk/calibration/CalibrationTypeEnum.hpp +++ b/include/rsl_drive_sdk/calibration/CalibrationTypeEnum.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include @@ -30,13 +31,11 @@ // rsl_drive_sdk #include "rsl_drive_sdk/common/ObjectDictionary.hpp" - namespace rsl_drive_sdk { namespace calibration { - //! Calibration type enumerators for type safe usage. enum class CalibrationTypeEnum { @@ -71,8 +70,7 @@ std::string calibrationTypeEnumToName(const CalibrationTypeEnum calibrationTypeE * @param calibrationTypeName Human readable string. * @return Calibration type enumerator. */ -CalibrationTypeEnum calibrationTypeNameToEnum(const std::string & calibrationTypeName); - +CalibrationTypeEnum calibrationTypeNameToEnum(const std::string& calibrationTypeName); -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/Calibration.hpp b/include/rsl_drive_sdk/calibration/parameter/Calibration.hpp index 443eb34..4175a7a 100644 --- a/include/rsl_drive_sdk/calibration/parameter/Calibration.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/Calibration.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp" #include "rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp" @@ -32,7 +33,6 @@ #include "rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp" #include "rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -40,7 +40,6 @@ namespace calibration namespace parameter { - /*! * Calibration class containing all calibration parameters. */ @@ -66,10 +65,9 @@ struct Calibration * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const Calibration & other) const; + bool operator==(const Calibration& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp b/include/rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp index 6430cec..f9063d6 100644 --- a/include/rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct FrictionEstimation { //! Break away friction [Nm]. @@ -51,10 +50,9 @@ struct FrictionEstimation * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const FrictionEstimation & other) const; + bool operator==(const FrictionEstimation& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp b/include/rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp index e1bbe5b..56444e6 100644 --- a/include/rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct GearAndJointEncoderHoming { //! Gear encoder homing position [ticks]. @@ -47,10 +46,9 @@ struct GearAndJointEncoderHoming * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const GearAndJointEncoderHoming & other) const; + bool operator==(const GearAndJointEncoderHoming& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/GearJointEncoderOffset.hpp b/include/rsl_drive_sdk/calibration/parameter/GearJointEncoderOffset.hpp index fd61b40..8847efc 100644 --- a/include/rsl_drive_sdk/calibration/parameter/GearJointEncoderOffset.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/GearJointEncoderOffset.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct GearJointEncoderOffset { //! Constant offset [ticks]. @@ -53,10 +52,9 @@ struct GearJointEncoderOffset * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const GearJointEncoderOffset & other) const; + bool operator==(const GearJointEncoderOffset& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/ImuGyroscopeDcBias.hpp b/include/rsl_drive_sdk/calibration/parameter/ImuGyroscopeDcBias.hpp index baeaf77..5cd1026 100644 --- a/include/rsl_drive_sdk/calibration/parameter/ImuGyroscopeDcBias.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/ImuGyroscopeDcBias.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct ImuGyroscopeDcBias { int32_t x_ = 0; @@ -46,10 +45,9 @@ struct ImuGyroscopeDcBias * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const ImuGyroscopeDcBias & other) const; + bool operator==(const ImuGyroscopeDcBias& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/MotorEncoderOffset.hpp b/include/rsl_drive_sdk/calibration/parameter/MotorEncoderOffset.hpp index eb1e779..6bf62f0 100644 --- a/include/rsl_drive_sdk/calibration/parameter/MotorEncoderOffset.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/MotorEncoderOffset.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct MotorEncoderOffset { int32_t value_ = 0; @@ -44,10 +43,9 @@ struct MotorEncoderOffset * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const MotorEncoderOffset & other) const; + bool operator==(const MotorEncoderOffset& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp b/include/rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp index 7ddc62b..100c6c6 100644 --- a/include/rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct MotorEncoderParameters { int32_t dGain_ = 0; @@ -50,10 +49,9 @@ struct MotorEncoderParameters * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const MotorEncoderParameters & other) const; + bool operator==(const MotorEncoderParameters& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp b/include/rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp index 4408f04..392e4c6 100644 --- a/include/rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp +++ b/include/rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace calibration @@ -34,7 +34,6 @@ namespace calibration namespace parameter { - struct SpringStiffness { //! Spring stiffness for negative deflections [Nm/rad]. @@ -47,10 +46,9 @@ struct SpringStiffness * @param other Other calibration parameters. * @return True if equal. */ - bool operator==(const SpringStiffness & other) const; + bool operator==(const SpringStiffness& other) const; }; - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/BuildInfo.h b/include/rsl_drive_sdk/common/BuildInfo.h index 6be95a6..10665bb 100644 --- a/include/rsl_drive_sdk/common/BuildInfo.h +++ b/include/rsl_drive_sdk/common/BuildInfo.h @@ -2,61 +2,62 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - #include #include #include - -namespace rsl_drive_sdk { - namespace common { - - - struct capabilities_t - { - uint32_t experimental:1; - uint32_t async_spi:1; - uint32_t async_i2c:1; - uint32_t watchdog:1; - uint32_t shunt_05:1; - uint32_t timing_measurements:1; - uint32_t fieldweakening:1; - }; - - union capabilities_u{ - capabilities_t cap; - uint32_t raw; - }; - - - struct BuildInfo - { - std::string buildDate = ""; - std::string gitTag = ""; - std::string gitHash = ""; - capabilities_t caps; - }; - - } - -} +namespace rsl_drive_sdk +{ +namespace common +{ + +struct capabilities_t +{ + uint32_t experimental : 1; + uint32_t async_spi : 1; + uint32_t async_i2c : 1; + uint32_t watchdog : 1; + uint32_t shunt_05 : 1; + uint32_t timing_measurements : 1; + uint32_t fieldweakening : 1; +}; + +union capabilities_u +{ + capabilities_t cap; + uint32_t raw; +}; + +struct BuildInfo +{ + std::string buildDate = ""; + std::string gitTag = ""; + std::string gitHash = ""; + capabilities_t caps; +}; + +} // namespace common + +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/DriveInfo.hpp b/include/rsl_drive_sdk/common/DriveInfo.hpp index 5b5382d..fc9eac3 100644 --- a/include/rsl_drive_sdk/common/DriveInfo.hpp +++ b/include/rsl_drive_sdk/common/DriveInfo.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include @@ -33,7 +34,6 @@ namespace rsl_drive_sdk { - class DriveInfo { protected: @@ -48,35 +48,29 @@ class DriveInfo public: DriveInfo(); - DriveInfo( - const std::string & serialNumber, - const std::string & model, - const std::string & name, - const uint16_t id, - const common::Version & bootloaderVersion, - const common::Version & firmwareVersion); + DriveInfo(const std::string& serialNumber, const std::string& model, const std::string& name, const uint16_t id, + const common::Version& bootloaderVersion, const common::Version& firmwareVersion); virtual ~DriveInfo(); - std::string & getSerialNumber(); - const std::string & getSerialNumber() const; + std::string& getSerialNumber(); + const std::string& getSerialNumber() const; - std::string & getModel(); - const std::string & getModel() const; + std::string& getModel(); + const std::string& getModel() const; - std::string & getName(); - const std::string & getName() const; + std::string& getName(); + const std::string& getName() const; - uint16_t & getId(); + uint16_t& getId(); uint16_t getId() const; - common::Version & getBootloaderVersion(); - const common::Version & getBootloaderVersion() const; + common::Version& getBootloaderVersion(); + const common::Version& getBootloaderVersion() const; - common::Version & getFirmwareVersion(); - const common::Version & getFirmwareVersion() const; + common::Version& getFirmwareVersion(); + const common::Version& getFirmwareVersion() const; }; -std::ostream & operator<<(std::ostream & ostream, const DriveInfo & driveInfo); - +std::ostream& operator<<(std::ostream& ostream, const DriveInfo& driveInfo); -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/FirmwareInfo.hpp b/include/rsl_drive_sdk/common/FirmwareInfo.hpp index f9033c0..6a8ca7b 100644 --- a/include/rsl_drive_sdk/common/FirmwareInfo.hpp +++ b/include/rsl_drive_sdk/common/FirmwareInfo.hpp @@ -2,33 +2,33 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include - namespace rsl_drive_sdk { namespace common @@ -45,7 +45,7 @@ struct FirmwareInfo std::string keyId = ""; }; -std::ostream & operator<<(std::ostream & ostream, const FirmwareInfo & info); +std::ostream& operator<<(std::ostream& ostream, const FirmwareInfo& info); -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/Limits.hpp b/include/rsl_drive_sdk/common/Limits.hpp index 9254225..870c751 100644 --- a/include/rsl_drive_sdk/common/Limits.hpp +++ b/include/rsl_drive_sdk/common/Limits.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - namespace rsl_drive_sdk { namespace common { - //! Class implementing limits. class Limits { @@ -79,7 +78,7 @@ class Limits * Get the lower limit by reference. * @return Reference to the lower limit. */ - double & min(); + double& min(); /*! * Get the upper limit. @@ -91,7 +90,7 @@ class Limits * Get the upper limit by reference. * @return Reference to the upper limit. */ - double & max(); + double& max(); /*! * Check if a value lies within the limits. @@ -112,7 +111,7 @@ class Limits * @param offset Offset. * @return Shifted limits. */ - Limits & operator+=(const double offset); + Limits& operator+=(const double offset); /*! * Create new shifted the limits. @@ -126,7 +125,7 @@ class Limits * @param offset Offset. * @return Reference to shifted limits. */ - Limits & operator-=(const double offset); + Limits& operator-=(const double offset); /*! * Create new scaled limits. @@ -140,9 +139,8 @@ class Limits * @param scaling Scaling factor. * @return Reference to scaled limits. */ - Limits & operator*=(const double scaling); + Limits& operator*=(const double scaling); }; - -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/Log.hpp b/include/rsl_drive_sdk/common/Log.hpp index 8450036..f26e704 100644 --- a/include/rsl_drive_sdk/common/Log.hpp +++ b/include/rsl_drive_sdk/common/Log.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include @@ -31,13 +32,11 @@ // rsl_drive_sdk #include - namespace rsl_drive_sdk { - //! Simple templated log class, used for calibration. -template +template class Log { protected: @@ -56,20 +55,22 @@ class Log * Constructor. * @param name Name of the log. */ - Log(const std::string name) - : name_(name), - stamp_(std::chrono::system_clock::now()) {} + explicit Log(const std::string name) : name_(name), stamp_(std::chrono::system_clock::now()) + { + } /*! * Destructor. */ - virtual ~Log() {} + virtual ~Log() + { + } /*! * Get the name of the log. * @return Name of the log. */ - const std::string & getName() const + const std::string& getName() const { return name_; } @@ -78,7 +79,7 @@ class Log * Get the time stamp of the log. * @return Time stamp of the log. */ - const TimePoint & getStamp() const + const TimePoint& getStamp() const { return stamp_; } @@ -87,7 +88,7 @@ class Log * Get the logged data. * @return Logged data. */ - const std::vector & getData() const + const std::vector& getData() const { return data_; } @@ -96,11 +97,10 @@ class Log * Add data to log. * @param data Data to add. */ - void addData(const DataT & data) + void addData(const DataT& data) { data_.push_back(data); } }; - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/Logger.hpp b/include/rsl_drive_sdk/common/Logger.hpp index 4e33cae..af44226 100644 --- a/include/rsl_drive_sdk/common/Logger.hpp +++ b/include/rsl_drive_sdk/common/Logger.hpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include +#include // rsl_drive_sdk #include "rsl_drive_sdk/common/Log.hpp" - namespace rsl_drive_sdk { namespace common { - //! Simple templated logger class, used for calibration. -template +template class Logger { protected: @@ -46,7 +46,7 @@ class Logger //! Type of a map of logs. using LogTMap = std::map; //! Type of a pointer to a log. - using LogTPtr = LogT *; + using LogTPtr = LogT*; //! Map of logs. LogTMap logs_; @@ -57,12 +57,16 @@ class Logger /*! * Constructor. */ - Logger() {} + Logger() + { + } /*! * Destructor. */ - virtual ~Logger() {} + virtual ~Logger() + { + } /*! * Check if logging is active. @@ -78,7 +82,7 @@ class Logger * @param name Name of the log. * @return Log. */ - LogT getLog(const std::string & name) const + LogT getLog(const std::string& name) const { const auto it = logs_.find(name); if (it == logs_.end()) { @@ -92,15 +96,14 @@ class Logger * Start a new log. Skips if a log with the given name already exists. * @param name Name of the new log. */ - void startLog(const std::string & name) + void startLog(const std::string& name) { if (logIsActive()) { - RSL_DRIVE_ERROR("Log with name '" << activeLog_->getName() << - "' is still active, cannot start a new one."); + RSL_DRIVE_ERROR("Log with name '" << activeLog_->getName() << "' is still active, cannot start a new one."); return; } - const auto result = logs_.insert({name, LogT(name)}); + const auto result = logs_.insert({ name, LogT(name) }); if (!result.second) { RSL_DRIVE_ERROR("Log with name '" << name << "' exists already, will not overwrite it."); return; @@ -114,7 +117,7 @@ class Logger * Add data to the active log. * @param data Data. */ - void addDataToLog(const DataT & data) + void addDataToLog(const DataT& data) { if (!logIsActive()) { RSL_DRIVE_ERROR("No log is active, cannot add data."); @@ -149,6 +152,5 @@ class Logger } }; - -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/common/ObjectDictionary.hpp b/include/rsl_drive_sdk/common/ObjectDictionary.hpp index 8ac0ec2..7c87836 100644 --- a/include/rsl_drive_sdk/common/ObjectDictionary.hpp +++ b/include/rsl_drive_sdk/common/ObjectDictionary.hpp @@ -2,31 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include - /*! * Object dictionary: * ID = index (16 bit) @@ -35,278 +35,265 @@ * VAL = pre-defined values */ - // DSP402 -#define OD_DSP402_MODEL_ID (uint16_t(0x1008)) -#define OD_DSP402_RX_PDO_ID (uint16_t(0x1C12)) -#define OD_DSP402_RX_PDO_SID (uint8_t(0x01)) -#define OD_DSP402_RX_PDO_SID_VAL_A (uint16_t(0x1600)) -#define OD_DSP402_RX_PDO_SID_VAL_B (uint16_t(0x1601)) -#define OD_DSP402_RX_PDO_SID_VAL_C (uint16_t(0x1602)) -#define OD_DSP402_RX_PDO_SID_VAL_D (uint16_t(0x1603)) -#define OD_DSP402_TX_PDO_ID (uint16_t(0x1C13)) -#define OD_DSP402_TX_PDO_SID (uint8_t(0x01)) -#define OD_DSP402_TX_PDO_SID_VAL_A (uint16_t(0x1A00)) -#define OD_DSP402_TX_PDO_SID_VAL_B (uint16_t(0x1A01)) -#define OD_DSP402_TX_PDO_SID_VAL_C (uint16_t(0x1A02)) -#define OD_DSP402_TX_PDO_SID_VAL_D (uint16_t(0x1A03)) -#define OD_DSP402_TX_PDO_SID_VAL_E (uint16_t(0x1A04)) -#define OD_DSP402_MEASURED_TEMPERATURE_ID (uint16_t(0x2002)) -#define OD_DSP402_MEASURED_MOTOR_VOLTAGE_ID (uint16_t(0x2003)) -#define OD_DSP402_MEASURED_GEAR_POSITION_ID (uint16_t(0x2005)) -#define OD_DSP402_MEASURED_JOINT_POSITION_ID (uint16_t(0x2006)) -#define OD_DSP402_MEASURED_MOTOR_CURRENT_ID (uint16_t(0x2007)) -#define OD_DSP402_MEASURED_GEAR_VELOCITY_ID (uint16_t(0x2009)) -#define OD_DSP402_MEASURED_JOINT_VELOCITY_ID (uint16_t(0x200A)) -#define OD_DSP402_MEASURED_JOINT_ACCELERATION_ID (uint16_t(0x200B)) -#define OD_DSP402_DESIRED_MOTOR_CURRENT_ID (uint16_t(0x2020)) -#define OD_DSP402_CONTROL_PARAMETER_A_ID (uint16_t(0x2024)) -#define OD_DSP402_CONTROL_PARAMETER_B_ID (uint16_t(0x2025)) -#define OD_DSP402_CONTROL_PARAMETER_C_ID (uint16_t(0x2026)) -#define OD_DSP402_CONTROL_PARAMETER_D_ID (uint16_t(0x2027)) -#define OD_DSP402_ERROR_CODE_ID (uint16_t(0x603F)) -#define OD_DSP402_CONTROL_WORD_ID (uint16_t(0x6040)) -#define OD_DSP402_STATUS_WORD_ID (uint16_t(0x6041)) -#define OD_DSP402_MAX_COMMUNICATION_TIMEOUT_ID (uint16_t(0x6046)) -#define OD_DSP402_QUICK_STOP_OPTION_CODE_ID (uint16_t(0x605A)) -#define OD_DSP402_SHUTDOWN_OPTION_CODE_ID (uint16_t(0x605B)) -#define OD_DSP402_DISABLE_OPERATION_OPTION_CODE_ID (uint16_t(0x605C)) -#define OD_DSP402_FAULT_REACTION_CODE_ID (uint16_t(0x605E)) -#define OD_DSP402_MODES_OF_OPERATION_ID (uint16_t(0x6060)) -#define OD_DSP402_MODES_OF_OPERATION_DISPLAY_ID (uint16_t(0x6061)) -#define OD_DSP402_MEASURED_MOTOR_POSITION_ID (uint16_t(0x6064)) -#define OD_DSP402_MEASURED_MOTOR_VELOCITY_ID (uint16_t(0x606C)) -#define OD_DSP402_DESIRED_JOINT_TORQUE_ID (uint16_t(0x6071)) -#define OD_DSP402_JOINT_TORQUE_MAX_ID (uint16_t(0x6072)) -#define OD_DSP402_CURRENT_MAX_ID (uint16_t(0x6073)) -#define OD_DSP402_MEASURED_JOINT_TORQUE_ID (uint16_t(0x6077)) -#define OD_DSP402_DESIRED_JOINT_POSITION_ID (uint16_t(0x607A)) -#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID (uint16_t(0x607B)) -#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN (uint8_t(0x01)) -#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX (uint8_t(0x02)) -#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID (uint16_t(0x607D)) -#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN (uint8_t(0x01)) -#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX (uint8_t(0x02)) -#define OD_DSP402_MOTOR_VELOCITY_MAX_ID (uint16_t(0x6080)) -#define OD_DSP402_QUICK_STOP_DECLARATION_ID (uint16_t(0x6085)) -#define OD_DSP402_INTERPOLATION_TIME_PERIOD_ID (uint16_t(0x60C2)) -#define OD_DSP402_DESIRED_JOINT_VELOCITY_ID (uint16_t(0x60FF)) -#define OD_DSP402_SUPPORTED_DRIVE_MODES_ID (uint16_t(0x6502)) +#define OD_DSP402_MODEL_ID (uint16_t(0x1008)) +#define OD_DSP402_RX_PDO_ID (uint16_t(0x1C12)) +#define OD_DSP402_RX_PDO_SID (uint8_t(0x01)) +#define OD_DSP402_RX_PDO_SID_VAL_A (uint16_t(0x1600)) +#define OD_DSP402_RX_PDO_SID_VAL_B (uint16_t(0x1601)) +#define OD_DSP402_RX_PDO_SID_VAL_C (uint16_t(0x1602)) +#define OD_DSP402_RX_PDO_SID_VAL_D (uint16_t(0x1603)) +#define OD_DSP402_TX_PDO_ID (uint16_t(0x1C13)) +#define OD_DSP402_TX_PDO_SID (uint8_t(0x01)) +#define OD_DSP402_TX_PDO_SID_VAL_A (uint16_t(0x1A00)) +#define OD_DSP402_TX_PDO_SID_VAL_B (uint16_t(0x1A01)) +#define OD_DSP402_TX_PDO_SID_VAL_C (uint16_t(0x1A02)) +#define OD_DSP402_TX_PDO_SID_VAL_D (uint16_t(0x1A03)) +#define OD_DSP402_TX_PDO_SID_VAL_E (uint16_t(0x1A04)) +#define OD_DSP402_MEASURED_TEMPERATURE_ID (uint16_t(0x2002)) +#define OD_DSP402_MEASURED_MOTOR_VOLTAGE_ID (uint16_t(0x2003)) +#define OD_DSP402_MEASURED_GEAR_POSITION_ID (uint16_t(0x2005)) +#define OD_DSP402_MEASURED_JOINT_POSITION_ID (uint16_t(0x2006)) +#define OD_DSP402_MEASURED_MOTOR_CURRENT_ID (uint16_t(0x2007)) +#define OD_DSP402_MEASURED_GEAR_VELOCITY_ID (uint16_t(0x2009)) +#define OD_DSP402_MEASURED_JOINT_VELOCITY_ID (uint16_t(0x200A)) +#define OD_DSP402_MEASURED_JOINT_ACCELERATION_ID (uint16_t(0x200B)) +#define OD_DSP402_DESIRED_MOTOR_CURRENT_ID (uint16_t(0x2020)) +#define OD_DSP402_CONTROL_PARAMETER_A_ID (uint16_t(0x2024)) +#define OD_DSP402_CONTROL_PARAMETER_B_ID (uint16_t(0x2025)) +#define OD_DSP402_CONTROL_PARAMETER_C_ID (uint16_t(0x2026)) +#define OD_DSP402_CONTROL_PARAMETER_D_ID (uint16_t(0x2027)) +#define OD_DSP402_ERROR_CODE_ID (uint16_t(0x603F)) +#define OD_DSP402_CONTROL_WORD_ID (uint16_t(0x6040)) +#define OD_DSP402_STATUS_WORD_ID (uint16_t(0x6041)) +#define OD_DSP402_MAX_COMMUNICATION_TIMEOUT_ID (uint16_t(0x6046)) +#define OD_DSP402_QUICK_STOP_OPTION_CODE_ID (uint16_t(0x605A)) +#define OD_DSP402_SHUTDOWN_OPTION_CODE_ID (uint16_t(0x605B)) +#define OD_DSP402_DISABLE_OPERATION_OPTION_CODE_ID (uint16_t(0x605C)) +#define OD_DSP402_FAULT_REACTION_CODE_ID (uint16_t(0x605E)) +#define OD_DSP402_MODES_OF_OPERATION_ID (uint16_t(0x6060)) +#define OD_DSP402_MODES_OF_OPERATION_DISPLAY_ID (uint16_t(0x6061)) +#define OD_DSP402_MEASURED_MOTOR_POSITION_ID (uint16_t(0x6064)) +#define OD_DSP402_MEASURED_MOTOR_VELOCITY_ID (uint16_t(0x606C)) +#define OD_DSP402_DESIRED_JOINT_TORQUE_ID (uint16_t(0x6071)) +#define OD_DSP402_JOINT_TORQUE_MAX_ID (uint16_t(0x6072)) +#define OD_DSP402_CURRENT_MAX_ID (uint16_t(0x6073)) +#define OD_DSP402_MEASURED_JOINT_TORQUE_ID (uint16_t(0x6077)) +#define OD_DSP402_DESIRED_JOINT_POSITION_ID (uint16_t(0x607A)) +#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID (uint16_t(0x607B)) +#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN (uint8_t(0x01)) +#define OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX (uint8_t(0x02)) +#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID (uint16_t(0x607D)) +#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN (uint8_t(0x01)) +#define OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX (uint8_t(0x02)) +#define OD_DSP402_MOTOR_VELOCITY_MAX_ID (uint16_t(0x6080)) +#define OD_DSP402_QUICK_STOP_DECLARATION_ID (uint16_t(0x6085)) +#define OD_DSP402_INTERPOLATION_TIME_PERIOD_ID (uint16_t(0x60C2)) +#define OD_DSP402_DESIRED_JOINT_VELOCITY_ID (uint16_t(0x60FF)) +#define OD_DSP402_SUPPORTED_DRIVE_MODES_ID (uint16_t(0x6502)) // Gains -#define OD_GAINS_CURRENT_CTRL_ID (uint16_t(0x7000)) -#define OD_GAINS_MOTOR_POSITION_CTRL_ID (uint16_t(0x7001)) -#define OD_GAINS_MOTOR_VELOCITY_CTRL_ID (uint16_t(0x7002)) -#define OD_GAINS_GEAR_POSITION_CTRL_ID (uint16_t(0x7003)) -#define OD_GAINS_GEAR_VELOCITY_CTRL_ID (uint16_t(0x7004)) -#define OD_GAINS_JOINT_POSITION_CTRL_ID (uint16_t(0x7005)) -#define OD_GAINS_JOINT_VELOCITY_CTRL_ID (uint16_t(0x7006)) -#define OD_GAINS_JOINT_TORQUE_CTRL_ID (uint16_t(0x7007)) -#define OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_CTRL_ID (uint16_t(0x7008)) -#define OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_PID_GAINS_CTRL_ID (uint16_t(0x7009)) -#define OD_GAINS_JOINT_POSITION_VELOCITY_CTRL_ID (uint16_t(0x7010)) -#define OD_GAINS_COMMON_SID_P (uint8_t(0x01)) -#define OD_GAINS_COMMON_SID_I (uint8_t(0x02)) -#define OD_GAINS_COMMON_SID_D (uint8_t(0x03)) +#define OD_GAINS_CURRENT_CTRL_ID (uint16_t(0x7000)) +#define OD_GAINS_MOTOR_POSITION_CTRL_ID (uint16_t(0x7001)) +#define OD_GAINS_MOTOR_VELOCITY_CTRL_ID (uint16_t(0x7002)) +#define OD_GAINS_GEAR_POSITION_CTRL_ID (uint16_t(0x7003)) +#define OD_GAINS_GEAR_VELOCITY_CTRL_ID (uint16_t(0x7004)) +#define OD_GAINS_JOINT_POSITION_CTRL_ID (uint16_t(0x7005)) +#define OD_GAINS_JOINT_VELOCITY_CTRL_ID (uint16_t(0x7006)) +#define OD_GAINS_JOINT_TORQUE_CTRL_ID (uint16_t(0x7007)) +#define OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_CTRL_ID (uint16_t(0x7008)) +#define OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_PID_GAINS_CTRL_ID (uint16_t(0x7009)) +#define OD_GAINS_JOINT_POSITION_VELOCITY_CTRL_ID (uint16_t(0x7010)) +#define OD_GAINS_COMMON_SID_P (uint8_t(0x01)) +#define OD_GAINS_COMMON_SID_I (uint8_t(0x02)) +#define OD_GAINS_COMMON_SID_D (uint8_t(0x03)) // Extended PDO -#define OD_EXT_PDO_MEASURED_GEAR_POSITION_TICKS_ID (uint16_t(0x7020)) -#define OD_EXT_PDO_MEASURED_JOINT_POSITION_TICKS_ID (uint16_t(0x7021)) -#define OD_EXT_PDO_TIMESTAMP_ID (uint16_t(0x7022)) -#define OD_EXT_PDO_DESIRED_CURRENT_D_ID (uint16_t(0x7023)) -#define OD_EXT_PDO_MEASURED_CURRENT_D_ID (uint16_t(0x7024)) -#define OD_EXT_PDO_DESIRED_CURRENT_Q_ID (uint16_t(0x7025)) -#define OD_EXT_PDO_MEASURED_CURRENT_Q_ID (uint16_t(0x7026)) -#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_U_ID (uint16_t(0x7027)) -#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_U_ID (uint16_t(0x7028)) -#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_V_ID (uint16_t(0x7029)) -#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_V_ID (uint16_t(0x702A)) -#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_W_ID (uint16_t(0x702B)) -#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_W_ID (uint16_t(0x702C)) -#define OD_EXT_PDO_VALUE_1_ID (uint16_t(0x702D)) -#define OD_EXT_PDO_VALUE_2_ID (uint16_t(0x702E)) -#define OD_EXT_PDO_VALUE_3_ID (uint16_t(0x702F)) -#define OD_EXT_PDO_VALUE_4_ID (uint16_t(0x7030)) -#define OD_EXT_PDO_VALUE_5_ID (uint16_t(0x7031)) -#define OD_EXT_PDO_VALUE_6_ID (uint16_t(0x7032)) +#define OD_EXT_PDO_MEASURED_GEAR_POSITION_TICKS_ID (uint16_t(0x7020)) +#define OD_EXT_PDO_MEASURED_JOINT_POSITION_TICKS_ID (uint16_t(0x7021)) +#define OD_EXT_PDO_TIMESTAMP_ID (uint16_t(0x7022)) +#define OD_EXT_PDO_DESIRED_CURRENT_D_ID (uint16_t(0x7023)) +#define OD_EXT_PDO_MEASURED_CURRENT_D_ID (uint16_t(0x7024)) +#define OD_EXT_PDO_DESIRED_CURRENT_Q_ID (uint16_t(0x7025)) +#define OD_EXT_PDO_MEASURED_CURRENT_Q_ID (uint16_t(0x7026)) +#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_U_ID (uint16_t(0x7027)) +#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_U_ID (uint16_t(0x7028)) +#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_V_ID (uint16_t(0x7029)) +#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_V_ID (uint16_t(0x702A)) +#define OD_EXT_PDO_MEASURED_CURRENT_PHASE_W_ID (uint16_t(0x702B)) +#define OD_EXT_PDO_MEASURED_VOLTAGE_PHASE_W_ID (uint16_t(0x702C)) +#define OD_EXT_PDO_VALUE_1_ID (uint16_t(0x702D)) +#define OD_EXT_PDO_VALUE_2_ID (uint16_t(0x702E)) +#define OD_EXT_PDO_VALUE_3_ID (uint16_t(0x702F)) +#define OD_EXT_PDO_VALUE_4_ID (uint16_t(0x7030)) +#define OD_EXT_PDO_VALUE_5_ID (uint16_t(0x7031)) +#define OD_EXT_PDO_VALUE_6_ID (uint16_t(0x7032)) // Calibration -#define OD_CALIB_MODE_ID (uint16_t(0x7050)) -#define OD_CALIB_MODE_ID_VAL_IDLE (uint16_t(0x0000)) -#define OD_CALIB_MODE_ID_VAL_MOTOR_ENCODER_OFFSET (uint16_t(0x0001)) -#define OD_CALIB_MODE_ID_VAL_MOTOR_ENCODER_PARAMETERS (uint16_t(0x0002)) -#define OD_CALIB_MODE_ID_VAL_GEAR_JOINT_ENCODER_OFFSET (uint16_t(0x0003)) -#define OD_CALIB_MODE_ID_VAL_GEAR_AND_JOINT_ENCODER_HOMING (uint16_t(0x0004)) -#define OD_CALIB_MODE_ID_VAL_GEAR_JOINT_ENCODER_OFFSET_END (uint16_t(0x0005)) -#define OD_CALIB_MODE_ID_VAL_IMU_GYROSCOPE_DC_BIAS (uint16_t(0x0006)) -#define OD_CALIB_MODE_ID_VAL_FRICTION_ESTIMATION (uint16_t(0x0007)) -#define OD_CALIB_MODE_ID_VAL_FRICTION_ESTIMATION_END (uint16_t(0x0008)) -#define OD_CALIB_MODE_ID_VAL_SPRING_STIFFNESS (uint16_t(0x0009)) -#define OD_CALIB_MODE_ID_VAL_SPRING_STIFFNESS_END (uint16_t(0x000A)) -#define OD_CALIB_MODE_ID_VAL_AKSIM_SELF_CALIB (uint16_t(0x000B)) -#define OD_CALIB_STATES_ID (uint16_t(0x7051)) -#define OD_CALIB_FACTORY_TO_CUSTOM_ID (uint16_t(0x7052)) -#define OD_CALIB_CUSTOM_TO_FACTORY_ID (uint16_t(0x7053)) -#define OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_IDLE (uint16_t(0x0000)) -#define OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_RUN (uint16_t(0x0001)) -#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_NEW_JOINT_POSITION_ID (uint16_t(0x7054)) -#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID (uint16_t(0x7055)) -#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR (uint8_t(0x01)) -#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT (uint8_t(0x02)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID (uint16_t(0x7056)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT (uint8_t(0x01)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_AMPLITUDE (uint8_t(0x02)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_PHASESHIFT (uint8_t(0x03)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_AMPLITUDE (uint8_t(0x04)) -#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_PHASESHIFT (uint8_t(0x05)) -#define OD_CALIB_SPRING_STIFFNESS_ID (uint16_t(0x7057)) -#define OD_CALIB_SPRING_STIFFNESS_SID_NEG (uint8_t(0x01)) -#define OD_CALIB_SPRING_STIFFNESS_SID_POS (uint8_t(0x02)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_ID (uint16_t(0x7058)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_OFFSET (uint8_t(0x01)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DGAIN (uint8_t(0x02)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFS (uint8_t(0x03)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFC (uint8_t(0x04)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DPH (uint8_t(0x05)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AGAIN (uint8_t(0x06)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFS (uint8_t(0x07)) -#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFC (uint8_t(0x08)) -#define OD_CALIB_FRICTION_ESTIMATION_ID (uint16_t(0x7059)) -#define OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION (uint8_t(0x01)) -#define OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND (uint8_t(0x02)) -#define OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG (uint8_t(0x03)) -#define OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS (uint8_t(0x04)) -#define OD_CALIB_SELECTION_ID (uint16_t(0x705A)) -#define OD_CALIB_SELECTION_ID_VAL_CUSTOM (uint16_t(0x0000)) -#define OD_CALIB_SELECTION_ID_VAL_FACTORY (uint16_t(0x0001)) -#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID (uint16_t(0x705B)) -#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_X (uint8_t(0x01)) -#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Y (uint8_t(0x02)) -#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Z (uint8_t(0x03)) +#define OD_CALIB_MODE_ID (uint16_t(0x7050)) +#define OD_CALIB_MODE_ID_VAL_IDLE (uint16_t(0x0000)) +#define OD_CALIB_MODE_ID_VAL_MOTOR_ENCODER_OFFSET (uint16_t(0x0001)) +#define OD_CALIB_MODE_ID_VAL_MOTOR_ENCODER_PARAMETERS (uint16_t(0x0002)) +#define OD_CALIB_MODE_ID_VAL_GEAR_JOINT_ENCODER_OFFSET (uint16_t(0x0003)) +#define OD_CALIB_MODE_ID_VAL_GEAR_AND_JOINT_ENCODER_HOMING (uint16_t(0x0004)) +#define OD_CALIB_MODE_ID_VAL_GEAR_JOINT_ENCODER_OFFSET_END (uint16_t(0x0005)) +#define OD_CALIB_MODE_ID_VAL_IMU_GYROSCOPE_DC_BIAS (uint16_t(0x0006)) +#define OD_CALIB_MODE_ID_VAL_FRICTION_ESTIMATION (uint16_t(0x0007)) +#define OD_CALIB_MODE_ID_VAL_FRICTION_ESTIMATION_END (uint16_t(0x0008)) +#define OD_CALIB_MODE_ID_VAL_SPRING_STIFFNESS (uint16_t(0x0009)) +#define OD_CALIB_MODE_ID_VAL_SPRING_STIFFNESS_END (uint16_t(0x000A)) +#define OD_CALIB_MODE_ID_VAL_AKSIM_SELF_CALIB (uint16_t(0x000B)) +#define OD_CALIB_STATES_ID (uint16_t(0x7051)) +#define OD_CALIB_FACTORY_TO_CUSTOM_ID (uint16_t(0x7052)) +#define OD_CALIB_CUSTOM_TO_FACTORY_ID (uint16_t(0x7053)) +#define OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_IDLE (uint16_t(0x0000)) +#define OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_RUN (uint16_t(0x0001)) +#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_NEW_JOINT_POSITION_ID (uint16_t(0x7054)) +#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID (uint16_t(0x7055)) +#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR (uint8_t(0x01)) +#define OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT (uint8_t(0x02)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID (uint16_t(0x7056)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT (uint8_t(0x01)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_AMPLITUDE (uint8_t(0x02)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_PHASESHIFT (uint8_t(0x03)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_AMPLITUDE (uint8_t(0x04)) +#define OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_PHASESHIFT (uint8_t(0x05)) +#define OD_CALIB_SPRING_STIFFNESS_ID (uint16_t(0x7057)) +#define OD_CALIB_SPRING_STIFFNESS_SID_NEG (uint8_t(0x01)) +#define OD_CALIB_SPRING_STIFFNESS_SID_POS (uint8_t(0x02)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_ID (uint16_t(0x7058)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_OFFSET (uint8_t(0x01)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DGAIN (uint8_t(0x02)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFS (uint8_t(0x03)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFC (uint8_t(0x04)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DPH (uint8_t(0x05)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AGAIN (uint8_t(0x06)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFS (uint8_t(0x07)) +#define OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFC (uint8_t(0x08)) +#define OD_CALIB_FRICTION_ESTIMATION_ID (uint16_t(0x7059)) +#define OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION (uint8_t(0x01)) +#define OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND (uint8_t(0x02)) +#define OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG (uint8_t(0x03)) +#define OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS (uint8_t(0x04)) +#define OD_CALIB_SELECTION_ID (uint16_t(0x705A)) +#define OD_CALIB_SELECTION_ID_VAL_CUSTOM (uint16_t(0x0000)) +#define OD_CALIB_SELECTION_ID_VAL_FACTORY (uint16_t(0x0001)) +#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID (uint16_t(0x705B)) +#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_X (uint8_t(0x01)) +#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Y (uint8_t(0x02)) +#define OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Z (uint8_t(0x03)) // Drive info -#define OD_DRIVE_INFO_FIRMWARE_VERSION_ID (uint16_t(0x7070)) -#define OD_DRIVE_INFO_BOOTLOADER_VERSION_ID (uint16_t(0x7071)) -#define OD_DRIVE_INFO_HARDWARE_SERIAL_NUMBER_ID (uint16_t(0x7072)) -#define OD_DRIVE_INFO_DRIVE_NAME_ID (uint16_t(0x7073)) -#define OD_DRIVE_INFO_DRIVE_ID_ID (uint16_t(0x7074)) -#define OD_DRIVE_FIRMWARE_INFO_ID (uint16_t(0x7075)) -#define OD_DRIVE_FIRMWARE_INFO_SID_0 (uint8_t(0x00)) -#define OD_DRIVE_FIRMWARE_INFO_SID_DATA (uint8_t(0x01)) -#define OD_GEARBOX_RATIO_ID (uint16_t(0x7076)) -#define OD_BUILDINFO_ID (uint16_t(0x7078)) -#define OD_DRIVETYPE_ID (uint16_t(0x7077)) +#define OD_DRIVE_INFO_FIRMWARE_VERSION_ID (uint16_t(0x7070)) +#define OD_DRIVE_INFO_BOOTLOADER_VERSION_ID (uint16_t(0x7071)) +#define OD_DRIVE_INFO_HARDWARE_SERIAL_NUMBER_ID (uint16_t(0x7072)) +#define OD_DRIVE_INFO_DRIVE_NAME_ID (uint16_t(0x7073)) +#define OD_DRIVE_INFO_DRIVE_ID_ID (uint16_t(0x7074)) +#define OD_DRIVE_FIRMWARE_INFO_ID (uint16_t(0x7075)) +#define OD_DRIVE_FIRMWARE_INFO_SID_0 (uint8_t(0x00)) +#define OD_DRIVE_FIRMWARE_INFO_SID_DATA (uint8_t(0x01)) +#define OD_GEARBOX_RATIO_ID (uint16_t(0x7076)) +#define OD_BUILDINFO_ID (uint16_t(0x7078)) +#define OD_DRIVETYPE_ID (uint16_t(0x7077)) // Flash -#define OD_FLASH_ERASE_ID (uint16_t(0x70A0)) -#define OD_FLASH_ERASE_ID_VAL_IDLE (uint16_t(0x0000)) -#define OD_FLASH_ERASE_ID_VAL_RUN (uint16_t(0x0001)) -#define OD_FLASH_RESET_ID (uint16_t(0x70A1)) -#define OD_FLASH_RESET_ID_VAL_RESET_CALIBRATION (uint16_t(0x0000)) -#define OD_FLASH_RESET_ID_VAL_RESET_CONFIGURATION (uint16_t(0x0001)) -#define OD_FLASH_WRITE_CONFIGURATION_ID (uint16_t(0x70A2)) -#define OD_FLASH_WRITE_CONFIGURATION_ID_VAL_IDLE (uint16_t(0x0000)) -#define OD_FLASH_WRITE_CONFIGURATION_ID_VAL_RUN (uint16_t(0x0001)) +#define OD_FLASH_ERASE_ID (uint16_t(0x70A0)) +#define OD_FLASH_ERASE_ID_VAL_IDLE (uint16_t(0x0000)) +#define OD_FLASH_ERASE_ID_VAL_RUN (uint16_t(0x0001)) +#define OD_FLASH_RESET_ID (uint16_t(0x70A1)) +#define OD_FLASH_RESET_ID_VAL_RESET_CALIBRATION (uint16_t(0x0000)) +#define OD_FLASH_RESET_ID_VAL_RESET_CONFIGURATION (uint16_t(0x0001)) +#define OD_FLASH_WRITE_CONFIGURATION_ID (uint16_t(0x70A2)) +#define OD_FLASH_WRITE_CONFIGURATION_ID_VAL_IDLE (uint16_t(0x0000)) +#define OD_FLASH_WRITE_CONFIGURATION_ID_VAL_RUN (uint16_t(0x0001)) // FSM -#define OD_FSM_ERROR_BEHAVIOR_ID (uint16_t(0x70C0)) -#define OD_FSM_ERROR_BEHAVIOR_ID_VAL_FREEZE (uint16_t(0x0000)) -#define OD_FSM_ERROR_BEHAVIOR_ID_VAL_DISABLE_BRIDGE (uint16_t(0x0001)) +#define OD_FSM_ERROR_BEHAVIOR_ID (uint16_t(0x70C0)) +#define OD_FSM_ERROR_BEHAVIOR_ID_VAL_FREEZE (uint16_t(0x0000)) +#define OD_FSM_ERROR_BEHAVIOR_ID_VAL_DISABLE_BRIDGE (uint16_t(0x0001)) // Filter -#define OD_FILTER_GEAR_JOINT_VELOCITY_ID (uint16_t(0x70E0)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE (uint8_t(0x01)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_DISABLED (uint32_t( \ - 0x00000000)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_KALMAN (uint32_t( \ - 0x00000001)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_IIR (uint32_t( \ - 0x00000002)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_EMA (uint32_t( \ - 0x00000002)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2 (uint8_t(0x03)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA (uint8_t(0x04)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA (uint8_t(0x05)) -#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA (uint8_t(0x05)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID (uint16_t(0x70E1)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE (uint8_t(0x01)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_DISABLED (uint32_t( \ - 0x00000000)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_KALMAN (uint32_t( \ - 0x00000001)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_IIR (uint32_t( \ - 0x00000002)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_EMA (uint32_t( \ - 0x00000002)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2 (uint8_t(0x03)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA (uint8_t(0x04)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA (uint8_t(0x05)) -#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA (uint8_t(0x05)) -#define OD_FILTER_JOINT_ACCELERATION_ID (uint16_t(0x70E2)) -#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE (uint8_t(0x01)) -#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_DISABLED (uint32_t( \ - 0x00000000)) -#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_KALMAN (uint32_t( \ - 0x00000001)) -#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_IIR (uint32_t( \ - 0x00000002)) -#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_EMA (uint32_t( \ - 0x00000002)) -#define OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) -#define OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2 (uint8_t(0x03)) -#define OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA (uint8_t(0x04)) -#define OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA (uint8_t(0x05)) -#define OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA (uint8_t(0x05)) -#define OD_FILTER_D_GAIN (uint16_t(0x7604)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_ID (uint16_t(0x70E0)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE (uint8_t(0x01)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_DISABLED (uint32_t(0x00000000)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_KALMAN (uint32_t(0x00000001)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_IIR (uint32_t(0x00000002)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE_VAL_EMA (uint32_t(0x00000002)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2 (uint8_t(0x03)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA (uint8_t(0x04)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA (uint8_t(0x05)) +#define OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA (uint8_t(0x05)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID (uint16_t(0x70E1)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE (uint8_t(0x01)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_DISABLED (uint32_t(0x00000000)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_KALMAN (uint32_t(0x00000001)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_IIR (uint32_t(0x00000002)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE_VAL_EMA (uint32_t(0x00000002)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2 (uint8_t(0x03)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA (uint8_t(0x04)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA (uint8_t(0x05)) +#define OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA (uint8_t(0x05)) +#define OD_FILTER_JOINT_ACCELERATION_ID (uint16_t(0x70E2)) +#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE (uint8_t(0x01)) +#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_DISABLED (uint32_t(0x00000000)) +#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_KALMAN (uint32_t(0x00000001)) +#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_IIR (uint32_t(0x00000002)) +#define OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE_VAL_EMA (uint32_t(0x00000002)) +#define OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE (uint8_t(0x02)) +#define OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2 (uint8_t(0x03)) +#define OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA (uint8_t(0x04)) +#define OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA (uint8_t(0x05)) +#define OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA (uint8_t(0x05)) +#define OD_FILTER_D_GAIN (uint16_t(0x7604)) // Control -#define OD_CONTROL_DIRECTION_ID (uint16_t(0x7100)) -#define OD_CONTROL_RTDL_CONTROL_ID (uint16_t(0x7101)) -#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE (uint8_t(0x01)) -#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_DISABLE (uint16_t(0x00)) -#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_ENABLE (uint16_t(0x01)) -#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND (uint8_t(0x02)) -#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_STOP (uint16_t(0x00)) -#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_RESET (uint16_t(0x01)) -#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_LOG (uint16_t(0x02)) -#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_STREAM (uint16_t(0x03)) -#define OD_CONTROL_RTDL_CONTROL_SID_STATUS (uint8_t(0x03)) -#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_IDLE (uint16_t(0x00)) -#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_LOGGING (uint16_t(0x01)) -#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_STREAMING (uint16_t(0x02)) -#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY (uint8_t(0x04)) -#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY_VAL_2_5_Hz (uint16_t(0x00)) -#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY_VAL_10_Hz (uint16_t(0x01)) -#define OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY (uint8_t(0x05)) -#define OD_CONTROL_RTDL_LAST_TIMESTAMP_ID (uint16_t(0x7103)) -#define OD_CONTROL_IMU_CONFIG_ID (uint16_t(0x7105)) -#define OD_CONTROL_IMU_CONFIG_SID_ENABLE (uint8_t(0x01)) -#define OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE (uint8_t(0x02)) -#define OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE (uint8_t(0x03)) -#define OD_CONTROL_FAN_ID (uint16_t(0x7106)) -#define OD_CONTROL_FAN_SID_MODE (uint8_t(0x1)) -#define OD_CONTROL_FAN_SID_INTENSITY (uint8_t(0x2)) -#define OD_CONTROL_FAN_SID_LOWER_TEMPERATURE (uint8_t(0x3)) -#define OD_CONTROL_FAN_SID_UPPER_TEMPERATURE (uint8_t(0x4)) -#define OD_CONTROL_BRAKE_ID (uint16_t(0x7107)) -#define OD_CONTROL_BRAKE_SID_CTRL (uint8_t(0x1)) -#define OD_CONTROL_BRAKE_SID_DUTY (uint8_t(0x2)) -#define OD_CONTROL_BRAKE_SID_CTRL_VAL_ENABLE (uint16_t(0x1)) -#define OD_CONTROL_BRAKE_SID_CTRL_VAL_DISABLE (uint16_t(0x0)) +#define OD_CONTROL_DIRECTION_ID (uint16_t(0x7100)) +#define OD_CONTROL_RTDL_CONTROL_ID (uint16_t(0x7101)) +#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE (uint8_t(0x01)) +#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_DISABLE (uint16_t(0x00)) +#define OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_ENABLE (uint16_t(0x01)) +#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND (uint8_t(0x02)) +#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_STOP (uint16_t(0x00)) +#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_RESET (uint16_t(0x01)) +#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_LOG (uint16_t(0x02)) +#define OD_CONTROL_RTDL_CONTROL_SID_COMMAND_VAL_STREAM (uint16_t(0x03)) +#define OD_CONTROL_RTDL_CONTROL_SID_STATUS (uint8_t(0x03)) +#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_IDLE (uint16_t(0x00)) +#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_LOGGING (uint16_t(0x01)) +#define OD_CONTROL_RTDL_CONTROL_SID_STATUS_VAL_STREAMING (uint16_t(0x02)) +#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY (uint8_t(0x04)) +#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY_VAL_2_5_Hz (uint16_t(0x00)) +#define OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY_VAL_10_Hz (uint16_t(0x01)) +#define OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY (uint8_t(0x05)) +#define OD_CONTROL_RTDL_LAST_TIMESTAMP_ID (uint16_t(0x7103)) +#define OD_CONTROL_IMU_CONFIG_ID (uint16_t(0x7105)) +#define OD_CONTROL_IMU_CONFIG_SID_ENABLE (uint8_t(0x01)) +#define OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE (uint8_t(0x02)) +#define OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE (uint8_t(0x03)) +#define OD_CONTROL_FAN_ID (uint16_t(0x7106)) +#define OD_CONTROL_FAN_SID_MODE (uint8_t(0x1)) +#define OD_CONTROL_FAN_SID_INTENSITY (uint8_t(0x2)) +#define OD_CONTROL_FAN_SID_LOWER_TEMPERATURE (uint8_t(0x3)) +#define OD_CONTROL_FAN_SID_UPPER_TEMPERATURE (uint8_t(0x4)) +#define OD_CONTROL_BRAKE_ID (uint16_t(0x7107)) +#define OD_CONTROL_BRAKE_SID_CTRL (uint8_t(0x1)) +#define OD_CONTROL_BRAKE_SID_DUTY (uint8_t(0x2)) +#define OD_CONTROL_BRAKE_SID_CTRL_VAL_ENABLE (uint16_t(0x1)) +#define OD_CONTROL_BRAKE_SID_CTRL_VAL_DISABLE (uint16_t(0x0)) // Various -#define OD_VARIOUS_PASSWORD_ID (uint16_t(0x7200)) +#define OD_VARIOUS_PASSWORD_ID (uint16_t(0x7200)) // Configuration -#define OD_CONFIG_CURRENT_INTEGRATOR_SATURATION (uint16_t(0x7600)) -#define OD_CONFIG_JOINT_TORQUE_INTEGRATOR_SATURATION (uint16_t(0x7601)) -#define OD_CONFIG_MAX_FREEZE_CURRENT (uint16_t(0x7603)) +#define OD_CONFIG_CURRENT_INTEGRATOR_SATURATION (uint16_t(0x7600)) +#define OD_CONFIG_JOINT_TORQUE_INTEGRATOR_SATURATION (uint16_t(0x7601)) +#define OD_CONFIG_MAX_FREEZE_CURRENT (uint16_t(0x7603)) // Data Logging -#define OD_DATALOGGING (uint16_t(0x7800)) -#define OD_DATALOGGING_HIST_EDGES (uint16_t(0x7801)) +#define OD_DATALOGGING (uint16_t(0x7800)) +#define OD_DATALOGGING_HIST_EDGES (uint16_t(0x7801)) -#define OD_SWITCH_PDO_ID (uint16_t(0xF030)) -#define OD_SWITCH_PDO_SID (uint8_t(0x01)) +#define OD_SWITCH_PDO_ID (uint16_t(0xF030)) +#define OD_SWITCH_PDO_SID (uint8_t(0x01)) diff --git a/include/rsl_drive_sdk/common/Version.hpp b/include/rsl_drive_sdk/common/Version.hpp index b752da5..915d04a 100644 --- a/include/rsl_drive_sdk/common/Version.hpp +++ b/include/rsl_drive_sdk/common/Version.hpp @@ -2,38 +2,37 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include - namespace rsl_drive_sdk { namespace common { - struct Version { int major_ = 0; @@ -57,29 +56,28 @@ struct Version * Create a version number by parsing a string of the form "major.minor.patch". * @param string String. */ - Version(const std::string & string); + explicit Version(const std::string& string); /*! * Create a version number by parsing a string of the form "major.minor.patch". * @param string String. */ - Version & fromString(const std::string & string); + Version& fromString(const std::string& string); /*! * Create a string from a version number. */ std::string toString() const; - bool operator==(const Version & other) const; - bool operator!=(const Version & other) const; - bool operator<(const Version & other) const; - bool operator<=(const Version & other) const; - bool operator>(const Version & other) const; - bool operator>=(const Version & other) const; + bool operator==(const Version& other) const; + bool operator!=(const Version& other) const; + bool operator<(const Version& other) const; + bool operator<=(const Version& other) const; + bool operator>(const Version& other) const; + bool operator>=(const Version& other) const; }; -std::ostream & operator<<(std::ostream & ostream, const Version & version); - +std::ostream& operator<<(std::ostream& ostream, const Version& version); -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/configuration/DriveConfiguration.hpp b/include/rsl_drive_sdk/configuration/DriveConfiguration.hpp index 319fc7d..308e651 100644 --- a/include/rsl_drive_sdk/configuration/DriveConfiguration.hpp +++ b/include/rsl_drive_sdk/configuration/DriveConfiguration.hpp @@ -2,33 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include #include #include +#include +#include // yaml tools #include @@ -39,13 +42,11 @@ #include "rsl_drive_sdk/fsm/StateEnum.hpp" #include "rsl_drive_sdk/mode/ModeBase.hpp" - namespace rsl_drive_sdk { namespace configuration { - class Configuration { protected: @@ -425,7 +426,6 @@ class Configuration */ std::map modes_; - std::optional dgainFilterCutoffFrequency_; /*! @@ -455,13 +455,13 @@ class Configuration public: Configuration(); - Configuration(const Configuration & other); + Configuration(const Configuration& other); virtual ~Configuration() = default; - Configuration & operator=(const Configuration & other); + Configuration& operator=(const Configuration& other); - void fromFile(const std::string & path); - void fromYamlNode(const YAML::Node & yamlNode); + void fromFile(const std::string& path); + void fromYamlNode(const YAML::Node& yamlNode); void setMaxCommandAge(const double maxCommandAge); double getMaxCommandAge() const; @@ -496,21 +496,19 @@ class Configuration void setDirection(const int16_t direction); std::optional getDirection() const; - void setJointPositionLimitsSdk(const common::Limits & limits); + void setJointPositionLimitsSdk(const common::Limits& limits); std::optional getJointPositionLimitsSdk() const; - void setJointPositionLimitsSoft(const common::Limits & limits); + void setJointPositionLimitsSoft(const common::Limits& limits); std::optional getJointPositionLimitsSoft() const; - void setJointPositionLimitsHard(const common::Limits & limits); + void setJointPositionLimitsHard(const common::Limits& limits); std::optional getJointPositionLimitsHard() const; - void addJointPositionConfiguration( - const std::string & jointPositionConfigurationName, - const double jointPositionConfigurationValue); - bool getJointPositionConfigurationValue( - const std::string & jointPositionConfigurationName, - double & jointPositionConfigurationValue) const; + void addJointPositionConfiguration(const std::string& jointPositionConfigurationName, + const double jointPositionConfigurationValue); + bool getJointPositionConfigurationValue(const std::string& jointPositionConfigurationName, + double& jointPositionConfigurationValue) const; std::map getJointPositionConfigurations() const; void setImuEnable(const bool enable); @@ -595,9 +593,8 @@ class Configuration bool getFake() const; protected: - void addMode(const mode::ModeBasePtr & mode); + void addMode(const mode::ModeBasePtr& mode); }; - -} // configuration -} // rsl_drive_sdk +} // namespace configuration +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/configuration/DriveConfigurationParser.hpp b/include/rsl_drive_sdk/configuration/DriveConfigurationParser.hpp index 7960f08..6c625f7 100644 --- a/include/rsl_drive_sdk/configuration/DriveConfigurationParser.hpp +++ b/include/rsl_drive_sdk/configuration/DriveConfigurationParser.hpp @@ -2,36 +2,39 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once +#include + #include "rsl_drive_sdk/configuration/DriveConfiguration.hpp" namespace rsl_drive_sdk { - class DriveConfigurationParser { public: - static configuration::Configuration fromFile(const std::string & path); - static configuration::Configuration fromYamlNode(const YAML::Node & yamlNode); + static configuration::Configuration fromFile(const std::string& path); + static configuration::Configuration fromYamlNode(const YAML::Node& yamlNode); }; -} +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/Controlword.hpp b/include/rsl_drive_sdk/fsm/Controlword.hpp index bbd6583..963d196 100644 --- a/include/rsl_drive_sdk/fsm/Controlword.hpp +++ b/include/rsl_drive_sdk/fsm/Controlword.hpp @@ -2,72 +2,70 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include - namespace rsl_drive_sdk { namespace fsm { - // Controlword IDs. // Note: This mapping must be equal to the mapping in the firmware. -#define RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE (0x05) -#define RSL_DRIVE_CW_ID_CLEAR_ERRORS_TO_MOTOR_OP (0x02) -#define RSL_DRIVE_CW_ID_CLEAR_ERRORS_TO_STANDBY (0x0C) -#define RSL_DRIVE_CW_ID_CONFIGURE_TO_CALIBRATE (0x06) -#define RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY (0x04) -#define RSL_DRIVE_CW_ID_CONTROL_OP_TO_MOTOR_OP (0x09) -#define RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY (0x0B) -#define RSL_DRIVE_CW_ID_MOTOR_OP_TO_CONTROL_OP (0x0A) -#define RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY (0x07) -#define RSL_DRIVE_CW_ID_NA (0x00) -#define RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE (0x03) -#define RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP (0x08) -#define RSL_DRIVE_CW_ID_WARM_RESET (0x01) +#define RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE (0x05) +#define RSL_DRIVE_CW_ID_CLEAR_ERRORS_TO_MOTOR_OP (0x02) +#define RSL_DRIVE_CW_ID_CLEAR_ERRORS_TO_STANDBY (0x0C) +#define RSL_DRIVE_CW_ID_CONFIGURE_TO_CALIBRATE (0x06) +#define RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY (0x04) +#define RSL_DRIVE_CW_ID_CONTROL_OP_TO_MOTOR_OP (0x09) +#define RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY (0x0B) +#define RSL_DRIVE_CW_ID_MOTOR_OP_TO_CONTROL_OP (0x0A) +#define RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY (0x07) +#define RSL_DRIVE_CW_ID_NA (0x00) +#define RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE (0x03) +#define RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP (0x08) +#define RSL_DRIVE_CW_ID_WARM_RESET (0x01) // Controlword names. -#define RSL_DRIVE_CW_NAME_CALIBRATE_TO_CONFIGURE ("Calibrate to Configure") -#define RSL_DRIVE_CW_NAME_CLEAR_ERRORS_TO_MOTOR_OP ("Clear Errors to MotorOp") -#define RSL_DRIVE_CW_NAME_CLEAR_ERRORS_TO_STANDBY ("Clear Errors to Standby") -#define RSL_DRIVE_CW_NAME_CONFIGURE_TO_CALIBRATE ("Configure to Calibrate") -#define RSL_DRIVE_CW_NAME_CONFIGURE_TO_STANDBY ("Configure to Standby") -#define RSL_DRIVE_CW_NAME_CONTROL_OP_TO_MOTOR_OP ("ControlOp to MotorOp") -#define RSL_DRIVE_CW_NAME_CONTROL_OP_TO_STANDBY ("ControlOp to Standby") -#define RSL_DRIVE_CW_NAME_MOTOR_OP_TO_CONTROL_OP ("MotorOp to ControlOp") -#define RSL_DRIVE_CW_NAME_MOTOR_OP_TO_STANDBY ("MotorOp to Standby") -#define RSL_DRIVE_CW_NAME_NA ("N/A") -#define RSL_DRIVE_CW_NAME_STANDBY_TO_CONFIGURE ("Standby to Configure") -#define RSL_DRIVE_CW_NAME_STANDBY_TO_MOTOR_PREOP ("Standby to MotorPreOp") -#define RSL_DRIVE_CW_NAME_WARM_RESET ("Warm Reset") +#define RSL_DRIVE_CW_NAME_CALIBRATE_TO_CONFIGURE ("Calibrate to Configure") +#define RSL_DRIVE_CW_NAME_CLEAR_ERRORS_TO_MOTOR_OP ("Clear Errors to MotorOp") +#define RSL_DRIVE_CW_NAME_CLEAR_ERRORS_TO_STANDBY ("Clear Errors to Standby") +#define RSL_DRIVE_CW_NAME_CONFIGURE_TO_CALIBRATE ("Configure to Calibrate") +#define RSL_DRIVE_CW_NAME_CONFIGURE_TO_STANDBY ("Configure to Standby") +#define RSL_DRIVE_CW_NAME_CONTROL_OP_TO_MOTOR_OP ("ControlOp to MotorOp") +#define RSL_DRIVE_CW_NAME_CONTROL_OP_TO_STANDBY ("ControlOp to Standby") +#define RSL_DRIVE_CW_NAME_MOTOR_OP_TO_CONTROL_OP ("MotorOp to ControlOp") +#define RSL_DRIVE_CW_NAME_MOTOR_OP_TO_STANDBY ("MotorOp to Standby") +#define RSL_DRIVE_CW_NAME_NA ("N/A") +#define RSL_DRIVE_CW_NAME_STANDBY_TO_CONFIGURE ("Standby to Configure") +#define RSL_DRIVE_CW_NAME_STANDBY_TO_MOTOR_PREOP ("Standby to MotorPreOp") +#define RSL_DRIVE_CW_NAME_WARM_RESET ("Warm Reset") std::string controlwordIdToString(const uint16_t controlwordId); -uint16_t controlwordStringToId(const std::string & controlwordString); - +uint16_t controlwordStringToId(const std::string& controlwordString); -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateBase.hpp b/include/rsl_drive_sdk/fsm/StateBase.hpp index 7edd8b6..1095023 100644 --- a/include/rsl_drive_sdk/fsm/StateBase.hpp +++ b/include/rsl_drive_sdk/fsm/StateBase.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include @@ -41,15 +42,14 @@ class DriveEthercatDevice; namespace fsm { - class StateBase { public: bool isDone_ = true; protected: - DriveEthercatDevice & rsl_drive_sdk_; - std::atomic & goalStateEnum_; + DriveEthercatDevice& rsl_drive_sdk_; + std::atomic& goalStateEnum_; StateEnum stateEnum_; std::string name_; @@ -59,11 +59,8 @@ class StateBase StateEnum controlwordSentForState_ = StateEnum::NA; protected: - StateBase( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum, - const StateEnum stateEnum, - const std::map & goalStateEnumToControlword = {}); + StateBase(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum, const StateEnum stateEnum, + const std::map& goalStateEnumToControlword = {}); virtual ~StateBase(); public: @@ -77,15 +74,20 @@ class StateBase std::string getName(); void enterBase(); - virtual void enterDerived() {} + virtual void enterDerived() + { + } void updateBase(); - virtual void updateDerived() {} + virtual void updateDerived() + { + } void leaveBase(); - virtual void leaveDerived() {} + virtual void leaveDerived() + { + } }; using StateBasePtr = std::shared_ptr; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateCalibrate.hpp b/include/rsl_drive_sdk/fsm/StateCalibrate.hpp index f7d7cdc..5d83c2d 100644 --- a/include/rsl_drive_sdk/fsm/StateCalibrate.hpp +++ b/include/rsl_drive_sdk/fsm/StateCalibrate.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateCalibrate : public StateBase { public: - StateCalibrate(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateCalibrate(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateCalibrate(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateColdStart.hpp b/include/rsl_drive_sdk/fsm/StateColdStart.hpp index 69d8f7b..566db64 100644 --- a/include/rsl_drive_sdk/fsm/StateColdStart.hpp +++ b/include/rsl_drive_sdk/fsm/StateColdStart.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateColdStart : public StateBase { public: - StateColdStart(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateColdStart(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateColdStart(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateConfigure.hpp b/include/rsl_drive_sdk/fsm/StateConfigure.hpp index 5c10080..2dcc36c 100644 --- a/include/rsl_drive_sdk/fsm/StateConfigure.hpp +++ b/include/rsl_drive_sdk/fsm/StateConfigure.hpp @@ -2,26 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once +#include // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" @@ -32,7 +35,6 @@ namespace rsl_drive_sdk namespace fsm { - class StateConfigure : public StateBase { protected: @@ -40,7 +42,7 @@ class StateConfigure : public StateBase std::map::iterator step_mode_; public: - StateConfigure(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateConfigure(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateConfigure(); protected: @@ -48,6 +50,5 @@ class StateConfigure : public StateBase void updateDerived(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateControlOp.hpp b/include/rsl_drive_sdk/fsm/StateControlOp.hpp index 2560b87..b9abf01 100644 --- a/include/rsl_drive_sdk/fsm/StateControlOp.hpp +++ b/include/rsl_drive_sdk/fsm/StateControlOp.hpp @@ -2,41 +2,40 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateControlOp : public StateBase { public: - StateControlOp(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateControlOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateControlOp(); protected: @@ -44,6 +43,5 @@ class StateControlOp : public StateBase void leaveDerived(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateDeviceMissing.hpp b/include/rsl_drive_sdk/fsm/StateDeviceMissing.hpp index 21258c6..e422339 100644 --- a/include/rsl_drive_sdk/fsm/StateDeviceMissing.hpp +++ b/include/rsl_drive_sdk/fsm/StateDeviceMissing.hpp @@ -2,41 +2,40 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateDeviceMissing : public StateBase { public: - StateDeviceMissing(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateDeviceMissing(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateDeviceMissing(); protected: @@ -44,6 +43,5 @@ class StateDeviceMissing : public StateBase void leaveDerived(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateEnum.hpp b/include/rsl_drive_sdk/fsm/StateEnum.hpp index 0c71c6a..5e02508 100644 --- a/include/rsl_drive_sdk/fsm/StateEnum.hpp +++ b/include/rsl_drive_sdk/fsm/StateEnum.hpp @@ -2,67 +2,66 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include - namespace rsl_drive_sdk { namespace fsm { - // FSM state IDs. // NOTE: This mapping must be equal to the mapping in the firmware. -#define RSL_DRIVE_STATE_ID_CALIBRATE (4) -#define RSL_DRIVE_STATE_ID_COLDSTART (1) -#define RSL_DRIVE_STATE_ID_CONFIGURE (3) -#define RSL_DRIVE_STATE_ID_CONTROLOP (7) -#define RSL_DRIVE_STATE_ID_DEVICE_MISSING (11) -#define RSL_DRIVE_STATE_ID_ERROR (8) -#define RSL_DRIVE_STATE_ID_FATAL (9) -#define RSL_DRIVE_STATE_ID_MOTOROP (6) -#define RSL_DRIVE_STATE_ID_MOTORPREOP (10) -#define RSL_DRIVE_STATE_ID_NA (0) -#define RSL_DRIVE_STATE_ID_STANDBY (5) -#define RSL_DRIVE_STATE_ID_WARMSTART (2) +#define RSL_DRIVE_STATE_ID_CALIBRATE (4) +#define RSL_DRIVE_STATE_ID_COLDSTART (1) +#define RSL_DRIVE_STATE_ID_CONFIGURE (3) +#define RSL_DRIVE_STATE_ID_CONTROLOP (7) +#define RSL_DRIVE_STATE_ID_DEVICE_MISSING (11) +#define RSL_DRIVE_STATE_ID_ERROR (8) +#define RSL_DRIVE_STATE_ID_FATAL (9) +#define RSL_DRIVE_STATE_ID_MOTOROP (6) +#define RSL_DRIVE_STATE_ID_MOTORPREOP (10) +#define RSL_DRIVE_STATE_ID_NA (0) +#define RSL_DRIVE_STATE_ID_STANDBY (5) +#define RSL_DRIVE_STATE_ID_WARMSTART (2) // FSM state names. -#define RSL_DRIVE_STATE_NAME_CALIBRATE ("Calibrate") -#define RSL_DRIVE_STATE_NAME_COLDSTART ("ColdStart") -#define RSL_DRIVE_STATE_NAME_CONFIGURE ("Configure") -#define RSL_DRIVE_STATE_NAME_CONTROLOP ("ControlOp") -#define RSL_DRIVE_STATE_NAME_DEVICE_MISSING ("DeviceMissing") -#define RSL_DRIVE_STATE_NAME_ERROR ("Error") -#define RSL_DRIVE_STATE_NAME_FATAL ("Fatal") -#define RSL_DRIVE_STATE_NAME_MOTOROP ("MotorOp") -#define RSL_DRIVE_STATE_NAME_MOTORPREOP ("MotorPreOp") -#define RSL_DRIVE_STATE_NAME_NA ("N/A") -#define RSL_DRIVE_STATE_NAME_STANDBY ("Standby") -#define RSL_DRIVE_STATE_NAME_WARMSTART ("WarmStart") +#define RSL_DRIVE_STATE_NAME_CALIBRATE ("Calibrate") +#define RSL_DRIVE_STATE_NAME_COLDSTART ("ColdStart") +#define RSL_DRIVE_STATE_NAME_CONFIGURE ("Configure") +#define RSL_DRIVE_STATE_NAME_CONTROLOP ("ControlOp") +#define RSL_DRIVE_STATE_NAME_DEVICE_MISSING ("DeviceMissing") +#define RSL_DRIVE_STATE_NAME_ERROR ("Error") +#define RSL_DRIVE_STATE_NAME_FATAL ("Fatal") +#define RSL_DRIVE_STATE_NAME_MOTOROP ("MotorOp") +#define RSL_DRIVE_STATE_NAME_MOTORPREOP ("MotorPreOp") +#define RSL_DRIVE_STATE_NAME_NA ("N/A") +#define RSL_DRIVE_STATE_NAME_STANDBY ("Standby") +#define RSL_DRIVE_STATE_NAME_WARMSTART ("WarmStart") // FSM state enumerators. enum class StateEnum @@ -85,10 +84,9 @@ uint16_t stateEnumToId(const StateEnum stateEnum); StateEnum stateIdToEnum(uint16_t stateId); std::string stateEnumToName(const StateEnum stateEnum); -StateEnum stateNameToEnum(const std::string & string); - -std::ostream & operator<<(std::ostream & out, const StateEnum stateEnum); +StateEnum stateNameToEnum(const std::string& string); +std::ostream& operator<<(std::ostream& out, const StateEnum stateEnum); -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateError.hpp b/include/rsl_drive_sdk/fsm/StateError.hpp index 4242db2..208aa5f 100644 --- a/include/rsl_drive_sdk/fsm/StateError.hpp +++ b/include/rsl_drive_sdk/fsm/StateError.hpp @@ -2,41 +2,40 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateError : public StateBase { public: - StateError(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateError(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateError(); protected: @@ -44,6 +43,5 @@ class StateError : public StateBase void leaveDerived(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateFatal.hpp b/include/rsl_drive_sdk/fsm/StateFatal.hpp index 27e3d27..a263ee0 100644 --- a/include/rsl_drive_sdk/fsm/StateFatal.hpp +++ b/include/rsl_drive_sdk/fsm/StateFatal.hpp @@ -2,41 +2,40 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateFatal : public StateBase { public: - StateFatal(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateFatal(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateFatal(); protected: @@ -44,6 +43,5 @@ class StateFatal : public StateBase void leaveDerived(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateMachine.hpp b/include/rsl_drive_sdk/fsm/StateMachine.hpp index 54fdd36..653c6d0 100644 --- a/include/rsl_drive_sdk/fsm/StateMachine.hpp +++ b/include/rsl_drive_sdk/fsm/StateMachine.hpp @@ -2,45 +2,45 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include +#include // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateMachine { protected: - DriveEthercatDevice & rsl_drive_sdk_; + DriveEthercatDevice& rsl_drive_sdk_; mutable std::recursive_mutex mutex_; std::atomic activeStateEnum_; @@ -48,7 +48,7 @@ class StateMachine std::map states_; public: - StateMachine(DriveEthercatDevice & rsl_drive_sdk); + explicit StateMachine(DriveEthercatDevice& rsl_drive_sdk); virtual ~StateMachine(); void updateActiveState(const StateEnum newActiveStateEnum); @@ -62,9 +62,8 @@ class StateMachine protected: std::string getName() const; - void addState(const StateBasePtr & state); + void addState(const StateBasePtr& state); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateMotorOp.hpp b/include/rsl_drive_sdk/fsm/StateMotorOp.hpp index b3a6422..836d824 100644 --- a/include/rsl_drive_sdk/fsm/StateMotorOp.hpp +++ b/include/rsl_drive_sdk/fsm/StateMotorOp.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateMotorOp : public StateBase { public: - StateMotorOp(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateMotorOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateMotorOp(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateMotorPreOp.hpp b/include/rsl_drive_sdk/fsm/StateMotorPreOp.hpp index d6eaf6c..cf77b0c 100644 --- a/include/rsl_drive_sdk/fsm/StateMotorPreOp.hpp +++ b/include/rsl_drive_sdk/fsm/StateMotorPreOp.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateMotorPreOp : public StateBase { public: - StateMotorPreOp(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateMotorPreOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateMotorPreOp(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateStandby.hpp b/include/rsl_drive_sdk/fsm/StateStandby.hpp index 38f9f6c..04245ff 100644 --- a/include/rsl_drive_sdk/fsm/StateStandby.hpp +++ b/include/rsl_drive_sdk/fsm/StateStandby.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateStandby : public StateBase { public: - StateStandby(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateStandby(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateStandby(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/fsm/StateWarmStart.hpp b/include/rsl_drive_sdk/fsm/StateWarmStart.hpp index 251cacd..f821720 100644 --- a/include/rsl_drive_sdk/fsm/StateWarmStart.hpp +++ b/include/rsl_drive_sdk/fsm/StateWarmStart.hpp @@ -2,44 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" - namespace rsl_drive_sdk { namespace fsm { - class StateWarmStart : public StateBase { public: - StateWarmStart(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum); + StateWarmStart(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum); virtual ~StateWarmStart(); }; - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeBase.hpp b/include/rsl_drive_sdk/mode/ModeBase.hpp index 95b6f55..b6e2b05 100644 --- a/include/rsl_drive_sdk/mode/ModeBase.hpp +++ b/include/rsl_drive_sdk/mode/ModeBase.hpp @@ -2,43 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include +#include // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeEnum.hpp" #include "rsl_drive_sdk/mode/PidGains.hpp" - namespace rsl_drive_sdk { namespace mode { - class ModeBase { protected: @@ -67,12 +67,11 @@ class ModeBase ModeEnum getModeEnum() const; std::string getName() const; - void setPidGains(const PidGainsF & pidGains); + void setPidGains(const PidGainsF& pidGains); std::optional getPidGains() const; }; using ModeBasePtr = std::shared_ptr; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeCurrent.hpp b/include/rsl_drive_sdk/mode/ModeCurrent.hpp index 27c29f8..eadc825 100644 --- a/include/rsl_drive_sdk/mode/ModeCurrent.hpp +++ b/include/rsl_drive_sdk/mode/ModeCurrent.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeCurrent : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeCurrent : public ModeBase virtual ~ModeCurrent(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeDisable.hpp b/include/rsl_drive_sdk/mode/ModeDisable.hpp index 27db6ba..cb00bf0 100644 --- a/include/rsl_drive_sdk/mode/ModeDisable.hpp +++ b/include/rsl_drive_sdk/mode/ModeDisable.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeDisable : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeDisable : public ModeBase virtual ~ModeDisable(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeEnum.hpp b/include/rsl_drive_sdk/mode/ModeEnum.hpp index 5c68699..3896740 100644 --- a/include/rsl_drive_sdk/mode/ModeEnum.hpp +++ b/include/rsl_drive_sdk/mode/ModeEnum.hpp @@ -2,91 +2,90 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include #include - namespace rsl_drive_sdk { namespace mode { - // Mode IDs. // This mapping must be equal to the firmware mapping. -#define RSL_DRIVE_MODE_ID_CURRENT (3) // Track current -#define RSL_DRIVE_MODE_ID_JOINT_POS_TOR (16) // Track position + feedforward torque -#define RSL_DRIVE_MODE_ID_DISABLE (2) // Disable motor -#define RSL_DRIVE_MODE_ID_FREEZE (1) // Freeze motor -#define RSL_DRIVE_MODE_ID_GEAR_POS (6) // Track gear position -#define RSL_DRIVE_MODE_ID_GEAR_VEL (7) // Track gear velocity -#define RSL_DRIVE_MODE_ID_JOINT_POS (8) // Track joint position -#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL (11) // Track joint position with feedforward velocity -#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL_TOR (12) // Track joint position with feedforward velocity and torque -#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL_TOR_PID (13) // Track joint position with feedforward velocity and torque using custom joint position gains -#define RSL_DRIVE_MODE_ID_JOINT_TOR (10) // Track joint torque -#define RSL_DRIVE_MODE_ID_JOINT_VEL (9) // Track joint velocity -#define RSL_DRIVE_MODE_ID_MOTOR_POS (4) // Track motor position -#define RSL_DRIVE_MODE_ID_MOTOR_VEL (5) // Track motor velocity -#define RSL_DRIVE_MODE_ID_NA (0) // Not available +#define RSL_DRIVE_MODE_ID_CURRENT (3) // Track current +#define RSL_DRIVE_MODE_ID_JOINT_POS_TOR (16) // Track position + feedforward torque +#define RSL_DRIVE_MODE_ID_DISABLE (2) // Disable motor +#define RSL_DRIVE_MODE_ID_FREEZE (1) // Freeze motor +#define RSL_DRIVE_MODE_ID_GEAR_POS (6) // Track gear position +#define RSL_DRIVE_MODE_ID_GEAR_VEL (7) // Track gear velocity +#define RSL_DRIVE_MODE_ID_JOINT_POS (8) // Track joint position +#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL (11) // Track joint position with feedforward velocity +#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL_TOR (12) // Track joint position with feedforward velocity and torque +#define RSL_DRIVE_MODE_ID_JOINT_POS_VEL_TOR_PID \ + (13) // Track joint position with feedforward velocity and torque using custom joint position gains +#define RSL_DRIVE_MODE_ID_JOINT_TOR (10) // Track joint torque +#define RSL_DRIVE_MODE_ID_JOINT_VEL (9) // Track joint velocity +#define RSL_DRIVE_MODE_ID_MOTOR_POS (4) // Track motor position +#define RSL_DRIVE_MODE_ID_MOTOR_VEL (5) // Track motor velocity +#define RSL_DRIVE_MODE_ID_NA (0) // Not available // Mode names. -#define RSL_DRIVE_MODE_NAME_CURRENT ("Current") -#define RSL_DRIVE_MODE_NAME_JOINT_POS_TOR ("JointPositionTorque") -#define RSL_DRIVE_MODE_NAME_DISABLE ("Disable") -#define RSL_DRIVE_MODE_NAME_FREEZE ("Freeze") -#define RSL_DRIVE_MODE_NAME_GEAR_POS ("GearPosition") -#define RSL_DRIVE_MODE_NAME_GEAR_VEL ("GearVelocity") -#define RSL_DRIVE_MODE_NAME_JOINT_POS ("JointPosition") -#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL ("JointPositionVelocity") -#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL_TOR ("JointPositionVelocityTorque") -#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL_TOR_PID ( \ - "JointPositionVelocityTorquePidGains") -#define RSL_DRIVE_MODE_NAME_JOINT_TOR ("JointTorque") -#define RSL_DRIVE_MODE_NAME_JOINT_VEL ("JointVelocity") -#define RSL_DRIVE_MODE_NAME_MOTOR_POS ("MotorPosition") -#define RSL_DRIVE_MODE_NAME_MOTOR_VEL ("MotorVelocity") -#define RSL_DRIVE_MODE_NAME_NA ("N/A") +#define RSL_DRIVE_MODE_NAME_CURRENT ("Current") +#define RSL_DRIVE_MODE_NAME_JOINT_POS_TOR ("JointPositionTorque") +#define RSL_DRIVE_MODE_NAME_DISABLE ("Disable") +#define RSL_DRIVE_MODE_NAME_FREEZE ("Freeze") +#define RSL_DRIVE_MODE_NAME_GEAR_POS ("GearPosition") +#define RSL_DRIVE_MODE_NAME_GEAR_VEL ("GearVelocity") +#define RSL_DRIVE_MODE_NAME_JOINT_POS ("JointPosition") +#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL ("JointPositionVelocity") +#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL_TOR ("JointPositionVelocityTorque") +#define RSL_DRIVE_MODE_NAME_JOINT_POS_VEL_TOR_PID ("JointPositionVelocityTorquePidGains") +#define RSL_DRIVE_MODE_NAME_JOINT_TOR ("JointTorque") +#define RSL_DRIVE_MODE_NAME_JOINT_VEL ("JointVelocity") +#define RSL_DRIVE_MODE_NAME_MOTOR_POS ("MotorPosition") +#define RSL_DRIVE_MODE_NAME_MOTOR_VEL ("MotorVelocity") +#define RSL_DRIVE_MODE_NAME_NA ("N/A") // Mode short names. -#define RSL_DRIVE_MODE_NAME_SHORT_CURRENT ("C") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_TOR ("JPT") -#define RSL_DRIVE_MODE_NAME_SHORT_DISABLE ("D") -#define RSL_DRIVE_MODE_NAME_SHORT_FREEZE ("F") -#define RSL_DRIVE_MODE_NAME_SHORT_GEAR_POS ("GP") -#define RSL_DRIVE_MODE_NAME_SHORT_GEAR_VEL ("GV") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS ("JP") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL ("JPV") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL_TOR ("JPVT") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL_TOR_PID ("JPVTPID") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_TOR ("JT") -#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_VEL ("JV") -#define RSL_DRIVE_MODE_NAME_SHORT_MOTOR_POS ("MP") -#define RSL_DRIVE_MODE_NAME_SHORT_MOTOR_VEL ("MV") -#define RSL_DRIVE_MODE_NAME_SHORT_NA ("N/A") +#define RSL_DRIVE_MODE_NAME_SHORT_CURRENT ("C") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_TOR ("JPT") +#define RSL_DRIVE_MODE_NAME_SHORT_DISABLE ("D") +#define RSL_DRIVE_MODE_NAME_SHORT_FREEZE ("F") +#define RSL_DRIVE_MODE_NAME_SHORT_GEAR_POS ("GP") +#define RSL_DRIVE_MODE_NAME_SHORT_GEAR_VEL ("GV") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS ("JP") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL ("JPV") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL_TOR ("JPVT") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_POS_VEL_TOR_PID ("JPVTPID") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_TOR ("JT") +#define RSL_DRIVE_MODE_NAME_SHORT_JOINT_VEL ("JV") +#define RSL_DRIVE_MODE_NAME_SHORT_MOTOR_POS ("MP") +#define RSL_DRIVE_MODE_NAME_SHORT_MOTOR_VEL ("MV") +#define RSL_DRIVE_MODE_NAME_SHORT_NA ("N/A") // Mode enumerators. enum class ModeEnum @@ -108,16 +107,14 @@ enum class ModeEnum NA }; - uint16_t modeEnumToId(const ModeEnum modeEnum); ModeEnum modeIdToEnum(const uint16_t modeId); std::string modeEnumToName(const ModeEnum modeEnum); std::string modeEnumToShortName(const ModeEnum modeEnum); -ModeEnum modeNameToEnum(const std::string & string); - -std::ostream & operator<<(std::ostream & out, const ModeEnum modeEnum); +ModeEnum modeNameToEnum(const std::string& string); +std::ostream& operator<<(std::ostream& out, const ModeEnum modeEnum); -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeFreeze.hpp b/include/rsl_drive_sdk/mode/ModeFreeze.hpp index 0eb135d..31a9bb5 100644 --- a/include/rsl_drive_sdk/mode/ModeFreeze.hpp +++ b/include/rsl_drive_sdk/mode/ModeFreeze.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeFreeze : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeFreeze : public ModeBase virtual ~ModeFreeze(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeGearPosition.hpp b/include/rsl_drive_sdk/mode/ModeGearPosition.hpp index e7ed750..20dd9c5 100644 --- a/include/rsl_drive_sdk/mode/ModeGearPosition.hpp +++ b/include/rsl_drive_sdk/mode/ModeGearPosition.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeGearPosition : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeGearPosition : public ModeBase virtual ~ModeGearPosition(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeGearVelocity.hpp b/include/rsl_drive_sdk/mode/ModeGearVelocity.hpp index 0536c1c..55645e2 100644 --- a/include/rsl_drive_sdk/mode/ModeGearVelocity.hpp +++ b/include/rsl_drive_sdk/mode/ModeGearVelocity.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeGearVelocity : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeGearVelocity : public ModeBase virtual ~ModeGearVelocity(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointPosition.hpp b/include/rsl_drive_sdk/mode/ModeJointPosition.hpp index 8794bca..00b1201 100644 --- a/include/rsl_drive_sdk/mode/ModeJointPosition.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointPosition.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointPosition : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointPosition : public ModeBase virtual ~ModeJointPosition(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointPositionTorque.hpp b/include/rsl_drive_sdk/mode/ModeJointPositionTorque.hpp index 29ab81b..f8f2149 100644 --- a/include/rsl_drive_sdk/mode/ModeJointPositionTorque.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointPositionTorque.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointPositionTorque : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointPositionTorque : public ModeBase virtual ~ModeJointPositionTorque(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointPositionVelocity.hpp b/include/rsl_drive_sdk/mode/ModeJointPositionVelocity.hpp index a0d193f..d1e69f4 100644 --- a/include/rsl_drive_sdk/mode/ModeJointPositionVelocity.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointPositionVelocity.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointPositionVelocity : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointPositionVelocity : public ModeBase virtual ~ModeJointPositionVelocity(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorque.hpp b/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorque.hpp index 837d09b..b39cbbd 100644 --- a/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorque.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorque.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointPositionVelocityTorque : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointPositionVelocityTorque : public ModeBase virtual ~ModeJointPositionVelocityTorque(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorquePidGains.hpp b/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorquePidGains.hpp index e875e80..b93782d 100644 --- a/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorquePidGains.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointPositionVelocityTorquePidGains.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointPositionVelocityTorquePidGains : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointPositionVelocityTorquePidGains : public ModeBase virtual ~ModeJointPositionVelocityTorquePidGains(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointTorque.hpp b/include/rsl_drive_sdk/mode/ModeJointTorque.hpp index d5f0cc2..313e719 100644 --- a/include/rsl_drive_sdk/mode/ModeJointTorque.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointTorque.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointTorque : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointTorque : public ModeBase virtual ~ModeJointTorque(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeJointVelocity.hpp b/include/rsl_drive_sdk/mode/ModeJointVelocity.hpp index 9d53a6f..c0f249b 100644 --- a/include/rsl_drive_sdk/mode/ModeJointVelocity.hpp +++ b/include/rsl_drive_sdk/mode/ModeJointVelocity.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeJointVelocity : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeJointVelocity : public ModeBase virtual ~ModeJointVelocity(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeMotorPosition.hpp b/include/rsl_drive_sdk/mode/ModeMotorPosition.hpp index 371d154..ddec2d1 100644 --- a/include/rsl_drive_sdk/mode/ModeMotorPosition.hpp +++ b/include/rsl_drive_sdk/mode/ModeMotorPosition.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeMotorPosition : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeMotorPosition : public ModeBase virtual ~ModeMotorPosition(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/ModeMotorVelocity.hpp b/include/rsl_drive_sdk/mode/ModeMotorVelocity.hpp index ccf0235..0d24dd2 100644 --- a/include/rsl_drive_sdk/mode/ModeMotorVelocity.hpp +++ b/include/rsl_drive_sdk/mode/ModeMotorVelocity.hpp @@ -2,37 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // rsl_drive_sdk #include - namespace rsl_drive_sdk { namespace mode { - class ModeMotorVelocity : public ModeBase { public: @@ -40,6 +39,5 @@ class ModeMotorVelocity : public ModeBase virtual ~ModeMotorVelocity(); }; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/mode/PidGains.hpp b/include/rsl_drive_sdk/mode/PidGains.hpp index b15d019..3b8ac74 100644 --- a/include/rsl_drive_sdk/mode/PidGains.hpp +++ b/include/rsl_drive_sdk/mode/PidGains.hpp @@ -2,27 +2,28 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include #include @@ -32,8 +33,7 @@ namespace rsl_drive_sdk namespace mode { - -template +template class PidGains { protected: @@ -42,21 +42,24 @@ class PidGains T d_ = 0.0; public: - PidGains() {} + PidGains() + { + } - PidGains(T p, T i, T d) - : p_(p), - i_(i), - d_(d) {} + PidGains(T p, T i, T d) : p_(p), i_(i), d_(d) + { + } - virtual ~PidGains() {} + virtual ~PidGains() + { + } T getP() const { return p_; } - T & getP() + T& getP() { return p_; } @@ -71,7 +74,7 @@ class PidGains return i_; } - T & getI() + T& getI() { return i_; } @@ -86,7 +89,7 @@ class PidGains return d_; } - T & getD() + T& getD() { return d_; } @@ -101,7 +104,7 @@ class PidGains return PidGains(p_ * scaling, i_ * scaling, d_ * scaling); } - PidGains & operator*=(const double scaling) + PidGains& operator*=(const double scaling) { *this = *this * scaling; return *this; @@ -109,15 +112,12 @@ class PidGains bool isValid() const { - return - std::isfinite(p_) && - std::isfinite(i_) && - std::isfinite(d_); + return std::isfinite(p_) && std::isfinite(i_) && std::isfinite(d_); } }; -template -std::ostream & operator<<(std::ostream & out, const PidGains & command) +template +std::ostream& operator<<(std::ostream& out, const PidGains& command) { return out << command.getP() << ", " << command.getI() << ", " << command.getD(); } @@ -125,6 +125,5 @@ std::ostream & operator<<(std::ostream & out, const PidGains & command) using PidGainsF = PidGains; using PidGainsD = PidGains; - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/thread_sleep.hpp b/include/rsl_drive_sdk/thread_sleep.hpp index 057d4ac..44f3b0b 100644 --- a/include/rsl_drive_sdk/thread_sleep.hpp +++ b/include/rsl_drive_sdk/thread_sleep.hpp @@ -2,32 +2,31 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - namespace rsl_drive_sdk { - void thread_sleep(const double duration); - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/include/rsl_drive_sdk/usings.hpp b/include/rsl_drive_sdk/usings.hpp index 72b6945..05711d0 100644 --- a/include/rsl_drive_sdk/usings.hpp +++ b/include/rsl_drive_sdk/usings.hpp @@ -2,50 +2,49 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #pragma once - // std #include +#include // rsl_drive_sdk #include "rsl_drive_sdk/ReadingExtended.hpp" - namespace rsl_drive_sdk { - // The first parameter of these callbacks is the name of the device. -using ReadingCb = std::function; -using ErrorCb = std::function; -using ErrorRecoveredCb = std::function; -using FatalCb = std::function; -using FatalRecoveredCb = std::function; -using DeviceDisconnectedCb = std::function; -using DeviceReconnectedCb = std::function; +using ReadingCb = std::function; +using ErrorCb = std::function; +using ErrorRecoveredCb = std::function; +using FatalCb = std::function; +using FatalRecoveredCb = std::function; +using DeviceDisconnectedCb = std::function; +using DeviceReconnectedCb = std::function; // Anonymous callbacks do not include the name of the device. -using AnonymousReadingCb = std::function; +using AnonymousReadingCb = std::function; using AnonymousErrorCb = std::function; - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/Command.cpp b/src/Command.cpp index 383eaca..d16548f 100644 --- a/src/Command.cpp +++ b/src/Command.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std #include @@ -28,21 +30,23 @@ #include "rsl_drive_sdk/Command.hpp" #include - namespace rsl_drive_sdk { +Command::Command() +{ +} -Command::Command() {} - -Command::~Command() {} +Command::~Command() +{ +} -const std::chrono::high_resolution_clock::time_point & Command::getStamp() const +const std::chrono::high_resolution_clock::time_point& Command::getStamp() const { return stamp_; } -void Command::setStamp(const std::chrono::high_resolution_clock::time_point & stamp) +void Command::setStamp(const std::chrono::high_resolution_clock::time_point& stamp) { stamp_ = stamp; } @@ -137,17 +141,17 @@ void Command::setJointTorque(const double jointTorque) jointTorque_ = jointTorque; } -mode::PidGainsF & Command::getPidGains() +mode::PidGainsF& Command::getPidGains() { return pidGains_; } -const mode::PidGainsF & Command::getPidGains() const +const mode::PidGainsF& Command::getPidGains() const { return pidGains_; } -void Command::setPidGains(const mode::PidGainsF & pidGains) +void Command::setPidGains(const mode::PidGainsF& pidGains) { pidGains_ = pidGains; } @@ -156,22 +160,16 @@ bool Command::isValid() const { return - modeEnum_ != mode::ModeEnum::NA && - std::isfinite(current_) && - std::isfinite(motorPosition_) && - std::isfinite(motorVelocity_) && - std::isfinite(gearPosition_) && - std::isfinite(gearVelocity_) && - std::isfinite(jointPosition_) && - std::isfinite(jointVelocity_) && - std::isfinite(jointTorque_) && - pidGains_.isValid(); + modeEnum_ != mode::ModeEnum::NA && std::isfinite(current_) && std::isfinite(motorPosition_) && + std::isfinite(motorVelocity_) && std::isfinite(gearPosition_) && std::isfinite(gearVelocity_) && + std::isfinite(jointPosition_) && std::isfinite(jointVelocity_) && std::isfinite(jointTorque_) && + pidGains_.isValid(); } -std::string Command::asString(const std::string & prefix) const +std::string Command::asString(const std::string& prefix) const { std::stringstream ss; - //ss << prefix << "Stamp: " << stamp_ << std::endl; + // ss << prefix << "Stamp: " << stamp_ << std::endl; ss << prefix << "Mode: " << modeEnum_ << std::endl; ss << prefix << "Current: " << current_ << std::endl; ss << prefix << "Motor position: " << motorPosition_ << std::endl; @@ -185,12 +183,11 @@ std::string Command::asString(const std::string & prefix) const return ss.str(); } -std::ostream & operator<<(std::ostream & out, const Command & command) +std::ostream& operator<<(std::ostream& out, const Command& command) { out << "Command:" << std::endl; out << command.asString(" ") << std::endl; return out; } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/Drive.cpp b/src/Drive.cpp index b25c936..029f02a 100644 --- a/src/Drive.cpp +++ b/src/Drive.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/thread_sleep.hpp" @@ -37,27 +39,25 @@ namespace rsl_drive_sdk { - std::map DriveEthercatDevice::modeEnumToOdIndex_ = { - {rsl_drive_sdk::mode::ModeEnum::Current, OD_GAINS_CURRENT_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::MotorPosition, OD_GAINS_MOTOR_POSITION_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::MotorVelocity, OD_GAINS_MOTOR_VELOCITY_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::GearPosition, OD_GAINS_GEAR_POSITION_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::GearVelocity, OD_GAINS_GEAR_VELOCITY_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointPosition, OD_GAINS_JOINT_POSITION_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointPositionTorque, OD_GAINS_JOINT_POSITION_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointVelocity, OD_GAINS_JOINT_VELOCITY_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointTorque, OD_GAINS_JOINT_TORQUE_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointPositionVelocity, OD_GAINS_JOINT_POSITION_VELOCITY_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointPositionVelocityTorque, - OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_CTRL_ID}, - {rsl_drive_sdk::mode::ModeEnum::JointPositionVelocityTorquePidGains, - OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_PID_GAINS_CTRL_ID} + { rsl_drive_sdk::mode::ModeEnum::Current, OD_GAINS_CURRENT_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::MotorPosition, OD_GAINS_MOTOR_POSITION_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::MotorVelocity, OD_GAINS_MOTOR_VELOCITY_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::GearPosition, OD_GAINS_GEAR_POSITION_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::GearVelocity, OD_GAINS_GEAR_VELOCITY_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointPosition, OD_GAINS_JOINT_POSITION_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointPositionTorque, OD_GAINS_JOINT_POSITION_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointVelocity, OD_GAINS_JOINT_VELOCITY_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointTorque, OD_GAINS_JOINT_TORQUE_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointPositionVelocity, OD_GAINS_JOINT_POSITION_VELOCITY_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointPositionVelocityTorque, OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_CTRL_ID }, + { rsl_drive_sdk::mode::ModeEnum::JointPositionVelocityTorquePidGains, + OD_GAINS_JOINT_POSITION_VELOCITY_TORQUE_PID_GAINS_CTRL_ID } }; -DriveEthercatDevice::SharedPtr DriveEthercatDevice::deviceFromFile( - const std::string & fileName, - const std::string & name, const uint32_t address, const PdoTypeEnum pdoTypeEnum) +DriveEthercatDevice::SharedPtr DriveEthercatDevice::deviceFromFile(const std::string& fileName, const std::string& name, + const uint32_t address, + const PdoTypeEnum pdoTypeEnum) { auto rsl_drive_sdk = std::make_shared(address, name, pdoTypeEnum); const auto configuration = DriveConfigurationParser::fromFile(fileName); @@ -65,12 +65,8 @@ DriveEthercatDevice::SharedPtr DriveEthercatDevice::deviceFromFile( return rsl_drive_sdk; } -DriveEthercatDevice::DriveEthercatDevice( - const uint32_t address, const std::string & name, - const PdoTypeEnum pdoTypeEnum) -:pdoTypeEnum_(pdoTypeEnum), - currentPdoTypeEnum_(PdoTypeEnum::NA), - stateMachine_(*this) +DriveEthercatDevice::DriveEthercatDevice(const uint32_t address, const std::string& name, const PdoTypeEnum pdoTypeEnum) + : pdoTypeEnum_(pdoTypeEnum), currentPdoTypeEnum_(PdoTypeEnum::NA), stateMachine_(*this) { address_ = address; name_ = name; @@ -83,36 +79,36 @@ DriveEthercatDevice::DriveEthercatDevice( pdoInfo.rxPdoSize_ = sizeof(RxPdo); pdoInfo.txPdoSize_ = sizeof(TxPdoA); pdoInfo.moduleId_ = 0x00119800; - pdoInfos_.insert({PdoTypeEnum::A, pdoInfo}); + pdoInfos_.insert({ PdoTypeEnum::A, pdoInfo }); pdoInfo.rxPdoId_ = OD_DSP402_RX_PDO_SID_VAL_B; pdoInfo.txPdoId_ = OD_DSP402_TX_PDO_SID_VAL_B; pdoInfo.rxPdoSize_ = sizeof(RxPdo); pdoInfo.txPdoSize_ = sizeof(TxPdoB); pdoInfo.moduleId_ = 0x00219800; - pdoInfos_.insert({PdoTypeEnum::B, pdoInfo}); + pdoInfos_.insert({ PdoTypeEnum::B, pdoInfo }); pdoInfo.rxPdoId_ = OD_DSP402_RX_PDO_SID_VAL_C; pdoInfo.txPdoId_ = OD_DSP402_TX_PDO_SID_VAL_C; pdoInfo.rxPdoSize_ = sizeof(RxPdo); pdoInfo.txPdoSize_ = sizeof(TxPdoC); pdoInfo.moduleId_ = 0x00319800; - pdoInfos_.insert({PdoTypeEnum::C, pdoInfo}); + pdoInfos_.insert({ PdoTypeEnum::C, pdoInfo }); pdoInfo.rxPdoId_ = OD_DSP402_RX_PDO_SID_VAL_D; pdoInfo.txPdoId_ = OD_DSP402_TX_PDO_SID_VAL_D; pdoInfo.rxPdoSize_ = sizeof(RxPdo); pdoInfo.txPdoSize_ = sizeof(TxPdoD); pdoInfo.moduleId_ = 0x00419800; - pdoInfos_.insert({PdoTypeEnum::D, pdoInfo}); + pdoInfos_.insert({ PdoTypeEnum::D, pdoInfo }); - //Note: E Type is special as it uses the D type rx pdo + // Note: E Type is special as it uses the D type rx pdo pdoInfo.rxPdoId_ = OD_DSP402_RX_PDO_SID_VAL_D; pdoInfo.txPdoId_ = OD_DSP402_TX_PDO_SID_VAL_E; pdoInfo.rxPdoSize_ = sizeof(RxPdo); pdoInfo.txPdoSize_ = sizeof(TxPdoE); pdoInfo.moduleId_ = 0x00519800; - pdoInfos_.insert({PdoTypeEnum::E, pdoInfo}); + pdoInfos_.insert({ PdoTypeEnum::E, pdoInfo }); } uint32_t DriveEthercatDevice::getAddress() const @@ -120,20 +116,17 @@ uint32_t DriveEthercatDevice::getAddress() const return address_; } - -void DriveEthercatDevice::applyConfiguration(const configuration::Configuration & config) +void DriveEthercatDevice::applyConfiguration(const configuration::Configuration& config) { MELO_DEBUG("Applying configuration"); config_ = config; } - std::string DriveEthercatDevice::getName() const { return name_; } - fsm::StateEnum DriveEthercatDevice::getActiveStateEnum() const { return stateMachine_.getActiveStateEnum(); @@ -148,11 +141,8 @@ bool DriveEthercatDevice::goalStateHasBeenReached() const { return stateMachine_.goalStateHasBeenReached(); } -bool DriveEthercatDevice::setFSMGoalState( - const fsm::StateEnum goalStateEnum, - const bool reachState, - const double timeout, - const double checkingFrequency) +bool DriveEthercatDevice::setFSMGoalState(const fsm::StateEnum goalStateEnum, const bool reachState, + const double timeout, const double checkingFrequency) { stateMachine_.setGoalStateEnum(goalStateEnum); @@ -181,17 +171,16 @@ void DriveEthercatDevice::clearGoalStateEnum() bool DriveEthercatDevice::startup() { - if(!bus_->waitForState(EC_STATE_PRE_OP, address_, 50)) { - MELO_ERROR_STREAM("[rsl_drive_sdk:Drive::startup] '" << name_ << - "' did not reach PRE_OP state."); + if (!bus_->waitForState(EC_STATE_PRE_OP, address_, 50)) { + MELO_ERROR_STREAM("[rsl_drive_sdk:Drive::startup] '" << name_ << "' did not reach PRE_OP state."); } RecLock lock(mutex_); - MELO_DEBUG_STREAM("Startup: " << getName() ); + MELO_DEBUG_STREAM("Startup: " << getName()); std::string model; getDriveModel(model); MELO_INFO("[" + getName() + "] model: " + model) - // Configure PDO setup + // Configure PDO setup if (!configurePdo(pdoTypeEnum_)) { MELO_ERROR("Could not configure pdo mapping"); return false; @@ -199,47 +188,49 @@ bool DriveEthercatDevice::startup() setFSMGoalState(config_.getGoalStateEnumStartup(), false, 0, 0); - // This sets the mode::ModeEnum of command_ to mode::ModeEnum::Freeze - // This means that the drives freeze after startup if and only if: - // - ControlOp state is reached. - // - No other command with a ModeEnum that requests another mode is staged - // after startup. - // - no other state than ControlOp is requested directly after or before startup. + // This sets the mode::ModeEnum of command_ to mode::ModeEnum::Freeze + // This means that the drives freeze after startup if and only if: + // - ControlOp state is reached. + // - No other command with a ModeEnum that requests another mode is staged + // after startup. + // - no other state than ControlOp is requested directly after or before startup. stageFreeze(); return true; } void DriveEthercatDevice::updateProcessReading() { - - // Get reading. + // Get reading. std::lock_guard lock(readingMutex_); - // Update statusword. + // Update statusword. Statusword statusword(reading_.getState().getStatusword()); setStatusword(statusword); statuswordRequested_ = false; - // execute reading callbacks - for (const auto & readingCb : readingCbs_) { + // execute reading callbacks + for (const auto& readingCb : readingCbs_) { readingCb.second(getName(), reading_); } - if (deviceIsMissing()) { Statusword statusword; statusword.setStateEnum(fsm::StateEnum::DeviceMissing); setStatusword(statusword); } else { - if (statusword_.isEmpty() /* || statusword_.getAge() > getConfiguration().getMaxStatuswordAge()*/) - { // TODO Delete statusword request over SDO? - // Device is not missing but statusword is empty or outdated. + if (statusword_ + .isEmpty() /* || statusword_.getAge() > getConfiguration().getMaxStatuswordAge()*/) { // TODO Delete + // statusword request + // over SDO? Device + // is not missing but + // statusword is + // empty or outdated. requestAndSetStatusword(); } if (statusword_.isEmpty()) { - // Device is not missing but statusword is still empty. + // Device is not missing but statusword is still empty. return; } } @@ -256,7 +247,7 @@ void DriveEthercatDevice::requestAndSetStatusword() { if (!statuswordRequested_) { MELO_DEBUG_STREAM("Requesting statusword over SDO ..."); - //requestStatusword(); + // requestStatusword(); statuswordRequested_ = true; } else { Statusword statusword; @@ -268,319 +259,265 @@ void DriveEthercatDevice::requestAndSetStatusword() } } -void DriveEthercatDevice::setStatusword(Statusword & statusword) +void DriveEthercatDevice::setStatusword(Statusword& statusword) { - // If the stamp has not changed, we assume it is again the same statusword. + // If the stamp has not changed, we assume it is again the same statusword. if (statusword.getStamp() == statusword_.getStamp()) { return; } - // Check if statusword contains new data. - if (statusword_.isEmpty() || - statusword.getData() != statusword_.getData()) - { + // Check if statusword contains new data. + if (statusword_.isEmpty() || statusword.getData() != statusword_.getData()) { MELO_DEBUG_STREAM("Received new statusword (" << statusword << ")."); std::vector infos; std::vector warnings; std::vector errors; std::vector fatals; statusword.getMessagesDiff(statusword_, infos, warnings, errors, fatals); - for (const std::string & info : infos) { + for (const std::string& info : infos) { MELO_INFO_STREAM("[" << getName() << "]: " << info); } - for (const std::string & warning : warnings) { + for (const std::string& warning : warnings) { MELO_WARN_STREAM("[" << getName() << "]: " << warning); } - for (const std::string & error : errors) { + for (const std::string& error : errors) { MELO_ERROR_STREAM("[" << getName() << "]: " << error); } - for (const std::string & fatal : fatals) { + for (const std::string& fatal : fatals) { MELO_ERROR_STREAM("[" << getName() << "]: " << fatal); } if (statusword.getModeEnum() != statusword_.getModeEnum()) { - MELO_DEBUG_STREAM("[" << getName() << "]Changed mode to '" << - mode::modeEnumToName(statusword.getModeEnum()) << "'."); + MELO_DEBUG_STREAM("[" << getName() << "]Changed mode to '" << mode::modeEnumToName(statusword.getModeEnum()) + << "'."); } } - // Always update statusword to set new time stamp. + // Always update statusword to set new time stamp. statusword_ = statusword; } void DriveEthercatDevice::updateRead() { - - const std::chrono::high_resolution_clock::time_point updateStamp = bus_->getUpdateReadStamp(); switch (getCurrentPdoTypeEnum()) { case PdoTypeEnum::A: - { - TxPdoA txPdoA; - bus_->readTxPdo(address_, txPdoA); - - RecLock lockReading(readingMutex_); - reading_.getState().setStamp(updateStamp); - reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoA.statusword_)); - reading_.getState().setTemperature(temperatureScalingInv_ * txPdoA.measuredTemperature_ + - temperatureOffset_); - reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoA.measuredMotorVoltage_); - reading_.getState().setMotorPosition(motorPositionScalingInv_ * - txPdoA.measuredMotorPosition_); - reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoA.measuredGearPosition_); - reading_.getState().setJointPosition(jointPositionScalingInv_ * - txPdoA.measuredJointPosition_); - reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoA.measuredMotorCurrent_); - reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * - txPdoA.measuredMotorVelocity_); - reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoA.measuredGearVelocity_); - reading_.getState().setJointVelocity(jointVelocityScalingInv_ * - txPdoA.measuredJointVelocity_); - reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * - txPdoA.measuredJointAcceleration_); - reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoA.measuredJointTorque_); - } - break; + { + TxPdoA txPdoA; + bus_->readTxPdo(address_, txPdoA); + + RecLock lockReading(readingMutex_); + reading_.getState().setStamp(updateStamp); + reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoA.statusword_)); + reading_.getState().setTemperature(temperatureScalingInv_ * txPdoA.measuredTemperature_ + temperatureOffset_); + reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoA.measuredMotorVoltage_); + reading_.getState().setMotorPosition(motorPositionScalingInv_ * txPdoA.measuredMotorPosition_); + reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoA.measuredGearPosition_); + reading_.getState().setJointPosition(jointPositionScalingInv_ * txPdoA.measuredJointPosition_); + reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoA.measuredMotorCurrent_); + reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * txPdoA.measuredMotorVelocity_); + reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoA.measuredGearVelocity_); + reading_.getState().setJointVelocity(jointVelocityScalingInv_ * txPdoA.measuredJointVelocity_); + reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * txPdoA.measuredJointAcceleration_); + reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoA.measuredJointTorque_); + } break; case PdoTypeEnum::B: - { - TxPdoB txPdoB; - bus_->readTxPdo(address_, txPdoB); - - RecLock lockReading(readingMutex_); - reading_.getState().setStamp(updateStamp); - reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoB.statusword_)); - reading_.getState().setTemperature(temperatureScalingInv_ * txPdoB.measuredTemperature_ + - temperatureOffset_); - reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoB.measuredMotorVoltage_); - reading_.getState().setMotorPosition(motorPositionScalingInv_ * - txPdoB.measuredMotorPosition_); - reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoB.measuredGearPosition_); - reading_.getState().setJointPosition(jointPositionScalingInv_ * - txPdoB.measuredJointPosition_); - reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoB.measuredMotorCurrent_); - reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * - txPdoB.measuredMotorVelocity_); - reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoB.measuredGearVelocity_); - reading_.getState().setJointVelocity(jointVelocityScalingInv_ * - txPdoB.measuredJointVelocity_); - reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * - txPdoB.measuredJointAcceleration_); - reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoB.measuredJointTorque_); - - Imu imu; - imu.acceleration_x = txPdoB.measuredImuLinearAccelerationX_; - imu.acceleration_y = txPdoB.measuredImuLinearAccelerationY_; - imu.acceleration_z = txPdoB.measuredImuLinearAccelerationZ_; - imu.angle_velocity_x = txPdoB.measuredImuAngularVelocityX_; - imu.angle_velocity_y = txPdoB.measuredImuAngularVelocityY_; - imu.angle_velocity_z = txPdoB.measuredImuAngularVelocityZ_; - // 7th IMU value is not read out at the moment. - reading_.getState().setImu(imu); - } - break; + { + TxPdoB txPdoB; + bus_->readTxPdo(address_, txPdoB); + + RecLock lockReading(readingMutex_); + reading_.getState().setStamp(updateStamp); + reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoB.statusword_)); + reading_.getState().setTemperature(temperatureScalingInv_ * txPdoB.measuredTemperature_ + temperatureOffset_); + reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoB.measuredMotorVoltage_); + reading_.getState().setMotorPosition(motorPositionScalingInv_ * txPdoB.measuredMotorPosition_); + reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoB.measuredGearPosition_); + reading_.getState().setJointPosition(jointPositionScalingInv_ * txPdoB.measuredJointPosition_); + reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoB.measuredMotorCurrent_); + reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * txPdoB.measuredMotorVelocity_); + reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoB.measuredGearVelocity_); + reading_.getState().setJointVelocity(jointVelocityScalingInv_ * txPdoB.measuredJointVelocity_); + reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * txPdoB.measuredJointAcceleration_); + reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoB.measuredJointTorque_); + + Imu imu; + imu.acceleration_x = txPdoB.measuredImuLinearAccelerationX_; + imu.acceleration_y = txPdoB.measuredImuLinearAccelerationY_; + imu.acceleration_z = txPdoB.measuredImuLinearAccelerationZ_; + imu.angle_velocity_x = txPdoB.measuredImuAngularVelocityX_; + imu.angle_velocity_y = txPdoB.measuredImuAngularVelocityY_; + imu.angle_velocity_z = txPdoB.measuredImuAngularVelocityZ_; + // 7th IMU value is not read out at the moment. + reading_.getState().setImu(imu); + } break; case PdoTypeEnum::C: - { - TxPdoC txPdoC; - bus_->readTxPdo(address_, txPdoC); - - RecLock lockReading(readingMutex_); - reading_.getState().setStamp(updateStamp); - reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoC.statusword_)); - reading_.getState().setTemperature(temperatureScalingInv_ * txPdoC.measuredTemperature_ + - temperatureOffset_); - reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoC.measuredMotorVoltage_); - reading_.getState().setMotorPosition(motorPositionScalingInv_ * - txPdoC.measuredMotorPosition_); - reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoC.measuredGearPosition_); - reading_.getState().setJointPosition(jointPositionScalingInv_ * - txPdoC.measuredJointPosition_); - reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoC.measuredMotorCurrent_); - reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * - txPdoC.measuredMotorVelocity_); - reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoC.measuredGearVelocity_); - reading_.getState().setJointVelocity(jointVelocityScalingInv_ * - txPdoC.measuredJointVelocity_); - reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * - txPdoC.measuredJointAcceleration_); - reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoC.measuredJointTorque_); - reading_.getState().setGearPositionTicks(txPdoC.measuredGearPositionTicks_); - reading_.getState().setJointPositionTicks(txPdoC.measuredJointPositionTicks_); - reading_.getState().setTimestamp(txPdoC.timestamp_); - reading_.getState().setDesiredCurrentD(txPdoC.desiredCurrentD_); - reading_.getState().setMeasuredCurrentD(txPdoC.measuredCurrentD_); - reading_.getState().setDesiredCurrentQ(txPdoC.desiredCurrentQ_); - reading_.getState().setMeasuredCurrentQ(txPdoC.measuredCurrentQ_); - reading_.getState().setAlpha(txPdoC.alpha_); - reading_.getState().setBeta(txPdoC.beta_); - reading_.getState().setDutyCycleU(txPdoC.dutyCycleU_); - reading_.getState().setDutyCycleV(txPdoC.dutyCycleV_); - reading_.getState().setDutyCycleW(txPdoC.dutyCycleW_); - reading_.getState().setMeasuredCurrentPhaseU(txPdoC.measuredCurrentPhaseU_); - reading_.getState().setMeasuredCurrentPhaseV(txPdoC.measuredCurrentPhaseV_); - reading_.getState().setMeasuredCurrentPhaseW(txPdoC.measuredCurrentPhaseW_); - reading_.getState().setMeasuredVoltagePhaseU(txPdoC.measuredVoltagePhaseU_); - reading_.getState().setMeasuredVoltagePhaseV(txPdoC.measuredVoltagePhaseV_); - reading_.getState().setMeasuredVoltagePhaseW(txPdoC.measuredVoltagePhaseW_); - reading_.getState().setMeasuredApparentPower(txPdoC.measuredCurrentPhaseU_ * - txPdoC.measuredVoltagePhaseU_ + txPdoC.measuredCurrentPhaseV_ * - txPdoC.measuredVoltagePhaseV_ + txPdoC.measuredCurrentPhaseW_ * - txPdoC.measuredVoltagePhaseW_); - reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / - txPdoC.measuredMotorVoltage_); - reading_.getCommanded().setStamp(updateStamp); - reading_.getCommanded().setCurrent(txPdoC.desiredCurrentQ_); - reading_.getCommanded().setMotorVelocity(motorVelocityScalingInv_ * - txPdoC.desiredMotorVelocity_); - reading_.getCommanded().setGearPosition(gearPositionScalingInv_ * - txPdoC.desiredGearPosition_); - reading_.getCommanded().setGearVelocity(gearVelocityScalingInv_ * - txPdoC.desiredGearVelocity_); - reading_.getCommanded().setJointPosition(jointPositionScalingInv_ * - txPdoC.desiredJointPosition_); - reading_.getCommanded().setJointVelocity(jointVelocityScalingInv_ * - txPdoC.desiredJointVelocity_); - reading_.getCommanded().setJointTorque(jointTorqueScalingInv_ * txPdoC.desiredJointTorque_); - - - Imu imu; - imu.acceleration_x = txPdoC.measuredImuLinearAccelerationX_; - imu.acceleration_y = txPdoC.measuredImuLinearAccelerationY_; - imu.acceleration_z = txPdoC.measuredImuLinearAccelerationZ_; - imu.angle_velocity_x = txPdoC.measuredImuAngularVelocityX_; - imu.angle_velocity_y = txPdoC.measuredImuAngularVelocityY_; - imu.angle_velocity_z = txPdoC.measuredImuAngularVelocityZ_; - reading_.getState().setImu(imu); - } - break; + { + TxPdoC txPdoC; + bus_->readTxPdo(address_, txPdoC); + + RecLock lockReading(readingMutex_); + reading_.getState().setStamp(updateStamp); + reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoC.statusword_)); + reading_.getState().setTemperature(temperatureScalingInv_ * txPdoC.measuredTemperature_ + temperatureOffset_); + reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoC.measuredMotorVoltage_); + reading_.getState().setMotorPosition(motorPositionScalingInv_ * txPdoC.measuredMotorPosition_); + reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoC.measuredGearPosition_); + reading_.getState().setJointPosition(jointPositionScalingInv_ * txPdoC.measuredJointPosition_); + reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoC.measuredMotorCurrent_); + reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * txPdoC.measuredMotorVelocity_); + reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoC.measuredGearVelocity_); + reading_.getState().setJointVelocity(jointVelocityScalingInv_ * txPdoC.measuredJointVelocity_); + reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * txPdoC.measuredJointAcceleration_); + reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoC.measuredJointTorque_); + reading_.getState().setGearPositionTicks(txPdoC.measuredGearPositionTicks_); + reading_.getState().setJointPositionTicks(txPdoC.measuredJointPositionTicks_); + reading_.getState().setTimestamp(txPdoC.timestamp_); + reading_.getState().setDesiredCurrentD(txPdoC.desiredCurrentD_); + reading_.getState().setMeasuredCurrentD(txPdoC.measuredCurrentD_); + reading_.getState().setDesiredCurrentQ(txPdoC.desiredCurrentQ_); + reading_.getState().setMeasuredCurrentQ(txPdoC.measuredCurrentQ_); + reading_.getState().setAlpha(txPdoC.alpha_); + reading_.getState().setBeta(txPdoC.beta_); + reading_.getState().setDutyCycleU(txPdoC.dutyCycleU_); + reading_.getState().setDutyCycleV(txPdoC.dutyCycleV_); + reading_.getState().setDutyCycleW(txPdoC.dutyCycleW_); + reading_.getState().setMeasuredCurrentPhaseU(txPdoC.measuredCurrentPhaseU_); + reading_.getState().setMeasuredCurrentPhaseV(txPdoC.measuredCurrentPhaseV_); + reading_.getState().setMeasuredCurrentPhaseW(txPdoC.measuredCurrentPhaseW_); + reading_.getState().setMeasuredVoltagePhaseU(txPdoC.measuredVoltagePhaseU_); + reading_.getState().setMeasuredVoltagePhaseV(txPdoC.measuredVoltagePhaseV_); + reading_.getState().setMeasuredVoltagePhaseW(txPdoC.measuredVoltagePhaseW_); + reading_.getState().setMeasuredApparentPower(txPdoC.measuredCurrentPhaseU_ * txPdoC.measuredVoltagePhaseU_ + + txPdoC.measuredCurrentPhaseV_ * txPdoC.measuredVoltagePhaseV_ + + txPdoC.measuredCurrentPhaseW_ * txPdoC.measuredVoltagePhaseW_); + reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / + txPdoC.measuredMotorVoltage_); + reading_.getCommanded().setStamp(updateStamp); + reading_.getCommanded().setCurrent(txPdoC.desiredCurrentQ_); + reading_.getCommanded().setMotorVelocity(motorVelocityScalingInv_ * txPdoC.desiredMotorVelocity_); + reading_.getCommanded().setGearPosition(gearPositionScalingInv_ * txPdoC.desiredGearPosition_); + reading_.getCommanded().setGearVelocity(gearVelocityScalingInv_ * txPdoC.desiredGearVelocity_); + reading_.getCommanded().setJointPosition(jointPositionScalingInv_ * txPdoC.desiredJointPosition_); + reading_.getCommanded().setJointVelocity(jointVelocityScalingInv_ * txPdoC.desiredJointVelocity_); + reading_.getCommanded().setJointTorque(jointTorqueScalingInv_ * txPdoC.desiredJointTorque_); + + Imu imu; + imu.acceleration_x = txPdoC.measuredImuLinearAccelerationX_; + imu.acceleration_y = txPdoC.measuredImuLinearAccelerationY_; + imu.acceleration_z = txPdoC.measuredImuLinearAccelerationZ_; + imu.angle_velocity_x = txPdoC.measuredImuAngularVelocityX_; + imu.angle_velocity_y = txPdoC.measuredImuAngularVelocityY_; + imu.angle_velocity_z = txPdoC.measuredImuAngularVelocityZ_; + reading_.getState().setImu(imu); + } break; case PdoTypeEnum::D: - { - - TxPdoD txPdoD; - bus_->readTxPdo(address_, txPdoD); - - RecLock lockReading(readingMutex_); - reading_.getState().setStamp(updateStamp); - reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoD.statusword_)); - reading_.getState().setTemperature(temperatureScalingInv_ * txPdoD.measuredTemperature_ + - temperatureOffset_); - reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoD.measuredMotorVoltage_); - reading_.getState().setMotorPosition(motorPositionScalingInv_ * - txPdoD.measuredMotorPosition_); - reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoD.measuredGearPosition_); - reading_.getState().setJointPosition(jointPositionScalingInv_ * - txPdoD.measuredJointPosition_); - reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoD.measuredMotorCurrent_); - reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * - txPdoD.measuredMotorVelocity_); - reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoD.measuredGearVelocity_); - reading_.getState().setJointVelocity(jointVelocityScalingInv_ * - txPdoD.measuredJointVelocity_); - reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * - txPdoD.measuredJointAcceleration_); - reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoD.measuredJointTorque_); - reading_.getState().setGearPositionTicks(txPdoD.measuredGearPositionTicks_); - reading_.getState().setJointPositionTicks(txPdoD.measuredJointPositionTicks_); - reading_.getState().setTimestamp(txPdoD.timestamp_); - reading_.getState().setDesiredCurrentD(txPdoD.desiredCurrentD_); - reading_.getState().setMeasuredCurrentD(txPdoD.measuredCurrentD_); - reading_.getState().setDesiredCurrentQ(txPdoD.desiredCurrentQ_); - reading_.getState().setMeasuredCurrentQ(txPdoD.measuredCurrentQ_); - reading_.getState().setMeasuredApparentPower(txPdoD.measuredCurrentPhaseU_ * - txPdoD.measuredVoltagePhaseU_ + txPdoD.measuredCurrentPhaseV_ * - txPdoD.measuredVoltagePhaseV_ + txPdoD.measuredCurrentPhaseW_ * - txPdoD.measuredVoltagePhaseW_); - reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / - txPdoD.measuredMotorVoltage_); - reading_.getState().setMeasuredCurrentPhaseU(txPdoD.measuredCurrentPhaseU_); - reading_.getState().setMeasuredCurrentPhaseV(txPdoD.measuredCurrentPhaseV_); - reading_.getState().setMeasuredCurrentPhaseW(txPdoD.measuredCurrentPhaseW_); - reading_.getState().setMeasuredVoltagePhaseU(txPdoD.measuredVoltagePhaseU_); - reading_.getState().setMeasuredVoltagePhaseV(txPdoD.measuredVoltagePhaseV_); - reading_.getState().setMeasuredVoltagePhaseW(txPdoD.measuredVoltagePhaseW_); - } - break; + { + TxPdoD txPdoD; + bus_->readTxPdo(address_, txPdoD); + + RecLock lockReading(readingMutex_); + reading_.getState().setStamp(updateStamp); + reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoD.statusword_)); + reading_.getState().setTemperature(temperatureScalingInv_ * txPdoD.measuredTemperature_ + temperatureOffset_); + reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoD.measuredMotorVoltage_); + reading_.getState().setMotorPosition(motorPositionScalingInv_ * txPdoD.measuredMotorPosition_); + reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoD.measuredGearPosition_); + reading_.getState().setJointPosition(jointPositionScalingInv_ * txPdoD.measuredJointPosition_); + reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoD.measuredMotorCurrent_); + reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * txPdoD.measuredMotorVelocity_); + reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoD.measuredGearVelocity_); + reading_.getState().setJointVelocity(jointVelocityScalingInv_ * txPdoD.measuredJointVelocity_); + reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * txPdoD.measuredJointAcceleration_); + reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoD.measuredJointTorque_); + reading_.getState().setGearPositionTicks(txPdoD.measuredGearPositionTicks_); + reading_.getState().setJointPositionTicks(txPdoD.measuredJointPositionTicks_); + reading_.getState().setTimestamp(txPdoD.timestamp_); + reading_.getState().setDesiredCurrentD(txPdoD.desiredCurrentD_); + reading_.getState().setMeasuredCurrentD(txPdoD.measuredCurrentD_); + reading_.getState().setDesiredCurrentQ(txPdoD.desiredCurrentQ_); + reading_.getState().setMeasuredCurrentQ(txPdoD.measuredCurrentQ_); + reading_.getState().setMeasuredApparentPower(txPdoD.measuredCurrentPhaseU_ * txPdoD.measuredVoltagePhaseU_ + + txPdoD.measuredCurrentPhaseV_ * txPdoD.measuredVoltagePhaseV_ + + txPdoD.measuredCurrentPhaseW_ * txPdoD.measuredVoltagePhaseW_); + reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / + txPdoD.measuredMotorVoltage_); + reading_.getState().setMeasuredCurrentPhaseU(txPdoD.measuredCurrentPhaseU_); + reading_.getState().setMeasuredCurrentPhaseV(txPdoD.measuredCurrentPhaseV_); + reading_.getState().setMeasuredCurrentPhaseW(txPdoD.measuredCurrentPhaseW_); + reading_.getState().setMeasuredVoltagePhaseU(txPdoD.measuredVoltagePhaseU_); + reading_.getState().setMeasuredVoltagePhaseV(txPdoD.measuredVoltagePhaseV_); + reading_.getState().setMeasuredVoltagePhaseW(txPdoD.measuredVoltagePhaseW_); + } break; case PdoTypeEnum::E: - { - TxPdoE txPdoE; - bus_->readTxPdo(address_, txPdoE); - - RecLock lockReading(readingMutex_); - reading_.getState().setStamp(updateStamp); - reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoE.statusword_)); - reading_.getState().setTemperature(temperatureScalingInv_ * txPdoE.measuredTemperature_ + - temperatureOffset_); - reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoE.measuredMotorVoltage_); - reading_.getState().setMotorPosition(motorPositionScalingInv_ * - txPdoE.measuredMotorPosition_); - reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoE.measuredGearPosition_); - reading_.getState().setJointPosition(jointPositionScalingInv_ * - txPdoE.measuredJointPosition_); - reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoE.measuredMotorCurrent_); - reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * - txPdoE.measuredMotorVelocity_); - reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoE.measuredGearVelocity_); - reading_.getState().setJointVelocity(jointVelocityScalingInv_ * - txPdoE.measuredJointVelocity_); - reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * - txPdoE.measuredJointAcceleration_); - reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoE.measuredJointTorque_); - reading_.getState().setGearPositionTicks(txPdoE.measuredGearPositionTicks_); - reading_.getState().setJointPositionTicks(txPdoE.measuredJointPositionTicks_); - reading_.getState().setTimestamp(txPdoE.timestamp_); - reading_.getState().setDesiredCurrentD(txPdoE.desiredCurrentD_); - reading_.getState().setMeasuredCurrentD(txPdoE.measuredCurrentD_); - reading_.getState().setDesiredCurrentQ(txPdoE.desiredCurrentQ_); - reading_.getState().setMeasuredCurrentQ(txPdoE.measuredCurrentQ_); - reading_.getState().setAlpha(txPdoE.alpha_); - reading_.getState().setBeta(txPdoE.beta_); - reading_.getState().setDutyCycleU(txPdoE.dutyCycleU_); - reading_.getState().setDutyCycleV(txPdoE.dutyCycleV_); - reading_.getState().setDutyCycleW(txPdoE.dutyCycleW_); - reading_.getState().setMeasuredCurrentPhaseU(txPdoE.measuredCurrentPhaseU_); - reading_.getState().setMeasuredCurrentPhaseV(txPdoE.measuredCurrentPhaseV_); - reading_.getState().setMeasuredCurrentPhaseW(txPdoE.measuredCurrentPhaseW_); - reading_.getState().setMeasuredVoltagePhaseU(txPdoE.measuredVoltagePhaseU_); - reading_.getState().setMeasuredVoltagePhaseV(txPdoE.measuredVoltagePhaseV_); - reading_.getState().setMeasuredVoltagePhaseW(txPdoE.measuredVoltagePhaseW_); - reading_.getState().setMeasuredApparentPower(txPdoE.measuredCurrentPhaseU_ * - txPdoE.measuredVoltagePhaseU_ + txPdoE.measuredCurrentPhaseV_ * - txPdoE.measuredVoltagePhaseV_ + txPdoE.measuredCurrentPhaseW_ * - txPdoE.measuredVoltagePhaseW_); - reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / - txPdoE.measuredMotorVoltage_); - reading_.getCommanded().setStamp(updateStamp); - reading_.getCommanded().setCurrent(txPdoE.desiredCurrentQ_); - reading_.getCommanded().setMotorVelocity(motorVelocityScalingInv_ * - txPdoE.desiredMotorVelocity_); - reading_.getCommanded().setGearPosition(gearPositionScalingInv_ * - txPdoE.desiredGearPosition_); - reading_.getCommanded().setGearVelocity(gearVelocityScalingInv_ * - txPdoE.desiredGearVelocity_); - reading_.getCommanded().setJointPosition(jointPositionScalingInv_ * - txPdoE.desiredJointPosition_); - reading_.getCommanded().setJointVelocity(jointVelocityScalingInv_ * - txPdoE.desiredJointVelocity_); - reading_.getCommanded().setJointTorque(jointTorqueScalingInv_ * txPdoE.desiredJointTorque_); - Imu imu; - imu.acceleration_x = txPdoE.measuredImuLinearAccelerationX_; - imu.acceleration_y = txPdoE.measuredImuLinearAccelerationY_; - imu.acceleration_z = txPdoE.measuredImuLinearAccelerationZ_; - imu.angle_velocity_x = txPdoE.measuredImuAngularVelocityX_; - imu.angle_velocity_y = txPdoE.measuredImuAngularVelocityY_; - imu.angle_velocity_z = txPdoE.measuredImuAngularVelocityZ_; - reading_.getState().setImu(imu); - - reading_.getState().setCoilTemp1(txPdoE.measuredCoilTemp1_ / 1000.0); - reading_.getState().setCoilTemp2(txPdoE.measuredCoilTemp2_ / 1000.0); - reading_.getState().setCoilTemp3(txPdoE.measuredCoilTemp3_ / 1000.0); - - } - break; + { + TxPdoE txPdoE; + bus_->readTxPdo(address_, txPdoE); + + RecLock lockReading(readingMutex_); + reading_.getState().setStamp(updateStamp); + reading_.getState().setStatusword(rsl_drive_sdk::Statusword(txPdoE.statusword_)); + reading_.getState().setTemperature(temperatureScalingInv_ * txPdoE.measuredTemperature_ + temperatureOffset_); + reading_.getState().setVoltage(motorVoltageScalingInv_ * txPdoE.measuredMotorVoltage_); + reading_.getState().setMotorPosition(motorPositionScalingInv_ * txPdoE.measuredMotorPosition_); + reading_.getState().setGearPosition(gearPositionScalingInv_ * txPdoE.measuredGearPosition_); + reading_.getState().setJointPosition(jointPositionScalingInv_ * txPdoE.measuredJointPosition_); + reading_.getState().setCurrent(motorCurrentScalingInv_ * txPdoE.measuredMotorCurrent_); + reading_.getState().setMotorVelocity(motorVelocityScalingInv_ * txPdoE.measuredMotorVelocity_); + reading_.getState().setGearVelocity(gearVelocityScalingInv_ * txPdoE.measuredGearVelocity_); + reading_.getState().setJointVelocity(jointVelocityScalingInv_ * txPdoE.measuredJointVelocity_); + reading_.getState().setJointAcceleration(jointAccelerationScalingInv_ * txPdoE.measuredJointAcceleration_); + reading_.getState().setJointTorque(jointTorqueScalingInv_ * txPdoE.measuredJointTorque_); + reading_.getState().setGearPositionTicks(txPdoE.measuredGearPositionTicks_); + reading_.getState().setJointPositionTicks(txPdoE.measuredJointPositionTicks_); + reading_.getState().setTimestamp(txPdoE.timestamp_); + reading_.getState().setDesiredCurrentD(txPdoE.desiredCurrentD_); + reading_.getState().setMeasuredCurrentD(txPdoE.measuredCurrentD_); + reading_.getState().setDesiredCurrentQ(txPdoE.desiredCurrentQ_); + reading_.getState().setMeasuredCurrentQ(txPdoE.measuredCurrentQ_); + reading_.getState().setAlpha(txPdoE.alpha_); + reading_.getState().setBeta(txPdoE.beta_); + reading_.getState().setDutyCycleU(txPdoE.dutyCycleU_); + reading_.getState().setDutyCycleV(txPdoE.dutyCycleV_); + reading_.getState().setDutyCycleW(txPdoE.dutyCycleW_); + reading_.getState().setMeasuredCurrentPhaseU(txPdoE.measuredCurrentPhaseU_); + reading_.getState().setMeasuredCurrentPhaseV(txPdoE.measuredCurrentPhaseV_); + reading_.getState().setMeasuredCurrentPhaseW(txPdoE.measuredCurrentPhaseW_); + reading_.getState().setMeasuredVoltagePhaseU(txPdoE.measuredVoltagePhaseU_); + reading_.getState().setMeasuredVoltagePhaseV(txPdoE.measuredVoltagePhaseV_); + reading_.getState().setMeasuredVoltagePhaseW(txPdoE.measuredVoltagePhaseW_); + reading_.getState().setMeasuredApparentPower(txPdoE.measuredCurrentPhaseU_ * txPdoE.measuredVoltagePhaseU_ + + txPdoE.measuredCurrentPhaseV_ * txPdoE.measuredVoltagePhaseV_ + + txPdoE.measuredCurrentPhaseW_ * txPdoE.measuredVoltagePhaseW_); + reading_.getState().setMeasuredInputCurrent(reading_.getState().getMeasuredApparentPower() / + txPdoE.measuredMotorVoltage_); + reading_.getCommanded().setStamp(updateStamp); + reading_.getCommanded().setCurrent(txPdoE.desiredCurrentQ_); + reading_.getCommanded().setMotorVelocity(motorVelocityScalingInv_ * txPdoE.desiredMotorVelocity_); + reading_.getCommanded().setGearPosition(gearPositionScalingInv_ * txPdoE.desiredGearPosition_); + reading_.getCommanded().setGearVelocity(gearVelocityScalingInv_ * txPdoE.desiredGearVelocity_); + reading_.getCommanded().setJointPosition(jointPositionScalingInv_ * txPdoE.desiredJointPosition_); + reading_.getCommanded().setJointVelocity(jointVelocityScalingInv_ * txPdoE.desiredJointVelocity_); + reading_.getCommanded().setJointTorque(jointTorqueScalingInv_ * txPdoE.desiredJointTorque_); + Imu imu; + imu.acceleration_x = txPdoE.measuredImuLinearAccelerationX_; + imu.acceleration_y = txPdoE.measuredImuLinearAccelerationY_; + imu.acceleration_z = txPdoE.measuredImuLinearAccelerationZ_; + imu.angle_velocity_x = txPdoE.measuredImuAngularVelocityX_; + imu.angle_velocity_y = txPdoE.measuredImuAngularVelocityY_; + imu.angle_velocity_z = txPdoE.measuredImuAngularVelocityZ_; + reading_.getState().setImu(imu); + + reading_.getState().setCoilTemp1(txPdoE.measuredCoilTemp1_ / 1000.0); + reading_.getState().setCoilTemp2(txPdoE.measuredCoilTemp2_ / 1000.0); + reading_.getState().setCoilTemp3(txPdoE.measuredCoilTemp3_ / 1000.0); + + } break; default: break; } - // Save the command in the reading. + // Save the command in the reading. if (!isRtdlRunning_) { RecLock lockReading(readingMutex_); RecLock lockCommand(commandMutex_); @@ -595,12 +532,12 @@ void DriveEthercatDevice::updateWrite() RecLock lock(commandMutex_); const rsl_drive_sdk::mode::ModeEnum modeEnum = command_.getModeEnum(); - if(modeEnum == rsl_drive_sdk::mode::ModeEnum::NA) { + if (modeEnum == rsl_drive_sdk::mode::ModeEnum::NA) { return; } const uint16_t modeOfOperation = rsl_drive_sdk::mode::modeEnumToId(modeEnum); - const auto & mode = config_.getMode(modeEnum); + const auto& mode = config_.getMode(modeEnum); if (!mode) { return; } @@ -633,19 +570,17 @@ void DriveEthercatDevice::updateWrite() rxPdo.controlParameterD_ = gainScaling_ * 0.0; bus_->writeRxPdo(address_, rxPdo); - } void DriveEthercatDevice::shutdown() { - // setGoalStateEnum(rsl_drive_sdk::fsm::StateEnum::Standby, true, 100000,1000); - + // setGoalStateEnum(rsl_drive_sdk::fsm::StateEnum::Standby, true, 100000,1000); } void DriveEthercatDevice::preShutdown() { auto shutdown_state = config_.getGoalStateEnumShutdown(); - if(shutdown_state == fsm::StateEnum::NA) { + if (shutdown_state == fsm::StateEnum::NA) { shutdown_state = fsm::StateEnum::Configure; } @@ -654,15 +589,14 @@ void DriveEthercatDevice::preShutdown() if (setFSMGoalState(shutdown_state, true, 10, 10)) { MELO_INFO("Pre shutdown finished"); } else { - MELO_ERROR_STREAM( - "[DriveEthercatDevice::preShutdown] shutdown state not reached for rsl_drive_sdk '" << - getName() << "'"); + MELO_ERROR_STREAM("[DriveEthercatDevice::preShutdown] shutdown state not reached for rsl_drive_sdk '" << getName() + << "'"); } } bool DriveEthercatDevice::deviceIsMissing() const { - return false; // TODO + return false; // TODO } void DriveEthercatDevice::setState(const uint16_t state) @@ -677,7 +611,6 @@ bool DriveEthercatDevice::waitForState(const uint16_t state) return bus_->waitForState(state, address_); } - PdoTypeEnum DriveEthercatDevice::getCurrentPdoTypeEnum() const { return currentPdoTypeEnum_; @@ -688,32 +621,32 @@ DriveEthercatDevice::PdoInfo DriveEthercatDevice::getCurrentPdoInfo() const return pdoInfos_.at(currentPdoTypeEnum_); } -bool DriveEthercatDevice::getDriveModel(std::string & model) +bool DriveEthercatDevice::getDriveModel(std::string& model) { return sendSdoReadVisibleString(OD_DSP402_MODEL_ID, 0, model); } -bool DriveEthercatDevice::getDriveInfoSerialNumber(std::string & serialNumber) +bool DriveEthercatDevice::getDriveInfoSerialNumber(std::string& serialNumber) { return sendSdoReadStringCustom(OD_DRIVE_INFO_HARDWARE_SERIAL_NUMBER_ID, serialNumber); } -bool DriveEthercatDevice::setDriveInfoSerialNumber(const std::string & serialNumber) +bool DriveEthercatDevice::setDriveInfoSerialNumber(const std::string& serialNumber) { return sendSdoWriteStringCustom(OD_DRIVE_INFO_HARDWARE_SERIAL_NUMBER_ID, serialNumber); } -bool DriveEthercatDevice::getDriveInfoName(std::string & name) +bool DriveEthercatDevice::getDriveInfoName(std::string& name) { return sendSdoReadStringCustom(OD_DRIVE_INFO_DRIVE_NAME_ID, name); } -bool DriveEthercatDevice::setDriveInfoName(const std::string & name) +bool DriveEthercatDevice::setDriveInfoName(const std::string& name) { return sendSdoWriteStringCustom(OD_DRIVE_INFO_DRIVE_NAME_ID, name); } -bool DriveEthercatDevice::getDriveInfoId(uint16_t & id) +bool DriveEthercatDevice::getDriveInfoId(uint16_t& id) { RecLock lock(mutex_); return sendSdoRead(OD_DRIVE_INFO_DRIVE_ID_ID, 0x00, false, id); @@ -725,8 +658,7 @@ bool DriveEthercatDevice::setDriveInfoId(const uint16_t id) return sendSdoWrite(OD_DRIVE_INFO_DRIVE_ID_ID, 0x00, false, id); } -bool DriveEthercatDevice::getDriveInfoBootloaderVersion( - rsl_drive_sdk::common::Version & bootloaderVersion) +bool DriveEthercatDevice::getDriveInfoBootloaderVersion(rsl_drive_sdk::common::Version& bootloaderVersion) { std::string bootloaderVersionString; if (!sendSdoReadStringCustom(OD_DRIVE_INFO_BOOTLOADER_VERSION_ID, bootloaderVersionString)) { @@ -736,15 +668,12 @@ bool DriveEthercatDevice::getDriveInfoBootloaderVersion( return true; } -bool DriveEthercatDevice::setDriveInfoBootloaderVersion( - const rsl_drive_sdk::common::Version & bootloaderVersion) +bool DriveEthercatDevice::setDriveInfoBootloaderVersion(const rsl_drive_sdk::common::Version& bootloaderVersion) { - return sendSdoWriteStringCustom(OD_DRIVE_INFO_BOOTLOADER_VERSION_ID, - bootloaderVersion.toString()); + return sendSdoWriteStringCustom(OD_DRIVE_INFO_BOOTLOADER_VERSION_ID, bootloaderVersion.toString()); } -bool DriveEthercatDevice::getDriveInfoFirmwareVersion( - rsl_drive_sdk::common::Version & firmwareVersion) +bool DriveEthercatDevice::getDriveInfoFirmwareVersion(rsl_drive_sdk::common::Version& firmwareVersion) { std::string firmwareVersionString; if (!sendSdoReadStringCustom(OD_DRIVE_INFO_FIRMWARE_VERSION_ID, firmwareVersionString)) { @@ -754,16 +683,14 @@ bool DriveEthercatDevice::getDriveInfoFirmwareVersion( return true; } -bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareInfo & firmwareInfo) +bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareInfo& firmwareInfo) { uint8_t infoRaw[128]; uint8_t numberOfSubindexes = 0; bool ok = false; for (int j = 0; j < 20; ++j) { - if (sendSdoRead(OD_DRIVE_FIRMWARE_INFO_ID, OD_DRIVE_FIRMWARE_INFO_SID_0, false, - numberOfSubindexes)) - { + if (sendSdoRead(OD_DRIVE_FIRMWARE_INFO_ID, OD_DRIVE_FIRMWARE_INFO_SID_0, false, numberOfSubindexes)) { ok = true; break; } @@ -783,9 +710,7 @@ bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareIn ok = false; for (int j = 0; j < 20; ++j) { rsl_drive_sdk::thread_sleep(0.0001); - if (sendSdoRead(OD_DRIVE_FIRMWARE_INFO_ID, OD_DRIVE_FIRMWARE_INFO_SID_DATA + i, false, - ((uint32_t *)infoRaw)[i])) - { + if (sendSdoRead(OD_DRIVE_FIRMWARE_INFO_ID, OD_DRIVE_FIRMWARE_INFO_SID_DATA + i, false, ((uint32_t*)infoRaw)[i])) { ok = true; break; } @@ -799,7 +724,7 @@ bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareIn firmwareInfo.infoVersion = infoRaw[0]; if (firmwareInfo.infoVersion == 3) { - // Get version. + // Get version. std::stringstream versionStream; for (int i = 0; i < 3; ++i) { versionStream << (int)infoRaw[1 + i]; @@ -809,29 +734,27 @@ bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareIn } firmwareInfo.version = versionStream.str(); - // Get fw hash. + // Get fw hash. std::stringstream hashStream; for (int i = 0; i < 16; ++i) { if (infoRaw[4 + i] == '\0') { break; } - hashStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) - << std::hex << (int)infoRaw[4 + i]; + hashStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) << std::hex << (int)infoRaw[4 + i]; } firmwareInfo.fwHash = hashStream.str(); - // Get channel id. + // Get channel id. std::stringstream channelIdStream; for (int i = 0; i < 16; ++i) { if (infoRaw[20 + i] == '\0') { break; } - channelIdStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) - << std::hex << (int)infoRaw[20 + i]; + channelIdStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) << std::hex << (int)infoRaw[20 + i]; } firmwareInfo.channelId = channelIdStream.str(); - // Get channel tid. + // Get channel tid. std::stringstream channelTidStream; for (int i = 0; i < 16; ++i) { if (infoRaw[36 + i] == '\0') { @@ -841,7 +764,7 @@ bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareIn } firmwareInfo.channelTid = channelTidStream.str(); - // Get serial number. + // Get serial number. std::stringstream serialNumberStream; for (int i = 0; i < 16; ++i) { if (infoRaw[52 + i] == '\0') { @@ -851,28 +774,25 @@ bool DriveEthercatDevice::getDriveFirmwareInfo(rsl_drive_sdk::common::FirmwareIn } firmwareInfo.serialNumber = serialNumberStream.str(); - // Get key id. + // Get key id. std::stringstream keyIdStream; for (int i = 0; i < 16; ++i) { - keyIdStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) - << std::hex << (int)infoRaw[68 + i]; + keyIdStream << std::setfill('0') << std::setw(sizeof(uint8_t) * 2) << std::hex << (int)infoRaw[68 + i]; if (i == 3 || i == 5 || i == 7 || i == 9) { keyIdStream << "-"; } } firmwareInfo.keyId = keyIdStream.str(); } else { - MELO_ERROR_STREAM( - "Firmware info version (" - << (int)firmwareInfo.infoVersion - << ") is not compatible with this SDK version. " - "Please update the SDK.") + MELO_ERROR_STREAM("Firmware info version (" << (int)firmwareInfo.infoVersion + << ") is not compatible with this SDK version. " + "Please update the SDK.") return false; } return true; } -bool DriveEthercatDevice::getBuildInfo(rsl_drive_sdk::common::BuildInfo & buildInfo) +bool DriveEthercatDevice::getBuildInfo(rsl_drive_sdk::common::BuildInfo& buildInfo) { bool success = true; success &= sendSdoReadVisibleString(OD_BUILDINFO_ID, 0x01, buildInfo.buildDate); @@ -886,11 +806,11 @@ bool DriveEthercatDevice::getBuildInfo(rsl_drive_sdk::common::BuildInfo & buildI return success; } -bool DriveEthercatDevice::getDriveType(std::string & type) +bool DriveEthercatDevice::getDriveType(std::string& type) { - return sendSdoReadVisibleString(OD_DRIVETYPE_ID, 0x01, type); + return sendSdoReadVisibleString(OD_DRIVETYPE_ID, 0x01, type); } -bool DriveEthercatDevice::getGearboxRatio(float & ratio) +bool DriveEthercatDevice::getGearboxRatio(float& ratio) { return sendSdoReadFloat(OD_GEARBOX_RATIO_ID, 0x00, false, ratio); } @@ -899,8 +819,7 @@ bool DriveEthercatDevice::eraseFlashStorage() { RecLock lock(mutex_); MELO_INFO_STREAM("Erasing flash storage."); - return sendSdoWrite(OD_FLASH_ERASE_ID, 0x00, false, - static_cast(OD_FLASH_ERASE_ID_VAL_RUN)); + return sendSdoWrite(OD_FLASH_ERASE_ID, 0x00, false, static_cast(OD_FLASH_ERASE_ID_VAL_RUN)); } bool DriveEthercatDevice::resetFlashStorageSections(const uint16_t flashStorageSections) @@ -910,13 +829,12 @@ bool DriveEthercatDevice::resetFlashStorageSections(const uint16_t flashStorageS return sendSdoWrite(OD_FLASH_RESET_ID, 0x00, false, flashStorageSections); } -bool DriveEthercatDevice::getCalibrationState( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::CalibrationState & calibrationState) +bool DriveEthercatDevice::getCalibrationState(const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::CalibrationState& calibrationState) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -930,26 +848,24 @@ bool DriveEthercatDevice::getCalibrationState( return success; } -bool DriveEthercatDevice::sendCalibrationGearAndJointEncoderHomingNewJointPosition( - const double newJointPosition) +bool DriveEthercatDevice::sendCalibrationGearAndJointEncoderHomingNewJointPosition(const double newJointPosition) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Sending homing calibration new joint position."); return sendSdoWrite(OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_NEW_JOINT_POSITION_ID, 0x00, false, - static_cast(jointPositionScaling_ * newJointPosition)); + static_cast(jointPositionScaling_ * newJointPosition)); } -bool DriveEthercatDevice::startCalibration( - const rsl_drive_sdk::calibration::CalibrationModeEnum calibrationModeEnum) +bool DriveEthercatDevice::startCalibration(const rsl_drive_sdk::calibration::CalibrationModeEnum calibrationModeEnum) { RecLock lock(mutex_); - MELO_INFO_STREAM("Starting calibration '" << - rsl_drive_sdk::calibration::calibrationModeEnumToName(calibrationModeEnum) << "'."); + MELO_INFO_STREAM("Starting calibration '" + << rsl_drive_sdk::calibration::calibrationModeEnumToName(calibrationModeEnum) << "'."); return sendSdoWrite(OD_CALIB_MODE_ID, 0x00, false, - rsl_drive_sdk::calibration::calibrationModeEnumToId(calibrationModeEnum)); + rsl_drive_sdk::calibration::calibrationModeEnumToId(calibrationModeEnum)); } -bool DriveEthercatDevice::calibrationIsRunning(bool & running) +bool DriveEthercatDevice::calibrationIsRunning(bool& running) { RecLock lock(mutex_); uint16_t calibrationMode = 0; @@ -961,20 +877,20 @@ bool DriveEthercatDevice::calibrationIsRunning(bool & running) } bool DriveEthercatDevice::getCalibrationMotorEncoderOffset( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::MotorEncoderOffset & motorEncoderOffset) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::MotorEncoderOffset& motorEncoderOffset) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } if (!setCalibrationTypeEnum(calibrationTypeEnum)) { return false; } - const bool success = sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, - OD_CALIB_MOTOR_ENCODER_PARAMS_SID_OFFSET, false, motorEncoderOffset.value_); + const bool success = sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_OFFSET, false, + motorEncoderOffset.value_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -982,12 +898,12 @@ bool DriveEthercatDevice::getCalibrationMotorEncoderOffset( } bool DriveEthercatDevice::getCalibrationMotorEncoderParameters( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::MotorEncoderParameters & motorEncoderParameters) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::MotorEncoderParameters& motorEncoderParameters) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -995,20 +911,20 @@ bool DriveEthercatDevice::getCalibrationMotorEncoderParameters( return false; } bool success = true; - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DGAIN, - false, motorEncoderParameters.dGain_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFS, - false, motorEncoderParameters.dOffs_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFC, - false, motorEncoderParameters.dOffc_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DPH, - false, motorEncoderParameters.dPh_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AGAIN, - false, motorEncoderParameters.aGain_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFS, - false, motorEncoderParameters.aOffs_); - success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFC, - false, motorEncoderParameters.aOffc_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DGAIN, false, + motorEncoderParameters.dGain_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFS, false, + motorEncoderParameters.dOffs_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DOFFC, false, + motorEncoderParameters.dOffc_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_DPH, false, + motorEncoderParameters.dPh_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AGAIN, false, + motorEncoderParameters.aGain_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFS, false, + motorEncoderParameters.aOffs_); + success &= sendSdoRead(OD_CALIB_MOTOR_ENCODER_PARAMS_ID, OD_CALIB_MOTOR_ENCODER_PARAMS_SID_AOFFC, false, + motorEncoderParameters.aOffc_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1016,12 +932,12 @@ bool DriveEthercatDevice::getCalibrationMotorEncoderParameters( } bool DriveEthercatDevice::getCalibrationGearJointEncoderOffset( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::GearJointEncoderOffset & gearJointEncoderOffset) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::GearJointEncoderOffset& gearJointEncoderOffset) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1029,20 +945,16 @@ bool DriveEthercatDevice::getCalibrationGearJointEncoderOffset( return false; } bool success = true; - success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, false, gearJointEncoderOffset.constant_); - success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_AMPLITUDE, false, - gearJointEncoderOffset.sin1Amplitude_); - success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_PHASESHIFT, false, - gearJointEncoderOffset.sin1Phaseshift_); - success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_AMPLITUDE, false, - gearJointEncoderOffset.sin2Amplitude_); - success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_PHASESHIFT, false, - gearJointEncoderOffset.sin2Phaseshift_); + success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, false, + gearJointEncoderOffset.constant_); + success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_AMPLITUDE, + false, gearJointEncoderOffset.sin1Amplitude_); + success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN1_PHASESHIFT, + false, gearJointEncoderOffset.sin1Phaseshift_); + success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_AMPLITUDE, + false, gearJointEncoderOffset.sin2Amplitude_); + success &= sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_SIN2_PHASESHIFT, + false, gearJointEncoderOffset.sin2Phaseshift_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1050,27 +962,27 @@ bool DriveEthercatDevice::getCalibrationGearJointEncoderOffset( } bool DriveEthercatDevice::setCalibrationGearAndJointEncoderHoming( - [[maybe_unused]] const calibration::CalibrationTypeEnum calibrationTypeEnum, - const calibration::parameter::GearAndJointEncoderHoming & gearAndJointEncoderHoming) + [[maybe_unused]] const calibration::CalibrationTypeEnum calibrationTypeEnum, + const calibration::parameter::GearAndJointEncoderHoming& gearAndJointEncoderHoming) { RecLock lock(mutex_); bool success = true; success &= sendSdoWrite(OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID, - OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR, false, - gearAndJointEncoderHoming.gearEncoderRawTicks_); + OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR, false, + gearAndJointEncoderHoming.gearEncoderRawTicks_); success &= sendSdoWrite(OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID, - OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT, false, - gearAndJointEncoderHoming.jointEncoderRawTicks_); + OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT, false, + gearAndJointEncoderHoming.jointEncoderRawTicks_); return success; } bool DriveEthercatDevice::getCalibrationGearAndJointEncoderHoming( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming & gearAndJointEncoderHoming) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::GearAndJointEncoderHoming& gearAndJointEncoderHoming) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1079,11 +991,11 @@ bool DriveEthercatDevice::getCalibrationGearAndJointEncoderHoming( } bool success = true; success &= sendSdoRead(OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID, - OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR, false, - gearAndJointEncoderHoming.gearEncoderRawTicks_); + OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_GEAR, false, + gearAndJointEncoderHoming.gearEncoderRawTicks_); success &= sendSdoRead(OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_ID, - OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT, false, - gearAndJointEncoderHoming.jointEncoderRawTicks_); + OD_CALIB_GEAR_AND_JOINT_ENCODER_HOMING_TICKS_SID_JOINT, false, + gearAndJointEncoderHoming.jointEncoderRawTicks_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1091,12 +1003,12 @@ bool DriveEthercatDevice::getCalibrationGearAndJointEncoderHoming( } bool DriveEthercatDevice::getCalibrationImuGyroscopeDcBias( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::ImuGyroscopeDcBias & imuGyroscopeDcBias) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::ImuGyroscopeDcBias& imuGyroscopeDcBias) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1104,12 +1016,12 @@ bool DriveEthercatDevice::getCalibrationImuGyroscopeDcBias( return false; } bool success = true; - success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_X, - false, imuGyroscopeDcBias.x_); - success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Y, - false, imuGyroscopeDcBias.y_); - success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Z, - false, imuGyroscopeDcBias.z_); + success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_X, false, + imuGyroscopeDcBias.x_); + success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Y, false, + imuGyroscopeDcBias.y_); + success &= sendSdoRead(OD_CALIB_IMU_GYROSCOPE_DC_BIAS_ID, OD_CALIB_IMU_GYROSCOPE_DC_BIAS_SID_Z, false, + imuGyroscopeDcBias.z_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1117,25 +1029,23 @@ bool DriveEthercatDevice::getCalibrationImuGyroscopeDcBias( } bool DriveEthercatDevice::setCalibrationSpringStiffness( - [[maybe_unused]] const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - const rsl_drive_sdk::calibration::parameter::SpringStiffness & springStiffness) + [[maybe_unused]] const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + const rsl_drive_sdk::calibration::parameter::SpringStiffness& springStiffness) { RecLock lock(mutex_); bool success = true; - success &= sendSdoWrite(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_NEG, false, - springStiffness.neg_); - success &= sendSdoWrite(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_POS, false, - springStiffness.pos_); + success &= sendSdoWrite(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_NEG, false, springStiffness.neg_); + success &= sendSdoWrite(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_POS, false, springStiffness.pos_); return success; } bool DriveEthercatDevice::getCalibrationSpringStiffness( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::SpringStiffness & springStiffness) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::SpringStiffness& springStiffness) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1143,10 +1053,8 @@ bool DriveEthercatDevice::getCalibrationSpringStiffness( return false; } bool success = true; - success &= sendSdoRead(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_NEG, false, - springStiffness.neg_); - success &= sendSdoRead(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_POS, false, - springStiffness.pos_); + success &= sendSdoRead(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_NEG, false, springStiffness.neg_); + success &= sendSdoRead(OD_CALIB_SPRING_STIFFNESS_ID, OD_CALIB_SPRING_STIFFNESS_SID_POS, false, springStiffness.pos_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1154,33 +1062,29 @@ bool DriveEthercatDevice::getCalibrationSpringStiffness( } bool DriveEthercatDevice::setCalibrationFrictionEstimation( - [[maybe_unused]] const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - const rsl_drive_sdk::calibration::parameter::FrictionEstimation & frictionEstimation) + [[maybe_unused]] const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + const rsl_drive_sdk::calibration::parameter::FrictionEstimation& frictionEstimation) { RecLock lock(mutex_); bool success = true; - success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION, false, - frictionEstimation.breakAwayFriction_); - success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND, false, - frictionEstimation.breakAwayFrictionBand_); - success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG, false, - frictionEstimation.viscousFrictionCoeffNeg_); - success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS, false, - frictionEstimation.viscousFrictionCoeffPos_); + success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION, false, + frictionEstimation.breakAwayFriction_); + success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND, + false, frictionEstimation.breakAwayFrictionBand_); + success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG, + false, frictionEstimation.viscousFrictionCoeffNeg_); + success &= sendSdoWrite(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS, + false, frictionEstimation.viscousFrictionCoeffPos_); return success; } bool DriveEthercatDevice::getCalibrationFrictionEstimation( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, - rsl_drive_sdk::calibration::parameter::FrictionEstimation & frictionEstimation) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum, + rsl_drive_sdk::calibration::parameter::FrictionEstimation& frictionEstimation) { RecLock lock(mutex_); rsl_drive_sdk::calibration::CalibrationTypeEnum previousCalibrationTypeEnum = - rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; + rsl_drive_sdk::calibration::CalibrationTypeEnum::NA; if (!getCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } @@ -1188,40 +1092,34 @@ bool DriveEthercatDevice::getCalibrationFrictionEstimation( return false; } bool success = true; - success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION, false, - frictionEstimation.breakAwayFriction_); - success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND, false, - frictionEstimation.breakAwayFrictionBand_); - success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG, false, - frictionEstimation.viscousFrictionCoeffNeg_); - success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, - OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS, false, - frictionEstimation.viscousFrictionCoeffPos_); + success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION, false, + frictionEstimation.breakAwayFriction_); + success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_BREAK_AWAY_FRICTION_BAND, + false, frictionEstimation.breakAwayFrictionBand_); + success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_NEG, + false, frictionEstimation.viscousFrictionCoeffNeg_); + success &= sendSdoRead(OD_CALIB_FRICTION_ESTIMATION_ID, OD_CALIB_FRICTION_ESTIMATION_SID_VISCOUS_FRICTION_COEFF_POS, + false, frictionEstimation.viscousFrictionCoeffPos_); if (!setCalibrationTypeEnum(previousCalibrationTypeEnum)) { return false; } return success; } -bool DriveEthercatDevice::getCalibrationGearJointEncoderOffsetConstant(int32_t & constant) +bool DriveEthercatDevice::getCalibrationGearJointEncoderOffsetConstant(int32_t& constant) { - return sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, - false, constant); + return sendSdoRead(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, false, + constant); } bool DriveEthercatDevice::setCalibrationGearJointEncoderOffsetConstant(const int32_t constant) { - return sendSdoWrite(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, - OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, - false, constant); + return sendSdoWrite(OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_ID, OD_CALIB_GEAR_JOINT_ENCODER_OFFSET_SID_CONSTANT, false, + constant); } bool DriveEthercatDevice::resetCustomCalibrationsToFactory( - const rsl_drive_sdk::calibration::CalibrationState calibrationState) + const rsl_drive_sdk::calibration::CalibrationState calibrationState) { RecLock lock(mutex_); MELO_INFO_STREAM("Resetting custom calibrations to factory. Fields: " << calibrationState.all_); @@ -1233,10 +1131,10 @@ bool DriveEthercatDevice::writeFactoryCalibration() RecLock lock(mutex_); MELO_INFO_STREAM("Writing factory calibration to flash."); return sendSdoWrite(OD_CALIB_CUSTOM_TO_FACTORY_ID, 0x00, false, - static_cast(OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_RUN)); + static_cast(OD_CALIB_CUSTOM_TO_FACTORY_ID_VAL_RUN)); } -bool DriveEthercatDevice::getMaxCurrent(double & maxCurrent) +bool DriveEthercatDevice::getMaxCurrent(double& maxCurrent) { RecLock lock(mutex_); float maxCurrentFloat = 0.0; @@ -1251,11 +1149,10 @@ bool DriveEthercatDevice::setMaxCurrent(const double maxCurrent) { MELO_DEBUG_STREAM("Setting max current (" << maxCurrent << " A)."); RecLock lock(mutex_); - return sendSdoWrite(OD_DSP402_CURRENT_MAX_ID, 0x00, false, - static_cast(motorCurrentScaling_ * maxCurrent)); + return sendSdoWrite(OD_DSP402_CURRENT_MAX_ID, 0x00, false, static_cast(motorCurrentScaling_ * maxCurrent)); } -bool DriveEthercatDevice::getMaxFreezeCurrent(double & current) +bool DriveEthercatDevice::getMaxFreezeCurrent(double& current) { RecLock lock(mutex_); float currentFloat = 0.0; @@ -1270,26 +1167,24 @@ bool DriveEthercatDevice::setMaxFreezeCurrent(const double current) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting freeze current limit (" << current << ")."); - return sendSdoWrite(OD_CONFIG_MAX_FREEZE_CURRENT, 0x00, false, - static_cast(motorCurrentScaling_ * current)); + return sendSdoWrite(OD_CONFIG_MAX_FREEZE_CURRENT, 0x00, false, static_cast(motorCurrentScaling_ * current)); } bool DriveEthercatDevice::clearLoggedData() { MELO_DEBUG_STREAM("Erasing logged data."); - // Subindex 2 erases the data + // Subindex 2 erases the data return sendSdoWriteUInt64(OD_DATALOGGING, 2, false, (uint64_t)1); } bool DriveEthercatDevice::refreshLoggedData() { MELO_DEBUG_STREAM("Refreshing logged data."); - // Subindex 1 refreshes the data + // Subindex 1 refreshes the data return sendSdoWriteUInt64(OD_DATALOGGING, 1, false, (uint64_t)1); } - -bool DriveEthercatDevice::getMaxMotorVelocity(double & maxMotorVelocity) +bool DriveEthercatDevice::getMaxMotorVelocity(double& maxMotorVelocity) { RecLock lock(mutex_); float maxMotorVelocityFloat = 0.0; @@ -1305,10 +1200,10 @@ bool DriveEthercatDevice::setMaxMotorVelocity(const double maxMotorVelocity) RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting max motor velocity (" << maxMotorVelocity << " rad/s)."); return sendSdoWrite(OD_DSP402_MOTOR_VELOCITY_MAX_ID, 0x00, false, - static_cast(motorVelocityScaling_ * maxMotorVelocity)); + static_cast(motorVelocityScaling_ * maxMotorVelocity)); } -bool DriveEthercatDevice::getMaxJointTorque(double & maxJointTorque) +bool DriveEthercatDevice::getMaxJointTorque(double& maxJointTorque) { RecLock lock(mutex_); float maxJointTorqueFloat = 0.0; @@ -1324,10 +1219,10 @@ bool DriveEthercatDevice::setMaxJointTorque(const double maxJointTorque) MELO_DEBUG_STREAM("Setting max joint torque (" << maxJointTorque << " Nm)."); RecLock lock(mutex_); return sendSdoWrite(OD_DSP402_JOINT_TORQUE_MAX_ID, 0x00, false, - static_cast(jointTorqueScaling_ * maxJointTorque)); + static_cast(jointTorqueScaling_ * maxJointTorque)); } -bool DriveEthercatDevice::getCurrentIntegratorSaturation(double & saturation) +bool DriveEthercatDevice::getCurrentIntegratorSaturation(double& saturation) { RecLock lock(mutex_); float saturationFloat = 0.0; @@ -1342,11 +1237,10 @@ bool DriveEthercatDevice::setCurrentIntegratorSaturation(const double saturation { MELO_DEBUG_STREAM("Setting current integrator saturation (" << saturation << " A)."); RecLock lock(mutex_); - return sendSdoWrite(OD_CONFIG_CURRENT_INTEGRATOR_SATURATION, 0x00, false, - static_cast(saturation)); + return sendSdoWrite(OD_CONFIG_CURRENT_INTEGRATOR_SATURATION, 0x00, false, static_cast(saturation)); } -bool DriveEthercatDevice::getJointTorqueIntegratorSaturation(double & saturation) +bool DriveEthercatDevice::getJointTorqueIntegratorSaturation(double& saturation) { RecLock lock(mutex_); float saturationFloat = 0.0; @@ -1361,11 +1255,10 @@ bool DriveEthercatDevice::setJointTorqueIntegratorSaturation(const double satura { MELO_DEBUG_STREAM("Setting joint torque integrator saturation (" << saturation << " Nm)."); RecLock lock(mutex_); - return sendSdoWrite(OD_CONFIG_JOINT_TORQUE_INTEGRATOR_SATURATION, 0x00, false, - static_cast(saturation)); + return sendSdoWrite(OD_CONFIG_JOINT_TORQUE_INTEGRATOR_SATURATION, 0x00, false, static_cast(saturation)); } -bool DriveEthercatDevice::getDirection(int16_t & direction) +bool DriveEthercatDevice::getDirection(int16_t& direction) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_DIRECTION_ID, 0x00, false, direction); @@ -1378,24 +1271,23 @@ bool DriveEthercatDevice::setDirection(const int16_t direction) return sendSdoWrite(OD_CONTROL_DIRECTION_ID, 0x00, false, direction); } -bool DriveEthercatDevice::getJointPositionLimitsSoft(rsl_drive_sdk::common::Limits & limits) +bool DriveEthercatDevice::getJointPositionLimitsSoft(rsl_drive_sdk::common::Limits& limits) { RecLock lock(mutex_); bool success = true; - success &= sendSdoRead(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, - OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN, false, limits.min()); - success &= sendSdoRead(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, - OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX, false, limits.max()); + success &= sendSdoRead(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN, false, + limits.min()); + success &= sendSdoRead(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX, false, + limits.max()); limits *= jointPositionScalingInv_; return success; } -bool DriveEthercatDevice::setJointPositionLimitsSoft(const rsl_drive_sdk::common::Limits & limits) +bool DriveEthercatDevice::setJointPositionLimitsSoft(const rsl_drive_sdk::common::Limits& limits) { RecLock lock(mutex_); - MELO_INFO_STREAM("Setting soft joint position limits ([" << limits.min() << ", " << - limits.max() << "] rad)."); + MELO_INFO_STREAM("Setting soft joint position limits ([" << limits.min() << ", " << limits.max() << "] rad)."); const auto direction = config_.getDirection(); rsl_drive_sdk::common::Limits adjustedLimits = limits; @@ -1403,56 +1295,50 @@ bool DriveEthercatDevice::setJointPositionLimitsSoft(const rsl_drive_sdk::common adjustedLimits *= direction.value(); } bool success = true; - success &= sendSdoWrite(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, - OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN, false, - static_cast(jointPositionScaling_ * adjustedLimits.min())); - success &= sendSdoWrite(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, - OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX, false, - static_cast(jointPositionScaling_ * adjustedLimits.max())); + success &= sendSdoWrite(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MIN, false, + static_cast(jointPositionScaling_ * adjustedLimits.min())); + success &= sendSdoWrite(OD_DSP402_SOFT_JOINT_POSITION_LIMIT_ID, OD_DSP402_SOFT_JOINT_POSITION_LIMIT_SID_MAX, false, + static_cast(jointPositionScaling_ * adjustedLimits.max())); return success; } -bool DriveEthercatDevice::getJointPositionLimitsHard(rsl_drive_sdk::common::Limits & limits) +bool DriveEthercatDevice::getJointPositionLimitsHard(rsl_drive_sdk::common::Limits& limits) { RecLock lock(mutex_); bool success = true; - success &= sendSdoRead(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, - OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN, false, limits.min()); - success &= sendSdoRead(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, - OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX, false, limits.max()); + success &= sendSdoRead(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN, false, + limits.min()); + success &= sendSdoRead(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX, false, + limits.max()); limits *= jointPositionScalingInv_; return success; } -bool DriveEthercatDevice::setJointPositionLimitsHard(const rsl_drive_sdk::common::Limits & limits) +bool DriveEthercatDevice::setJointPositionLimitsHard(const rsl_drive_sdk::common::Limits& limits) { RecLock lock(mutex_); - MELO_DEBUG_STREAM("Setting hard joint position limits ([" << limits.min() << ", " << - limits.max() << "] rad)."); + MELO_DEBUG_STREAM("Setting hard joint position limits ([" << limits.min() << ", " << limits.max() << "] rad)."); const auto direction = config_.getDirection(); rsl_drive_sdk::common::Limits adjustedLimits = limits; if (direction) { adjustedLimits *= direction.value(); } bool success = true; - success &= sendSdoWrite(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, - OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN, false, - static_cast(jointPositionScaling_ * adjustedLimits.min())); - success &= sendSdoWrite(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, - OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX, false, - static_cast(jointPositionScaling_ * adjustedLimits.max())); + success &= sendSdoWrite(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MIN, false, + static_cast(jointPositionScaling_ * adjustedLimits.min())); + success &= sendSdoWrite(OD_DSP402_HARD_JOINT_POSITION_LIMIT_ID, OD_DSP402_HARD_JOINT_POSITION_LIMIT_SID_MAX, false, + static_cast(jointPositionScaling_ * adjustedLimits.max())); return success; } -bool DriveEthercatDevice::getControlGains( - const rsl_drive_sdk::mode::ModeEnum mode, - rsl_drive_sdk::mode::PidGainsF & pidGains) +bool DriveEthercatDevice::getControlGains(const rsl_drive_sdk::mode::ModeEnum mode, + rsl_drive_sdk::mode::PidGainsF& pidGains) { auto odIndex = modeEnumToOdIndex_.find(mode); if (odIndex == modeEnumToOdIndex_.end()) { - MELO_ERROR_STREAM("Getting control gains is not supported for mode '" << - rsl_drive_sdk::mode::modeEnumToName(mode) << "'."); + MELO_ERROR_STREAM("Getting control gains is not supported for mode '" << rsl_drive_sdk::mode::modeEnumToName(mode) + << "'."); return false; } @@ -1465,31 +1351,29 @@ bool DriveEthercatDevice::getControlGains( return success; } -bool DriveEthercatDevice::setControlGains( - rsl_drive_sdk::mode::ModeEnum mode, - const rsl_drive_sdk::mode::PidGainsF & pidGains) +bool DriveEthercatDevice::setControlGains(rsl_drive_sdk::mode::ModeEnum mode, + const rsl_drive_sdk::mode::PidGainsF& pidGains) { auto odIndex = modeEnumToOdIndex_.find(mode); if (odIndex == modeEnumToOdIndex_.end()) { - MELO_ERROR_STREAM("Setting control gains is not supported for mode '" << - rsl_drive_sdk::mode::modeEnumToName(mode) << "'."); + MELO_ERROR_STREAM("Setting control gains is not supported for mode '" << rsl_drive_sdk::mode::modeEnumToName(mode) + << "'."); return false; } RecLock lock(mutex_); - MELO_DEBUG_STREAM("Setting control gains for mode '" << - rsl_drive_sdk::mode::modeEnumToName(mode) << "'."); + MELO_DEBUG_STREAM("Setting control gains for mode '" << rsl_drive_sdk::mode::modeEnumToName(mode) << "'."); bool success = true; - success &= sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_P, false, - static_cast(gainScaling_ * pidGains.getP())); - success &= sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_I, false, - static_cast(gainScaling_ * pidGains.getI())); - success &= sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_D, false, - static_cast(gainScaling_ * pidGains.getD())); + success &= + sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_P, false, static_cast(gainScaling_ * pidGains.getP())); + success &= + sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_I, false, static_cast(gainScaling_ * pidGains.getI())); + success &= + sendSdoWrite(odIndex->second, OD_GAINS_COMMON_SID_D, false, static_cast(gainScaling_ * pidGains.getD())); return success; } -bool DriveEthercatDevice::getErrorStateBehavior(uint16_t & errorStateBehavior) +bool DriveEthercatDevice::getErrorStateBehavior(uint16_t& errorStateBehavior) { RecLock lock(mutex_); return sendSdoRead(OD_FSM_ERROR_BEHAVIOR_ID, 0x00, false, errorStateBehavior); @@ -1502,12 +1386,11 @@ bool DriveEthercatDevice::setErrorStateBehavior(const uint16_t errorStateBehavio return sendSdoWrite(OD_FSM_ERROR_BEHAVIOR_ID, 0x00, false, errorStateBehavior); } -bool DriveEthercatDevice::getImuEnable(bool & enabled) +bool DriveEthercatDevice::getImuEnable(bool& enabled) { RecLock lock(mutex_); uint32_t enabledInt = 0; - bool isOk = sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, - enabledInt); + bool isOk = sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, enabledInt); enabled = (enabledInt == 1); return isOk; } @@ -1516,41 +1399,36 @@ bool DriveEthercatDevice::setImuEnable(const bool enable) { RecLock lock(mutex_); MELO_DEBUG_STREAM((enable ? "En" : "Dis") << "abling IMU."); - return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, - (uint32_t)(enable ? 1 : 0)); + return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, (uint32_t)(enable ? 1 : 0)); } -bool DriveEthercatDevice::getImuAccelerometerRange(uint32_t & range) +bool DriveEthercatDevice::getImuAccelerometerRange(uint32_t& range) { RecLock lock(mutex_); - return sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE, false, - range); + return sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE, false, range); } bool DriveEthercatDevice::setImuAccelerometerRange(const uint32_t range) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting IMU accelerometer range (" << range << ")."); - return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE, - false, range); + return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_ACCELEROMETER_RANGE, false, range); } -bool DriveEthercatDevice::getImuGyroscopeRange(uint32_t & range) +bool DriveEthercatDevice::getImuGyroscopeRange(uint32_t& range) { RecLock lock(mutex_); - return sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE, false, - range); + return sendSdoRead(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE, false, range); } bool DriveEthercatDevice::setImuGyroscopeRange(const uint32_t range) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting IMU gyroscope range (" << range << ")."); - return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE, false, - range); + return sendSdoWrite(OD_CONTROL_IMU_CONFIG_ID, OD_CONTROL_IMU_CONFIG_SID_GYROSCOPE_RANGE, false, range); } -bool DriveEthercatDevice::getFanMode(uint32_t & mode) +bool DriveEthercatDevice::getFanMode(uint32_t& mode) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_MODE, false, mode); @@ -1563,7 +1441,7 @@ bool DriveEthercatDevice::setFanMode(const uint32_t mode) return sendSdoWrite(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_MODE, false, mode); } -bool DriveEthercatDevice::getFanIntensity(uint32_t & intensity) +bool DriveEthercatDevice::getFanIntensity(uint32_t& intensity) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_INTENSITY, false, intensity); @@ -1576,7 +1454,7 @@ bool DriveEthercatDevice::setFanIntensity(const uint32_t intensity) return sendSdoWrite(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_INTENSITY, false, intensity); } -bool DriveEthercatDevice::getFanLowerTemperature(float & temperature) +bool DriveEthercatDevice::getFanLowerTemperature(float& temperature) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_LOWER_TEMPERATURE, false, temperature); @@ -1589,7 +1467,7 @@ bool DriveEthercatDevice::setFanLowerTemperature(const float temperature) return sendSdoWrite(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_LOWER_TEMPERATURE, false, temperature); } -bool DriveEthercatDevice::getFanUpperTemperature(float & temperature) +bool DriveEthercatDevice::getFanUpperTemperature(float& temperature) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_FAN_ID, OD_CONTROL_FAN_SID_UPPER_TEMPERATURE, false, temperature); @@ -1607,9 +1485,7 @@ bool DriveEthercatDevice::setBrakeMode(const bool mode) RecLock lock(mutex_); if (mode) { MELO_DEBUG_STREAM("Enabling Brake Mode") - if(!sendSdoWrite(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_CTRL, false, - OD_CONTROL_BRAKE_SID_CTRL_VAL_ENABLE)) - { + if (!sendSdoWrite(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_CTRL, false, OD_CONTROL_BRAKE_SID_CTRL_VAL_ENABLE)) { MELO_ERROR_STREAM("Could not enable brake. Fan enabled?"); return false; } else { @@ -1618,9 +1494,7 @@ bool DriveEthercatDevice::setBrakeMode(const bool mode) } } else { MELO_DEBUG_STREAM("Disabling Brake Mode") - if(!sendSdoWrite(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_CTRL, false, - OD_CONTROL_BRAKE_SID_CTRL_VAL_DISABLE)) - { + if (!sendSdoWrite(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_CTRL, false, OD_CONTROL_BRAKE_SID_CTRL_VAL_DISABLE)) { MELO_ERROR_STREAM("Could not disable brake."); return false; } else { @@ -1630,7 +1504,7 @@ bool DriveEthercatDevice::setBrakeMode(const bool mode) } } -bool DriveEthercatDevice::getBrakeMode(bool & mode) +bool DriveEthercatDevice::getBrakeMode(bool& mode) { RecLock lock(mutex_); uint16_t m; @@ -1651,7 +1525,7 @@ bool DriveEthercatDevice::getBrakeMode(bool & mode) bool DriveEthercatDevice::setBrakeDuty(const float d) { - // Expected duty cycle range: 0...1 + // Expected duty cycle range: 0...1 RecLock lock(mutex_); float d_lim; if (d > 1.0f) { @@ -1665,112 +1539,102 @@ bool DriveEthercatDevice::setBrakeDuty(const float d) return sendSdoWrite(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_DUTY, false, d_disc); } -bool DriveEthercatDevice::getBrakeDuty(float & d) +bool DriveEthercatDevice::getBrakeDuty(float& d) { RecLock lock(mutex_); uint16_t d_disc; - if(!sendSdoRead(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_DUTY, false, d_disc)) { + if (!sendSdoRead(OD_CONTROL_BRAKE_ID, OD_CONTROL_BRAKE_SID_DUTY, false, d_disc)) { return false; } d = (float)d_disc / 65535.0f; return true; } -bool DriveEthercatDevice::getGearJointVelocityFilterType(uint32_t & type) +bool DriveEthercatDevice::getGearJointVelocityFilterType(uint32_t& type) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE, false, type); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE, false, type); } bool DriveEthercatDevice::setGearJointVelocityFilterType(const uint32_t type) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity filter type (" << type << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE, false, type); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_FILTER_TYPE, false, type); } -bool DriveEthercatDevice::getGearJointVelocityKfNoiseVariance(float & variance) +bool DriveEthercatDevice::getGearJointVelocityKfNoiseVariance(float& variance) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE, false, variance); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE, false, + variance); } bool DriveEthercatDevice::setGearJointVelocityKfNoiseVariance(const float variance) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity KF noies variance (" << variance << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE, false, variance); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_NOISE_VARIANCE, false, + variance); } -bool DriveEthercatDevice::getGearJointVelocityKfLambda2(float & lambda) +bool DriveEthercatDevice::getGearJointVelocityKfLambda2(float& lambda) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2, false, lambda); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2, false, lambda); } bool DriveEthercatDevice::setGearJointVelocityKfLambda2(const float lambda) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity KF lambda^2 (" << lambda << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, - OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2, false, lambda); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_LAMBDA_2, false, lambda); } -bool DriveEthercatDevice::getGearJointVelocityKfGamma(float & gamma) +bool DriveEthercatDevice::getGearJointVelocityKfGamma(float& gamma) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA, - false, gamma); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA, false, gamma); } bool DriveEthercatDevice::setGearJointVelocityKfGamma(const float gamma) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity KF gamma (" << gamma << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA, - false, gamma); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_KF_GAMMA, false, gamma); } -bool DriveEthercatDevice::getGearJointVelocityIirAlpha(float & alpha) +bool DriveEthercatDevice::getGearJointVelocityIirAlpha(float& alpha) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA, - false, alpha); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA, false, alpha); } bool DriveEthercatDevice::setGearJointVelocityIirAlpha(const float alpha) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity IIR alpha (" << alpha << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA, - false, alpha); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_IIR_ALPHA, false, alpha); } -bool DriveEthercatDevice::getGearJointVelocityEmaAlpha(float & alpha) +bool DriveEthercatDevice::getGearJointVelocityEmaAlpha(float& alpha) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA, - false, alpha); + return sendSdoRead(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA, false, alpha); } bool DriveEthercatDevice::setGearJointVelocityEmaAlpha(const float alpha) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting gear joint velicity IIR alpha (" << alpha << ")."); - return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA, - false, alpha); + return sendSdoWrite(OD_FILTER_GEAR_JOINT_VELOCITY_ID, OD_FILTER_GEAR_JOINT_VELOCITY_SID_EMA_ALPHA, false, alpha); } -bool DriveEthercatDevice::getJointVelocityForAccelerationFilterType(uint32_t & type) +bool DriveEthercatDevice::getJointVelocityForAccelerationFilterType(uint32_t& type) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); } bool DriveEthercatDevice::setJointVelocityForAccelerationFilterType(const uint32_t type) @@ -1778,30 +1642,29 @@ bool DriveEthercatDevice::setJointVelocityForAccelerationFilterType(const uint32 RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint velocity for acceleration filter type (" << type << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); } -bool DriveEthercatDevice::getJointVelocityForAccelerationKfNoiseVariance(float & variance) +bool DriveEthercatDevice::getJointVelocityForAccelerationKfNoiseVariance(float& variance) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); } bool DriveEthercatDevice::setJointVelocityForAccelerationKfNoiseVariance(const float variance) { RecLock lock(mutex_); - MELO_DEBUG_STREAM("Setting joint velocity for acceleration KF noies variance (" << variance << - ")."); + MELO_DEBUG_STREAM("Setting joint velocity for acceleration KF noies variance (" << variance << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); } -bool DriveEthercatDevice::getJointVelocityForAccelerationKfLambda2(float & lambda) +bool DriveEthercatDevice::getJointVelocityForAccelerationKfLambda2(float& lambda) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); } bool DriveEthercatDevice::setJointVelocityForAccelerationKfLambda2(const float lambda) @@ -1809,14 +1672,14 @@ bool DriveEthercatDevice::setJointVelocityForAccelerationKfLambda2(const float l RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint velocity for acceleration KF lambda^2 (" << lambda << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); } -bool DriveEthercatDevice::getJointVelocityForAccelerationKfGamma(float & gamma) +bool DriveEthercatDevice::getJointVelocityForAccelerationKfGamma(float& gamma) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA, false, gamma); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA, false, gamma); } bool DriveEthercatDevice::setJointVelocityForAccelerationKfGamma(const float gamma) @@ -1824,14 +1687,14 @@ bool DriveEthercatDevice::setJointVelocityForAccelerationKfGamma(const float gam RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint velocity for acceleration KF gamma (" << gamma << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA, false, gamma); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_KF_GAMMA, false, gamma); } -bool DriveEthercatDevice::getJointVelocityForAccelerationIirAlpha(float & alpha) +bool DriveEthercatDevice::getJointVelocityForAccelerationIirAlpha(float& alpha) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA, false, alpha); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA, false, alpha); } bool DriveEthercatDevice::setJointVelocityForAccelerationIirAlpha(const float alpha) @@ -1839,15 +1702,14 @@ bool DriveEthercatDevice::setJointVelocityForAccelerationIirAlpha(const float al RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint velocity for acceleration KF gamma (" << alpha << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA, false, alpha); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_IIR_ALPHA, false, alpha); } - -bool DriveEthercatDevice::getJointVelocityForAccelerationEmaAlpha(float & alpha) +bool DriveEthercatDevice::getJointVelocityForAccelerationEmaAlpha(float& alpha) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA, false, alpha); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA, false, alpha); } bool DriveEthercatDevice::setJointVelocityForAccelerationEmaAlpha(const float alpha) @@ -1855,98 +1717,88 @@ bool DriveEthercatDevice::setJointVelocityForAccelerationEmaAlpha(const float al RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint velocity for acceleration IIR alpha (" << alpha << ")."); return sendSdoWrite(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA, false, alpha); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_EMA_ALPHA, false, alpha); } -bool DriveEthercatDevice::getJointAccelerationFilterType(uint32_t & type) +bool DriveEthercatDevice::getJointAccelerationFilterType(uint32_t& type) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_ID, - OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); + OD_FILTER_JOINT_VELOCITY_FOR_ACCELERATION_SID_FILTER_TYPE, false, type); } bool DriveEthercatDevice::setJointAccelerationFilterType(const uint32_t type) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration filter type (" << type << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE, - false, type); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_FILTER_TYPE, false, type); } -bool DriveEthercatDevice::getJointAccelerationKfNoiseVariance(float & variance) +bool DriveEthercatDevice::getJointAccelerationKfNoiseVariance(float& variance) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, - OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); + return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE, false, + variance); } bool DriveEthercatDevice::setJointAccelerationKfNoiseVariance(const float variance) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration KF noies variance (" << variance << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, - OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE, false, variance); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_NOISE_VARIANCE, false, + variance); } -bool DriveEthercatDevice::getJointAccelerationKfLambda2(float & lambda) +bool DriveEthercatDevice::getJointAccelerationKfLambda2(float& lambda) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2, - false, lambda); + return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); } bool DriveEthercatDevice::setJointAccelerationKfLambda2(const float lambda) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration KF lambda^2 (" << lambda << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2, - false, lambda); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_LAMBDA_2, false, lambda); } -bool DriveEthercatDevice::getJointAccelerationKfGamma(float & gamma) +bool DriveEthercatDevice::getJointAccelerationKfGamma(float& gamma) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA, - false, gamma); + return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA, false, gamma); } bool DriveEthercatDevice::setJointAccelerationKfGamma(const float gamma) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration KF gamma (" << gamma << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA, - false, gamma); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_KF_GAMMA, false, gamma); } -bool DriveEthercatDevice::getJointAccelerationIirAlpha(float & alpha) +bool DriveEthercatDevice::getJointAccelerationIirAlpha(float& alpha) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA, - false, alpha); + return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA, false, alpha); } bool DriveEthercatDevice::setJointAccelerationIirAlpha(const float alpha) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration IIR Alpha (" << alpha << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA, - false, alpha); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_IIR_ALPHA, false, alpha); } - -bool DriveEthercatDevice::getJointAccelerationEmaAlpha(float & alpha) +bool DriveEthercatDevice::getJointAccelerationEmaAlpha(float& alpha) { RecLock lock(mutex_); - return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA, - false, alpha); + return sendSdoRead(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA, false, alpha); } bool DriveEthercatDevice::setJointAccelerationEmaAlpha(const float alpha) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting joint acceleration IIR alpha (" << alpha << ")."); - return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA, - false, alpha); + return sendSdoWrite(OD_FILTER_JOINT_ACCELERATION_ID, OD_FILTER_JOINT_ACCELERATION_SID_EMA_ALPHA, false, alpha); } bool DriveEthercatDevice::writeConfiguration() @@ -1954,7 +1806,7 @@ bool DriveEthercatDevice::writeConfiguration() RecLock lock(mutex_); MELO_INFO_STREAM("Writing configuration to flash."); return sendSdoWrite(OD_FLASH_WRITE_CONFIGURATION_ID, 0x00, false, - static_cast(OD_FLASH_WRITE_CONFIGURATION_ID_VAL_RUN)); + static_cast(OD_FLASH_WRITE_CONFIGURATION_ID_VAL_RUN)); } bool DriveEthercatDevice::setDGainFilterCutoffFrequency(const float freq) @@ -1963,13 +1815,13 @@ bool DriveEthercatDevice::setDGainFilterCutoffFrequency(const float freq) return sendSdoWrite(OD_FILTER_D_GAIN, 0, false, freq); } -bool DriveEthercatDevice::getDGainFilterCutoffFrequency(float & freq) +bool DriveEthercatDevice::getDGainFilterCutoffFrequency(float& freq) { RecLock lock(mutex_); return sendSdoRead(OD_FILTER_D_GAIN, 0, false, freq); } -bool DriveEthercatDevice::getStatuswordSdo(rsl_drive_sdk::Statusword & statusword) +bool DriveEthercatDevice::getStatuswordSdo(rsl_drive_sdk::Statusword& statusword) { RecLock lock(mutex_); uint32_t data = 0; @@ -1985,7 +1837,6 @@ rsl_drive_sdk::Statusword DriveEthercatDevice::getStatusword() return statusword_; } - void DriveEthercatDevice::setControlword(const uint16_t controlwordId) { MELO_DEBUG_STREAM("Setting controlword (" << controlwordId << ")."); @@ -2002,31 +1853,28 @@ void DriveEthercatDevice::resetControlword() void DriveEthercatDevice::stageFreeze() { - Command command; command.setModeEnum(mode::ModeEnum::Freeze); setCommand(command); - } -void DriveEthercatDevice::setCommand(const rsl_drive_sdk::Command & command) +void DriveEthercatDevice::setCommand(const rsl_drive_sdk::Command& command) { RecLock lock(commandMutex_); command_ = command; } -void DriveEthercatDevice::getReading(rsl_drive_sdk::ReadingExtended & reading) +void DriveEthercatDevice::getReading(rsl_drive_sdk::ReadingExtended& reading) { RecLock lock(readingMutex_); reading = reading_; } -bool DriveEthercatDevice::getRtdlEnabled(bool & enabled) +bool DriveEthercatDevice::getRtdlEnabled(bool& enabled) { RecLock lock(mutex_); uint16_t value; - bool isOk = sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_ENABLE, false, - value); + bool isOk = sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_ENABLE, false, value); enabled = (value == OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_ENABLE); return isOk; } @@ -2035,9 +1883,9 @@ bool DriveEthercatDevice::setRtdlEnable(const bool enable) { RecLock lock(mutex_); MELO_INFO_STREAM((enable ? "En" : "Dis") << "abling RTDL."); - bool isOk = sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, - (enable ? OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_ENABLE : - OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_DISABLE)); + bool isOk = sendSdoWrite( + OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_IMU_CONFIG_SID_ENABLE, false, + (enable ? OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_ENABLE : OD_CONTROL_RTDL_CONTROL_SID_ENABLE_VAL_DISABLE)); if (isOk) { isRtdlRunning_ = enable; } @@ -2052,21 +1900,19 @@ bool DriveEthercatDevice::setRtdlCommand(const uint16_t command) return false; } MELO_DEBUG_STREAM("Setting RTDL command (" << command << ")."); - return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_COMMAND, false, - command); + return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_COMMAND, false, command); } -bool DriveEthercatDevice::getRtdlStatus(uint16_t & status) +bool DriveEthercatDevice::getRtdlStatus(uint16_t& status) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_STATUS, false, status); } -bool DriveEthercatDevice::getRtdlLoggingFrequency(uint16_t & frequency) +bool DriveEthercatDevice::getRtdlLoggingFrequency(uint16_t& frequency) { RecLock lock(mutex_); - return sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY, - false, frequency); + return sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY, false, frequency); } bool DriveEthercatDevice::setRtdlLoggingFrequency(const uint16_t frequency) @@ -2077,26 +1923,23 @@ bool DriveEthercatDevice::setRtdlLoggingFrequency(const uint16_t frequency) return false; } MELO_DEBUG_STREAM("Setting RTDL logging frequency (" << frequency << ")."); - return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY, - false, frequency); + return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_LOGGING_FREQUENCY, false, frequency); } -bool DriveEthercatDevice::getRtdlStreamingFrequency(uint16_t & frequency) +bool DriveEthercatDevice::getRtdlStreamingFrequency(uint16_t& frequency) { RecLock lock(mutex_); - return sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY, - false, frequency); + return sendSdoRead(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY, false, frequency); } bool DriveEthercatDevice::setRtdlStreamingFrequency(const uint16_t frequency) { RecLock lock(mutex_); MELO_DEBUG_STREAM("Setting RTDL streaming frequency (" << frequency << " Hz)."); - return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY, - false, frequency); + return sendSdoWrite(OD_CONTROL_RTDL_CONTROL_ID, OD_CONTROL_RTDL_CONTROL_SID_STREAMING_FREQUENCY, false, frequency); } -bool DriveEthercatDevice::getRtdlLastTimestamp(uint64_t & timestamp) +bool DriveEthercatDevice::getRtdlLastTimestamp(uint64_t& timestamp) { RecLock lock(mutex_); return sendSdoRead(OD_CONTROL_RTDL_LAST_TIMESTAMP_ID, 0x0, false, timestamp); @@ -2109,16 +1952,14 @@ bool DriveEthercatDevice::configurePdo(const PdoTypeEnum pdoTypeEnum) return false; } - // If PDO setup is already active, return. + // If PDO setup is already active, return. if (pdoTypeEnum == getCurrentPdoTypeEnum()) { return true; } { RecLock lock(mutex_); - if (!sendSdoWrite(OD_SWITCH_PDO_ID, OD_SWITCH_PDO_SID, false, - pdoInfos_.at(pdoTypeEnum).moduleId_)) - { + if (!sendSdoWrite(OD_SWITCH_PDO_ID, OD_SWITCH_PDO_SID, false, pdoInfos_.at(pdoTypeEnum).moduleId_)) { return false; } } @@ -2127,12 +1968,10 @@ bool DriveEthercatDevice::configurePdo(const PdoTypeEnum pdoTypeEnum) return true; } - -bool DriveEthercatDevice::getCalibrationTypeEnum( - rsl_drive_sdk::calibration::CalibrationTypeEnum & calibrationTypeEnum) +bool DriveEthercatDevice::getCalibrationTypeEnum(rsl_drive_sdk::calibration::CalibrationTypeEnum& calibrationTypeEnum) { uint16_t id; - if(getCalibrationTypeId(id)) { + if (getCalibrationTypeId(id)) { calibrationTypeEnum = rsl_drive_sdk::calibration::calibrationTypeIdToEnum(id); return true; } @@ -2140,13 +1979,12 @@ bool DriveEthercatDevice::getCalibrationTypeEnum( } bool DriveEthercatDevice::setCalibrationTypeEnum( - const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum) + const rsl_drive_sdk::calibration::CalibrationTypeEnum calibrationTypeEnum) { - return setCalibrationTypeId(rsl_drive_sdk::calibration::calibrationTypeEnumToId( - calibrationTypeEnum)); + return setCalibrationTypeId(rsl_drive_sdk::calibration::calibrationTypeEnumToId(calibrationTypeEnum)); } -bool DriveEthercatDevice::getCalibrationTypeId(uint16_t & calibrationTypeId) +bool DriveEthercatDevice::getCalibrationTypeId(uint16_t& calibrationTypeId) { RecLock lock(mutex_); return sendSdoRead(OD_CALIB_SELECTION_ID, 0x00, false, calibrationTypeId); @@ -2158,61 +1996,52 @@ bool DriveEthercatDevice::setCalibrationTypeId(const uint16_t calibrationTypeId) return sendSdoWrite(OD_CALIB_SELECTION_ID, 0x00, false, calibrationTypeId); } -bool DriveEthercatDevice::getLockStatus(uint32_t & status) +bool DriveEthercatDevice::getLockStatus(uint32_t& status) { RecLock lock(mutex_); return sendSdoRead(OD_VARIOUS_PASSWORD_ID, 3, false, status); } -bool DriveEthercatDevice::sendPassword(const std::string & password) +bool DriveEthercatDevice::sendPassword(const std::string& password) { RecLock lock(mutex_); - return sendSdoWriteGeneric(std::to_string(OD_VARIOUS_PASSWORD_ID), - "1", "string", password); + return sendSdoWriteGeneric(std::to_string(OD_VARIOUS_PASSWORD_ID), "1", "string", password); } // Add callbacks -void DriveEthercatDevice::addReadingCb(const ReadingCb & readingCb, const int priority) +void DriveEthercatDevice::addReadingCb(const ReadingCb& readingCb, const int priority) { - readingCbs_.insert({priority, readingCb}); + readingCbs_.insert({ priority, readingCb }); } -void DriveEthercatDevice::addErrorCb(const ErrorCb & errorCb, const int priority) +void DriveEthercatDevice::addErrorCb(const ErrorCb& errorCb, const int priority) { - errorCbs_.insert({priority, errorCb}); + errorCbs_.insert({ priority, errorCb }); } -void DriveEthercatDevice::addErrorRecoveredCb( - const ErrorRecoveredCb & errorRecoveredCb, - const int priority) +void DriveEthercatDevice::addErrorRecoveredCb(const ErrorRecoveredCb& errorRecoveredCb, const int priority) { - errorRecoveredCbs_.insert({priority, errorRecoveredCb}); + errorRecoveredCbs_.insert({ priority, errorRecoveredCb }); } -void DriveEthercatDevice::addFatalCb(const FatalCb & fatalCb, const int priority) +void DriveEthercatDevice::addFatalCb(const FatalCb& fatalCb, const int priority) { - fatalCbs_.insert({priority, fatalCb}); + fatalCbs_.insert({ priority, fatalCb }); } -void DriveEthercatDevice::addFatalRecoveredCb( - const FatalRecoveredCb & fatalRecoveredCb, - const int priority) +void DriveEthercatDevice::addFatalRecoveredCb(const FatalRecoveredCb& fatalRecoveredCb, const int priority) { - fatalRecoveredCbs_.insert({priority, fatalRecoveredCb}); + fatalRecoveredCbs_.insert({ priority, fatalRecoveredCb }); } -void DriveEthercatDevice::addDeviceDisconnectedCb( - const DeviceDisconnectedCb & deviceDisconnectedCb, - const int priority) +void DriveEthercatDevice::addDeviceDisconnectedCb(const DeviceDisconnectedCb& deviceDisconnectedCb, const int priority) { - deviceDisconnectedCbs_.insert({priority, deviceDisconnectedCb}); + deviceDisconnectedCbs_.insert({ priority, deviceDisconnectedCb }); } -void DriveEthercatDevice::addDeviceReconnectedCb( - const DeviceReconnectedCb & deviceReconnectedCb, - const int priority) +void DriveEthercatDevice::addDeviceReconnectedCb(const DeviceReconnectedCb& deviceReconnectedCb, const int priority) { - deviceReconnectedCbs_.insert({priority, deviceReconnectedCb}); + deviceReconnectedCbs_.insert({ priority, deviceReconnectedCb }); } // execute the added callbacks void DriveEthercatDevice::errorCb() { - for (const auto & errorCb : errorCbs_) { + for (const auto& errorCb : errorCbs_) { errorCb.second(getName()); } } @@ -2220,7 +2049,7 @@ void DriveEthercatDevice::errorCb() void DriveEthercatDevice::errorRecoveredCb() { clearGoalStateEnum(); - for (const auto & errorRecoveredCb : errorRecoveredCbs_) { + for (const auto& errorRecoveredCb : errorRecoveredCbs_) { errorRecoveredCb.second(getName()); } } @@ -2232,7 +2061,7 @@ bool DriveEthercatDevice::deviceIsInErrorState() void DriveEthercatDevice::fatalCb() { - for (const auto & fatalCb : fatalCbs_) { + for (const auto& fatalCb : fatalCbs_) { fatalCb.second(getName()); } } @@ -2240,7 +2069,7 @@ void DriveEthercatDevice::fatalCb() void DriveEthercatDevice::fatalRecoveredCb() { clearGoalStateEnum(); - for (const auto & fatalRecoveredCb : fatalRecoveredCbs_) { + for (const auto& fatalRecoveredCb : fatalRecoveredCbs_) { fatalRecoveredCb.second(getName()); } } @@ -2254,24 +2083,23 @@ void DriveEthercatDevice::deviceDisconnectedCb() { statuswordRequested_ = false; clearGoalStateEnum(); - for (const auto & deviceDisconnectedCb : deviceDisconnectedCbs_) { + for (const auto& deviceDisconnectedCb : deviceDisconnectedCbs_) { deviceDisconnectedCb.second(getName()); } - // Set statusword and reading accordingly. - // statusword_.resetData(); + // Set statusword and reading accordingly. + // statusword_.resetData(); ReadingExtended reading; - const std::chrono::high_resolution_clock::time_point stamp = - std::chrono::high_resolution_clock::now(); + const std::chrono::high_resolution_clock::time_point stamp = std::chrono::high_resolution_clock::now(); { std::lock_guard lock(readingMutex_); - StateExtended & state = reading_.getState(); + StateExtended& state = reading_.getState(); state.setStamp(stamp); state.setStatusword(statusword_); if (getConfiguration().getSetReadingToNanOnDisconnect()) { static constexpr auto _NAN_ = std::numeric_limits::quiet_NaN(); - // State. + // State. state.setCurrent(_NAN_); state.setGearPosition(_NAN_); state.setGearVelocity(_NAN_); @@ -2280,7 +2108,7 @@ void DriveEthercatDevice::deviceDisconnectedCb() state.setJointAcceleration(_NAN_); state.setJointTorque(_NAN_); - // Extended state. + // Extended state. state.setMotorPosition(_NAN_); state.setMotorVelocity(_NAN_); state.setGearPositionTicks(0); @@ -2298,13 +2126,12 @@ void DriveEthercatDevice::deviceDisconnectedCb() state.setMeasuredVoltagePhaseV(_NAN_); state.setMeasuredCurrentPhaseW(_NAN_); state.setMeasuredVoltagePhaseW(_NAN_); - } reading = reading_; } - // External reading callbacks. - for (const auto & readingCb : readingCbs_) { + // External reading callbacks. + for (const auto& readingCb : readingCbs_) { readingCb.second(getName(), reading); } } @@ -2312,7 +2139,7 @@ void DriveEthercatDevice::deviceDisconnectedCb() void DriveEthercatDevice::deviceReconnectedCb() { setFSMGoalState(getConfiguration().getGoalStateEnumStartup(), false, 5.0, 100.0); - for (const auto & deviceReconnectedCb : deviceReconnectedCbs_) { + for (const auto& deviceReconnectedCb : deviceReconnectedCbs_) { deviceReconnectedCb.second(getName()); } } @@ -2327,63 +2154,65 @@ bool DriveEthercatDevice::isWithinJointPositionLimitsHard() const return statusword_.hasFatalJointPositionLimitsHard(); } -bool DriveEthercatDevice::sendSdoReadStringCustom(const uint16_t index, std::string & value) +bool DriveEthercatDevice::sendSdoReadStringCustom(const uint16_t index, std::string& value) { const uint8_t lenSubindex = 0; const uint8_t dataSubindex = 1; - // Read number of subindices. + // Read number of subindices. uint8_t nSubindex = 0; if (!sendSdoRead(index, lenSubindex, false, nSubindex)) { return false; } - // Read char casted as an uint32 array. + // Read char casted as an uint32 array. const uint8_t nChars = nSubindex * sizeof(uint32_t) / sizeof(char); std::vector arrayChar(nChars + 1, 0); for (uint8_t i = 0; i < nSubindex; i++) { - if (!sendSdoRead(index, dataSubindex + i, false, ((uint32_t *)arrayChar.data())[i])) { //NOTE: This might lead to alignment issues on ARM platforms + if (!sendSdoRead(index, dataSubindex + i, false, + ((uint32_t*)arrayChar.data())[i])) { // NOTE: This might lead to alignment issues on ARM platforms return false; } } arrayChar[nChars] = '\0'; - // Transform char array to string. + // Transform char array to string. value = std::string(arrayChar.begin(), arrayChar.end()); return true; } -bool DriveEthercatDevice::sendSdoWriteStringCustom(const uint16_t index, const std::string & value) +bool DriveEthercatDevice::sendSdoWriteStringCustom(const uint16_t index, const std::string& value) { const uint8_t lenSubindex = 0; const uint8_t dataSubindex = 1; - // Read number of subindices. + // Read number of subindices. uint8_t nSubindex = 0; if (!sendSdoRead(index, lenSubindex, false, nSubindex)) { return false; } - // Check if the string is not too big. + // Check if the string is not too big. const uint8_t nChars = nSubindex * sizeof(uint32_t) / sizeof(char); if (value.size() > nChars) { MELO_ERROR_STREAM("[DriveEthercatDevice::sendSdoWriteStringCustom] (Drive '" - << name_ << "') String is too big (" - << value.size() << " > " << (uint16_t)nChars - << "), cannot be written."); + << name_ << "') String is too big (" << value.size() << " > " << (uint16_t)nChars + << "), cannot be written."); return false; } - // Transform string to char array. + // Transform string to char array. std::vector arrayChar(nChars, 0); for (uint8_t i = 0; i < nChars; i++) { arrayChar[i] = (i < value.size()) ? value[i] : '\0'; } - // Write char casted as an uint32 array. + // Write char casted as an uint32 array. for (uint8_t i = 0; i < nSubindex; i++) { - if (!sendSdoWrite(index, dataSubindex + i, false, ((uint32_t *)arrayChar.data())[i])) { //NOTE: This might lead to alignment issues on ARM platforms + if (!sendSdoWrite(index, dataSubindex + i, false, ((uint32_t*)arrayChar.data())[i])) { // NOTE: This might lead to + // alignment issues on ARM + // platforms return false; } } @@ -2391,5 +2220,4 @@ bool DriveEthercatDevice::sendSdoWriteStringCustom(const uint16_t index, const s return true; } - -} // rsl_drive_sdk_ethercat +} // namespace rsl_drive_sdk diff --git a/src/DriveCalibrator.cpp b/src/DriveCalibrator.cpp index fed0900..d6a49fb 100644 --- a/src/DriveCalibrator.cpp +++ b/src/DriveCalibrator.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rsl_drive_sdk/DriveCalibrator.hpp" @@ -27,25 +29,23 @@ namespace rsl_drive_sdk { - DriveCalibrator::DriveCalibrator(DriveEthercatDevice::SharedPtr drive) - : _drive(drive) - { - } +DriveCalibrator::DriveCalibrator(DriveEthercatDevice::SharedPtr drive) : _drive(drive) +{ +} - bool DriveCalibrator::calibrate( - const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute, - const double gearAndJointEncoderHomingNewJointPosition) - { - // 1. Check if drive is calibrate state - if (_drive->getActiveStateEnum() != fsm::StateEnum::Calibrate) - { - MELO_ERROR_STREAM("Device: " << _drive->getName() << ":" << _drive->getAddress() << " needs to be in calibrate state for calibration, is in: " << _drive->getActiveStateEnum()); - return false; - } - using cme = calibration::CalibrationModeEnum; - switch (calibrationModeEnum) - { +bool DriveCalibrator::calibrate(const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute, + const double gearAndJointEncoderHomingNewJointPosition) +{ + // 1. Check if drive is calibrate state + if (_drive->getActiveStateEnum() != fsm::StateEnum::Calibrate) { + MELO_ERROR_STREAM("Device: " << _drive->getName() << ":" << _drive->getAddress() + << " needs to be in calibrate state for calibration, is in: " + << _drive->getActiveStateEnum()); + return false; + } + using cme = calibration::CalibrationModeEnum; + switch (calibrationModeEnum) { case cme::GearAndJointEncoderHoming: { /* First send new joint position (it will be wrapped in the firmware if outside of [-pi, pi)). @@ -54,8 +54,7 @@ namespace rsl_drive_sdk * Relative: The new joint position will be set as the new zero. */ double newJointPosition = gearAndJointEncoderHomingNewJointPosition; - if (!gearAndJointEncoderHomingAbsolute) - { + if (!gearAndJointEncoderHomingAbsolute) { ReadingExtended reading; _drive->getReading(reading); newJointPosition += reading.getState().getJointPosition(); @@ -77,40 +76,36 @@ namespace rsl_drive_sdk _drive->startCalibration(calibrationModeEnum); bool calib_running = true; size_t runs = 0; - while (calib_running) - { + while (calib_running) { _drive->calibrationIsRunning(calib_running); std::this_thread::sleep_for(std::chrono::milliseconds(100)); runs++; - if (runs > 4000) - { // More than 240s + if (runs > 4000) { // More than 240s return false; } } return true; - } - break; + } break; default: return false; - } } +} - void DriveCalibrator::start_calibration( - const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute , - const double gearAndJointEncoderHomingNewJointPosition ) - { - // 1. Check if drive is calibrate state - if (_drive->getActiveStateEnum() != fsm::StateEnum::Calibrate) - { - MELO_ERROR_STREAM("Device: " << _drive->getName() << ":" << _drive->getAddress() << " needs to be in calibrate state for calibration, is in: " << _drive->getActiveStateEnum()); - throw std::runtime_error("Device is not in calibration mode"); - } +void DriveCalibrator::start_calibration(const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute, + const double gearAndJointEncoderHomingNewJointPosition) +{ + // 1. Check if drive is calibrate state + if (_drive->getActiveStateEnum() != fsm::StateEnum::Calibrate) { + MELO_ERROR_STREAM("Device: " << _drive->getName() << ":" << _drive->getAddress() + << " needs to be in calibrate state for calibration, is in: " + << _drive->getActiveStateEnum()); + throw std::runtime_error("Device is not in calibration mode"); + } - using cme = calibration::CalibrationModeEnum; + using cme = calibration::CalibrationModeEnum; - switch (calibrationModeEnum) - { + switch (calibrationModeEnum) { case cme::GearAndJointEncoderHoming: { /* First send new joint position (it will be wrapped in the firmware if outside of [-pi, pi)). @@ -119,8 +114,7 @@ namespace rsl_drive_sdk * Relative: The new joint position will be set as the new zero. */ double newJointPosition = gearAndJointEncoderHomingNewJointPosition; - if (!gearAndJointEncoderHomingAbsolute) - { + if (!gearAndJointEncoderHomingAbsolute) { ReadingExtended reading; _drive->getReading(reading); newJointPosition += reading.getState().getJointPosition(); @@ -139,16 +133,13 @@ namespace rsl_drive_sdk case cme::MotorEncoderParameters: { MELO_INFO_STREAM("Starting calibration: " << calibrationModeEnum); - if (!_drive->startCalibration(calibrationModeEnum)) - { + if (!_drive->startCalibration(calibrationModeEnum)) { throw std::runtime_error("Error starting calibration"); } - } - break; + } break; default: return; - } - } - } + +} // namespace rsl_drive_sdk diff --git a/src/DriveCollection.cpp b/src/DriveCollection.cpp index 6076088..09cf546 100644 --- a/src/DriveCollection.cpp +++ b/src/DriveCollection.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std @@ -31,18 +33,18 @@ namespace rsl_drive_sdk { -DriveCollection::DriveCollection(const DrivesVector & drives) -: shutdownRequested_(false), - drives_(drives) +DriveCollection::DriveCollection(const DrivesVector& drives) : shutdownRequested_(false), drives_(drives) { #ifndef NDEBUG MELO_WARN_STREAM("CMake Build Type is 'Debug'. Change to 'Release' for better performance."); #endif } -DriveCollection::~DriveCollection() {} +DriveCollection::~DriveCollection() +{ +} -bool DriveCollection::addDrive(const DriveEthercatDevice::SharedPtr & drive) +bool DriveCollection::addDrive(const DriveEthercatDevice::SharedPtr& drive) { const std::string name = drive->getName(); if (driveExists(name)) { @@ -53,9 +55,9 @@ bool DriveCollection::addDrive(const DriveEthercatDevice::SharedPtr & drive) return true; } -bool DriveCollection::driveExists(const std::string & name) const +bool DriveCollection::driveExists(const std::string& name) const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->getName() == name) { return true; } @@ -63,9 +65,9 @@ bool DriveCollection::driveExists(const std::string & name) const return false; } -DriveEthercatDevice::SharedPtr DriveCollection::getDrive(const std::string & name) const +DriveEthercatDevice::SharedPtr DriveCollection::getDrive(const std::string& name) const { - for (auto & rsl_drive_sdk : drives_) { + for (auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->getName() == name) { return rsl_drive_sdk; } @@ -83,13 +85,10 @@ size_t DriveCollection::getNumberOfDrives() const return drives_.size(); } -bool DriveCollection::setGoalStatesEnum( - const fsm::StateEnum goalStateEnum, - const bool reachStates, - const double timeout, - const double checkingFrequency) +bool DriveCollection::setGoalStatesEnum(const fsm::StateEnum goalStateEnum, const bool reachStates, + const double timeout, const double checkingFrequency) { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { rsl_drive_sdk->setFSMGoalState(goalStateEnum, false, 0.0, 100.0); } @@ -104,9 +103,8 @@ bool DriveCollection::setGoalStatesEnum( return false; } bool goalStatesHaveBeenReached = true; - for (const auto & rsl_drive_sdk : drives_) { - goalStatesHaveBeenReached = goalStatesHaveBeenReached && - rsl_drive_sdk->goalStateHasBeenReached(); + for (const auto& rsl_drive_sdk : drives_) { + goalStatesHaveBeenReached = goalStatesHaveBeenReached && rsl_drive_sdk->goalStateHasBeenReached(); } if (goalStatesHaveBeenReached) { return true; @@ -118,14 +116,14 @@ bool DriveCollection::setGoalStatesEnum( void DriveCollection::clearGoalStatesEnum() { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { rsl_drive_sdk->clearGoalStateEnum(); } } bool DriveCollection::allDevicesAreConnected() const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->deviceIsMissing()) { return false; } @@ -139,17 +137,16 @@ bool DriveCollection::allDevicesAreInTheState(const fsm::StateEnum stateEnum) co return false; } - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->getActiveStateEnum() != stateEnum) { return false; } } - return true; } -bool DriveCollection::allDevicesAreInTheSameState(fsm::StateEnum & stateEnum) const +bool DriveCollection::allDevicesAreInTheSameState(fsm::StateEnum& stateEnum) const { if (getNumberOfDrives() == 0) { stateEnum = fsm::StateEnum::NA; @@ -157,7 +154,7 @@ bool DriveCollection::allDevicesAreInTheSameState(fsm::StateEnum & stateEnum) co } bool stateEnumSet = false; - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (!stateEnumSet) { stateEnum = rsl_drive_sdk->getActiveStateEnum(); stateEnumSet = true; @@ -177,7 +174,7 @@ bool DriveCollection::allDevicesAreInTheMode(const mode::ModeEnum modeEnum) cons return false; } - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->getStatusword().getModeEnum() != modeEnum) { return false; } @@ -185,7 +182,7 @@ bool DriveCollection::allDevicesAreInTheMode(const mode::ModeEnum modeEnum) cons return true; } -bool DriveCollection::allDevicesAreInTheSameMode(mode::ModeEnum & modeEnum) const +bool DriveCollection::allDevicesAreInTheSameMode(mode::ModeEnum& modeEnum) const { if (getNumberOfDrives() == 0) { modeEnum = mode::ModeEnum::NA; @@ -193,7 +190,7 @@ bool DriveCollection::allDevicesAreInTheSameMode(mode::ModeEnum & modeEnum) cons } bool modeEnumSet = false; - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (!modeEnumSet) { modeEnum = rsl_drive_sdk->getStatusword().getModeEnum(); modeEnumSet = true; @@ -209,7 +206,7 @@ bool DriveCollection::allDevicesAreInTheSameMode(mode::ModeEnum & modeEnum) cons bool DriveCollection::noDeviceIsInErrorState() const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->deviceIsInErrorState()) { return false; } @@ -220,7 +217,7 @@ bool DriveCollection::noDeviceIsInErrorState() const bool DriveCollection::noDeviceIsInFatalState() const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (rsl_drive_sdk->deviceIsInFatalState()) { return false; } @@ -231,7 +228,7 @@ bool DriveCollection::noDeviceIsInFatalState() const bool DriveCollection::allDevicesAreWithinJointPositionLimitsSoft() const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (!rsl_drive_sdk->isWithinJointPositionLimitsSoft()) { return false; } @@ -241,7 +238,7 @@ bool DriveCollection::allDevicesAreWithinJointPositionLimitsSoft() const bool DriveCollection::allDevicesAreWithinJointPositionLimitsHard() const { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { if (!rsl_drive_sdk->isWithinJointPositionLimitsHard()) { return false; } @@ -249,27 +246,27 @@ bool DriveCollection::allDevicesAreWithinJointPositionLimitsHard() const return true; } -bool DriveCollection::calibrate( - const std::string & deviceName, const calibration::CalibrationModeEnum calibrationModeEnum, - const bool gearAndJointEncoderHomingAbsolute, - const double gearAndJointEncoderHomingNewJointPosition) +bool DriveCollection::calibrate(const std::string& deviceName, + const calibration::CalibrationModeEnum calibrationModeEnum, + const bool gearAndJointEncoderHomingAbsolute, + const double gearAndJointEncoderHomingNewJointPosition) { auto rsl_drive_sdk = getDrive(deviceName); DriveCalibrator calibrator(rsl_drive_sdk); return calibrator.calibrate(calibrationModeEnum, gearAndJointEncoderHomingAbsolute, - gearAndJointEncoderHomingNewJointPosition); + gearAndJointEncoderHomingNewJointPosition); } void DriveCollection::sendControlwords(const uint16_t controlwordId) { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { rsl_drive_sdk->setControlword(controlwordId); } } void DriveCollection::stageDisables() { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { Command command; command.setModeEnum(mode::ModeEnum::Disable); rsl_drive_sdk->setCommand(command); @@ -278,7 +275,7 @@ void DriveCollection::stageDisables() void DriveCollection::stageFreezes() { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { Command command; command.setModeEnum(mode::ModeEnum::Freeze); rsl_drive_sdk->setCommand(command); @@ -287,22 +284,21 @@ void DriveCollection::stageFreezes() void DriveCollection::stageZeroJointTorques() { - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { Command command; command.setModeEnum(mode::ModeEnum::JointTorque); rsl_drive_sdk->setCommand(command); } } -void DriveCollection::setCommands(const std::vector & commands) +void DriveCollection::setCommands(const std::vector& commands) { assert(commands.size() == drives_.size()); unsigned int i = 0; - for (const auto & rsl_drive_sdk : drives_) { + for (const auto& rsl_drive_sdk : drives_) { rsl_drive_sdk->setCommand(commands[i]); i++; } } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/State.cpp b/src/State.cpp index af37c9b..a4657b6 100644 --- a/src/State.cpp +++ b/src/State.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std @@ -28,31 +30,33 @@ // rsl_drive_sdk #include "rsl_drive_sdk/State.hpp" - namespace rsl_drive_sdk { +State::State() +{ +} -State::State() {} - -State::~State() {} +State::~State() +{ +} -const std::chrono::high_resolution_clock::time_point & State::getStamp() const +const std::chrono::high_resolution_clock::time_point& State::getStamp() const { return stamp_; } -void State::setStamp(const std::chrono::high_resolution_clock::time_point & stamp) +void State::setStamp(const std::chrono::high_resolution_clock::time_point& stamp) { stamp_ = stamp; } -const Statusword & State::getStatusword() const +const Statusword& State::getStatusword() const { return statusword_; } -void State::setStatusword(const Statusword & statusword) +void State::setStatusword(const Statusword& statusword) { statusword_ = statusword; } @@ -127,12 +131,12 @@ void State::setJointTorque(const double jointTorque) jointTorque_ = jointTorque; } -const Imu & State::getImu() const +const Imu& State::getImu() const { return imu_; } -void State::setImu(const Imu & imu) +void State::setImu(const Imu& imu) { imu_ = imu; } @@ -140,27 +144,20 @@ void State::setImu(const Imu & imu) bool State::isValid() const { return ( - //!stamp_.isZero() && - !statusword_.isEmpty() && - std::isfinite(current_) && - std::isfinite(gearPosition_) && - std::isfinite(gearVelocity_) && - std::isfinite(jointPosition_) && - std::isfinite(jointVelocity_) && - std::isfinite(jointTorque_) && - //std::isfinite(!imu_.time_.isZero()) && - std::isfinite(imu_.acceleration_x) && - std::isfinite(imu_.acceleration_y)) && - std::isfinite(imu_.acceleration_z) && - std::isfinite(imu_.angle_velocity_x) && - std::isfinite(imu_.angle_velocity_y) && - std::isfinite(imu_.angle_velocity_z); -} - -std::string State::asString(const std::string & prefix) const + //! stamp_.isZero() && + !statusword_.isEmpty() && std::isfinite(current_) && std::isfinite(gearPosition_) && + std::isfinite(gearVelocity_) && std::isfinite(jointPosition_) && std::isfinite(jointVelocity_) && + std::isfinite(jointTorque_) && + // std::isfinite(!imu_.time_.isZero()) && + std::isfinite(imu_.acceleration_x) && std::isfinite(imu_.acceleration_y)) && + std::isfinite(imu_.acceleration_z) && std::isfinite(imu_.angle_velocity_x) && + std::isfinite(imu_.angle_velocity_y) && std::isfinite(imu_.angle_velocity_z); +} + +std::string State::asString(const std::string& prefix) const { std::stringstream ss; - //ss << prefix << "Stamp: " << stamp_ << std::endl; + // ss << prefix << "Stamp: " << stamp_ << std::endl; ss << prefix << "Statusword: " << statusword_ << std::endl; ss << prefix << "Current: " << current_ << std::endl; ss << prefix << "Gear position: " << gearPosition_ << std::endl; @@ -170,17 +167,17 @@ std::string State::asString(const std::string & prefix) const ss << prefix << "Joint acceleration: " << jointAcceleration_ << std::endl; ss << prefix << "Joint torque: " << jointTorque_ << std::endl; ss << prefix << "IMU:" << std::endl; - //ss << prefix << prefix << "Stamp: " << imu_.time_ << std::endl; - //ss << prefix << prefix << "Linear acceleration: " << imu_.linearAcceleration_ << std::endl; - //ss << prefix << prefix << "Angular velocity: " << imu_.angularVelocity_; + // ss << prefix << prefix << "Stamp: " << imu_.time_ << std::endl; + // ss << prefix << prefix << "Linear acceleration: " << imu_.linearAcceleration_ << std::endl; + // ss << prefix << prefix << "Angular velocity: " << imu_.angularVelocity_; return ss.str(); } -std::ostream & operator<<(std::ostream & out, const State & state) +std::ostream& operator<<(std::ostream& out, const State& state) { out << "State:" << std::endl; out << state.asString(" "); return out; } -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/StateExtended.cpp b/src/StateExtended.cpp index 5821609..30c0a44 100644 --- a/src/StateExtended.cpp +++ b/src/StateExtended.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std #include @@ -27,11 +29,9 @@ // rsl_drive_sdk #include "rsl_drive_sdk/StateExtended.hpp" - namespace rsl_drive_sdk { - double StateExtended::getMeasuredApparentPower() const { return measuredApparentPower; @@ -52,10 +52,13 @@ void StateExtended::setMeasuredInputCurrent(double value) measuredInputCurrent = value; } -StateExtended::StateExtended() -: State() {} +StateExtended::StateExtended() : State() +{ +} -StateExtended::~StateExtended() {} +StateExtended::~StateExtended() +{ +} double StateExtended::getMotorPosition() const { @@ -307,11 +310,10 @@ void StateExtended::setCoilTemp3(const double coilTemp) coilTemp3_ = coilTemp; } - -std::string StateExtended::asString(const std::string & prefix) const +std::string StateExtended::asString(const std::string& prefix) const { std::stringstream ss; - ss << prefix << static_cast(*this); + ss << prefix << static_cast(*this); ss << prefix << "Motor position: " << motorPosition_ << std::endl; ss << prefix << "Motor velocity: " << motorVelocity_ << std::endl; ss << prefix << "Gear position ticks: " << gearPositionTicks_ << std::endl; @@ -342,12 +344,11 @@ std::string StateExtended::asString(const std::string & prefix) const return ss.str(); } -std::ostream & operator<<(std::ostream & out, const StateExtended & stateExtended) +std::ostream& operator<<(std::ostream& out, const StateExtended& stateExtended) { out << "State extended:" << std::endl; out << stateExtended.asString(" "); return out; } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/Statusword.cpp b/src/Statusword.cpp index 40ec9f9..0679b49 100644 --- a/src/Statusword.cpp +++ b/src/Statusword.cpp @@ -2,62 +2,68 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/Statusword.hpp" - namespace rsl_drive_sdk { +Statusword::Data::Data() +{ +} -Statusword::Data::Data() {} - -Statusword::Data::Data(const uint32_t data) -: all_(data) {} +Statusword::Data::Data(const uint32_t data) : all_(data) +{ +} -bool Statusword::Data::operator==(const Data & other) +bool Statusword::Data::operator==(const Data& other) { return all_ == other.all_; } -bool Statusword::Data::operator!=(const Data & other) +bool Statusword::Data::operator!=(const Data& other) { return !(*this == other); } +Statusword::Statusword() +{ +} -Statusword::Statusword() {} - -Statusword::Statusword(const Statusword & statusword) -: stamp_(statusword.getStamp()), - data_(statusword.getData()) {} +Statusword::Statusword(const Statusword& statusword) : stamp_(statusword.getStamp()), data_(statusword.getData()) +{ +} Statusword::Statusword(uint32_t data) { setData(data); } -Statusword::~Statusword() {} +Statusword::~Statusword() +{ +} -Statusword & Statusword::operator=(const Statusword & statusword) +Statusword& Statusword::operator=(const Statusword& statusword) { std::lock_guard lock(mutex_); stamp_ = statusword.getStamp(); @@ -97,7 +103,7 @@ uint32_t Statusword::getData() const return data_.all_; } -Statusword::Data & Statusword::getDataField() +Statusword::Data& Statusword::getDataField() { std::lock_guard lock(mutex_); return data_; @@ -129,36 +135,26 @@ void Statusword::setModeEnum(const mode::ModeEnum modeEnum) data_.bits_.modeId_ = mode::modeEnumToId(modeEnum); } -void Statusword::getMessages( - std::vector & infos, - std::vector & warnings, - std::vector & errors, - std::vector & fatals) const +void Statusword::getMessages(std::vector& infos, std::vector& warnings, + std::vector& errors, std::vector& fatals) const { Statusword previousStatusword; return getMessagesDiff(previousStatusword, infos, warnings, errors, fatals); } -void Statusword::getMessagesDiff( - Statusword & previousStatusword, - std::vector & infos, - std::vector & warnings, - std::vector & errors, - std::vector & fatals) const +void Statusword::getMessagesDiff(Statusword& previousStatusword, std::vector& infos, + std::vector& warnings, std::vector& errors, + std::vector& fatals) const { // Warnings. if (!previousStatusword.hasWarningHighTemperatureBridge() && hasWarningHighTemperatureBridge()) { warnings.push_back("Warning: High power stage temperature."); - } else if (previousStatusword.hasWarningHighTemperatureBridge() && - !hasWarningHighTemperatureBridge()) - { + } else if (previousStatusword.hasWarningHighTemperatureBridge() && !hasWarningHighTemperatureBridge()) { infos.push_back("Warning disappeared: High power stage temperature."); } if (!previousStatusword.hasWarningHighTemperatureStator() && hasWarningHighTemperatureStator()) { warnings.push_back("Warning: High electric motor temperature."); - } else if (previousStatusword.hasWarningHighTemperatureStator() && - !hasWarningHighTemperatureStator()) - { + } else if (previousStatusword.hasWarningHighTemperatureStator() && !hasWarningHighTemperatureStator()) { infos.push_back("Warning disappeared: High electric motor temperature."); } if (!previousStatusword.hasWarningHighTemperatureCpu() && hasWarningHighTemperatureCpu()) { @@ -168,9 +164,7 @@ void Statusword::getMessagesDiff( } if (!previousStatusword.hasWarningEncoderOutlierMotor() && hasWarningEncoderOutlierMotor()) { warnings.push_back("Warning: Motor encoder outlier."); - } else if (previousStatusword.hasWarningEncoderOutlierMotor() && - !hasWarningEncoderOutlierMotor()) - { + } else if (previousStatusword.hasWarningEncoderOutlierMotor() && !hasWarningEncoderOutlierMotor()) { infos.push_back("Warning disappeared: Motor encoder outlier."); } if (!previousStatusword.hasWarningEncoderOutlierGear() && hasWarningEncoderOutlierGear()) { @@ -180,16 +174,12 @@ void Statusword::getMessagesDiff( } if (!previousStatusword.hasWarningEncoderOutlierJoint() && hasWarningEncoderOutlierJoint()) { warnings.push_back("Warning: Joint encoder outlier."); - } else if (previousStatusword.hasWarningEncoderOutlierJoint() && - !hasWarningEncoderOutlierJoint()) - { + } else if (previousStatusword.hasWarningEncoderOutlierJoint() && !hasWarningEncoderOutlierJoint()) { infos.push_back("Warning disappeared: Joint encoder outlier."); } if (!previousStatusword.hasWarningIncompleteCalibration() && hasWarningIncompleteCalibration()) { warnings.push_back("Warning: Incomplete calibration."); - } else if (previousStatusword.hasWarningIncompleteCalibration() && - !hasWarningIncompleteCalibration()) - { + } else if (previousStatusword.hasWarningIncompleteCalibration() && !hasWarningIncompleteCalibration()) { infos.push_back("Warning disappeared: Incomplete calibration."); } if (!previousStatusword.hasWarningEncoderCrcGear() && hasWarningEncoderCrcGear()) { @@ -216,9 +206,7 @@ void Statusword::getMessagesDiff( } if (!previousStatusword.hasErrorInvalidElecZeroOffset() && hasErrorInvalidElecZeroOffset()) { errors.push_back("Error: Direct Drive Elec Zero Offset Invalid."); - } else if (previousStatusword.hasErrorInvalidElecZeroOffset() && - !hasErrorInvalidElecZeroOffset()) - { + } else if (previousStatusword.hasErrorInvalidElecZeroOffset() && !hasErrorInvalidElecZeroOffset()) { infos.push_back("Error disappeared: Direct Drive Elec Zero Offset Invalid"); } if (!previousStatusword.hasErrorMaxVelocityExceeded() && hasErrorMaxVelocityExceeded()) { @@ -228,25 +216,19 @@ void Statusword::getMessagesDiff( } if (!previousStatusword.hasErrorJointPositionLimitsSoft() && hasErrorJointPositionLimitsSoft()) { errors.push_back("Error: Reached soft joint position limits."); - } else if (previousStatusword.hasErrorJointPositionLimitsSoft() && - !hasErrorJointPositionLimitsSoft()) - { + } else if (previousStatusword.hasErrorJointPositionLimitsSoft() && !hasErrorJointPositionLimitsSoft()) { infos.push_back("Error disappeared: Reached soft joint position limits."); } // Fatals. if (!previousStatusword.hasFatalOvertemperatureBridge() && hasFatalOvertemperatureBridge()) { fatals.push_back("Fatal: Power stage overtemperature."); - } else if (previousStatusword.hasFatalOvertemperatureBridge() && - !hasFatalOvertemperatureBridge()) - { + } else if (previousStatusword.hasFatalOvertemperatureBridge() && !hasFatalOvertemperatureBridge()) { infos.push_back("Fatal disappeared: Power stage overtemperature."); } if (!previousStatusword.hasFatalOvertemperatureStator() && hasFatalOvertemperatureStator()) { fatals.push_back("Fatal: Electric motor overtemperature."); - } else if (previousStatusword.hasFatalOvertemperatureStator() && - !hasFatalOvertemperatureStator()) - { + } else if (previousStatusword.hasFatalOvertemperatureStator() && !hasFatalOvertemperatureStator()) { infos.push_back("Fatal disappeared: Electric motor overtemperature."); } if (!previousStatusword.hasFatalOvertemperatureCpu() && hasFatalOvertemperatureCpu()) { @@ -256,9 +238,7 @@ void Statusword::getMessagesDiff( } if (!previousStatusword.hasFatalJointPositionLimitsHard() && hasFatalJointPositionLimitsHard()) { fatals.push_back("Fatal: Reached hard joint position limits."); - } else if (previousStatusword.hasFatalJointPositionLimitsHard() && - !hasFatalJointPositionLimitsHard()) - { + } else if (previousStatusword.hasFatalJointPositionLimitsHard() && !hasFatalJointPositionLimitsHard()) { infos.push_back("Fatal disappeared: Reached hard joint position limits."); } if (!previousStatusword.hasFatalMotorEncoder() && hasFatalMotorEncoder()) { @@ -426,7 +406,7 @@ bool Statusword::hasFatalJointMotorEncoder() const return data_.bits_.fatal_joint_motor_encoder; } -std::ostream & operator<<(std::ostream & os, const Statusword & statusword) +std::ostream& operator<<(std::ostream& os, const Statusword& statusword) { for (uint32_t i = 8 * sizeof(uint32_t); i > uint32_t(0); i--) { os << ((statusword.getData() & (uint32_t(1) << (i - uint32_t(1)))) ? "1" : "0"); @@ -434,5 +414,4 @@ std::ostream & operator<<(std::ostream & os, const Statusword & statusword) return os; } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/calibration/CalibrationModeEnum.cpp b/src/calibration/CalibrationModeEnum.cpp index 0769505..97b0964 100644 --- a/src/calibration/CalibrationModeEnum.cpp +++ b/src/calibration/CalibrationModeEnum.cpp @@ -2,35 +2,35 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/CalibrationModeEnum.hpp" #include "rsl_drive_sdk/common/ObjectDictionary.hpp" - namespace rsl_drive_sdk { namespace calibration { - uint16_t calibrationModeEnumToId(const CalibrationModeEnum calibrationModeEnum) { if (calibrationModeEnum == CalibrationModeEnum::FrictionEstimation) { @@ -124,7 +124,7 @@ std::string calibrationModeEnumToName(const CalibrationModeEnum calibrationModeE return RSL_DRIVE_CALIB_MODE_NAME_SHORT_NA; } -CalibrationModeEnum calibrationModeNameToEnum(const std::string & calibrationModeName) +CalibrationModeEnum calibrationModeNameToEnum(const std::string& calibrationModeName) { if (calibrationModeName == RSL_DRIVE_CALIB_MODE_NAME_SHORT_FRICTION_ESTIMATION) { return CalibrationModeEnum::FrictionEstimation; @@ -159,11 +159,10 @@ CalibrationModeEnum calibrationModeNameToEnum(const std::string & calibrationMod return CalibrationModeEnum::NA; } -std::ostream & operator<<(std::ostream & out, const CalibrationModeEnum calibrationModeEnum) +std::ostream& operator<<(std::ostream& out, const CalibrationModeEnum calibrationModeEnum) { return out << calibrationModeEnumToName(calibrationModeEnum); } - -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/CalibrationState.cpp b/src/calibration/CalibrationState.cpp index 8b6ddf1..9f67a52 100644 --- a/src/calibration/CalibrationState.cpp +++ b/src/calibration/CalibrationState.cpp @@ -2,36 +2,35 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/CalibrationState.hpp" - namespace rsl_drive_sdk { namespace calibration { - -bool CalibrationState::getSingleCalibrationState( - const CalibrationModeEnum calibrationModeEnum) const +bool CalibrationState::getSingleCalibrationState(const CalibrationModeEnum calibrationModeEnum) const { switch (calibrationModeEnum) { case CalibrationModeEnum::MotorEncoderOffset: @@ -63,9 +62,7 @@ bool CalibrationState::getSingleCalibrationState( * @param calibrationModeEnum Calibration mode enumerator. * @param isCalibrated The new state of the calibration. */ -void CalibrationState::setSingleCalibrationState( - const CalibrationModeEnum calibrationModeEnum, - const bool isCalibrated) +void CalibrationState::setSingleCalibrationState(const CalibrationModeEnum calibrationModeEnum, const bool isCalibrated) { switch (calibrationModeEnum) { case CalibrationModeEnum::MotorEncoderOffset: @@ -94,6 +91,5 @@ void CalibrationState::setSingleCalibrationState( } } - -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/CalibrationTypeEnum.cpp b/src/calibration/CalibrationTypeEnum.cpp index 4281b77..a20433e 100644 --- a/src/calibration/CalibrationTypeEnum.cpp +++ b/src/calibration/CalibrationTypeEnum.cpp @@ -2,37 +2,39 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/CalibrationTypeEnum.hpp" - namespace rsl_drive_sdk { namespace calibration { - uint16_t calibrationTypeEnumToId(const CalibrationTypeEnum calibrationTypeEnum) { - if (calibrationTypeEnum == CalibrationTypeEnum::Custom) {return OD_CALIB_SELECTION_ID_VAL_CUSTOM;} + if (calibrationTypeEnum == CalibrationTypeEnum::Custom) { + return OD_CALIB_SELECTION_ID_VAL_CUSTOM; + } if (calibrationTypeEnum == CalibrationTypeEnum::Factory) { return OD_CALIB_SELECTION_ID_VAL_FACTORY; } @@ -41,25 +43,36 @@ uint16_t calibrationTypeEnumToId(const CalibrationTypeEnum calibrationTypeEnum) CalibrationTypeEnum calibrationTypeIdToEnum(const uint16_t calibrationTypeId) { - if (calibrationTypeId == OD_CALIB_SELECTION_ID_VAL_CUSTOM) {return CalibrationTypeEnum::Custom;} - if (calibrationTypeId == OD_CALIB_SELECTION_ID_VAL_FACTORY) {return CalibrationTypeEnum::Factory;} + if (calibrationTypeId == OD_CALIB_SELECTION_ID_VAL_CUSTOM) { + return CalibrationTypeEnum::Custom; + } + if (calibrationTypeId == OD_CALIB_SELECTION_ID_VAL_FACTORY) { + return CalibrationTypeEnum::Factory; + } return CalibrationTypeEnum::NA; } std::string calibrationTypeEnumToName(const CalibrationTypeEnum calibrationTypeEnum) { - if (calibrationTypeEnum == CalibrationTypeEnum::Custom) {return "Custom";} - if (calibrationTypeEnum == CalibrationTypeEnum::Factory) {return "Factory";} + if (calibrationTypeEnum == CalibrationTypeEnum::Custom) { + return "Custom"; + } + if (calibrationTypeEnum == CalibrationTypeEnum::Factory) { + return "Factory"; + } return "N/A"; } -CalibrationTypeEnum calibrationTypeNameToEnum(const std::string & calibrationTypeName) +CalibrationTypeEnum calibrationTypeNameToEnum(const std::string& calibrationTypeName) { - if (calibrationTypeName == "Custom") {return CalibrationTypeEnum::Custom;} - if (calibrationTypeName == "Factory") {return CalibrationTypeEnum::Factory;} + if (calibrationTypeName == "Custom") { + return CalibrationTypeEnum::Custom; + } + if (calibrationTypeName == "Factory") { + return CalibrationTypeEnum::Factory; + } return CalibrationTypeEnum::NA; } - -} // calibration -} // rsl_drive_sdk +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/Calibration.cpp b/src/calibration/parameter/Calibration.cpp index e924131..36c3b2a 100644 --- a/src/calibration/parameter/Calibration.cpp +++ b/src/calibration/parameter/Calibration.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/Calibration.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,20 +32,15 @@ namespace calibration namespace parameter { - -bool Calibration::operator==(const Calibration & other) const +bool Calibration::operator==(const Calibration& other) const { - return - motorEncoderOffset_ == other.motorEncoderOffset_ && - motorEncoderParameters_ == other.motorEncoderParameters_ && - gearJointEncoderOffset_ == other.gearJointEncoderOffset_ && - gearAndJointEncoderHoming_ == other.gearAndJointEncoderHoming_ && - imuGyroscopeDcBias_ == other.imuGyroscopeDcBias_ && - springStiffness_ == other.springStiffness_ && - frictionEstimation_ == other.frictionEstimation_; + return motorEncoderOffset_ == other.motorEncoderOffset_ && motorEncoderParameters_ == other.motorEncoderParameters_ && + gearJointEncoderOffset_ == other.gearJointEncoderOffset_ && + gearAndJointEncoderHoming_ == other.gearAndJointEncoderHoming_ && + imuGyroscopeDcBias_ == other.imuGyroscopeDcBias_ && springStiffness_ == other.springStiffness_ && + frictionEstimation_ == other.frictionEstimation_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/FrictionEstimation.cpp b/src/calibration/parameter/FrictionEstimation.cpp index d9c40e0..78b966a 100644 --- a/src/calibration/parameter/FrictionEstimation.cpp +++ b/src/calibration/parameter/FrictionEstimation.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/FrictionEstimation.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,17 +32,13 @@ namespace calibration namespace parameter { - -bool FrictionEstimation::operator==(const FrictionEstimation & other) const +bool FrictionEstimation::operator==(const FrictionEstimation& other) const { - return - breakAwayFriction_ == other.breakAwayFriction_ && - breakAwayFrictionBand_ == other.breakAwayFrictionBand_ && - viscousFrictionCoeffNeg_ == other.viscousFrictionCoeffNeg_ && - viscousFrictionCoeffPos_ == other.viscousFrictionCoeffPos_; + return breakAwayFriction_ == other.breakAwayFriction_ && breakAwayFrictionBand_ == other.breakAwayFrictionBand_ && + viscousFrictionCoeffNeg_ == other.viscousFrictionCoeffNeg_ && + viscousFrictionCoeffPos_ == other.viscousFrictionCoeffPos_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/GearAndJointEncoderHoming.cpp b/src/calibration/parameter/GearAndJointEncoderHoming.cpp index 5a5228c..0bef74c 100644 --- a/src/calibration/parameter/GearAndJointEncoderHoming.cpp +++ b/src/calibration/parameter/GearAndJointEncoderHoming.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/GearAndJointEncoderHoming.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,15 +32,11 @@ namespace calibration namespace parameter { - -bool GearAndJointEncoderHoming::operator==(const GearAndJointEncoderHoming & other) const +bool GearAndJointEncoderHoming::operator==(const GearAndJointEncoderHoming& other) const { - return - gearEncoderRawTicks_ == other.gearEncoderRawTicks_ && - jointEncoderRawTicks_ == other.jointEncoderRawTicks_; + return gearEncoderRawTicks_ == other.gearEncoderRawTicks_ && jointEncoderRawTicks_ == other.jointEncoderRawTicks_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/GearJointEncoderOffset.cpp b/src/calibration/parameter/GearJointEncoderOffset.cpp index 0ae787e..4f738af 100644 --- a/src/calibration/parameter/GearJointEncoderOffset.cpp +++ b/src/calibration/parameter/GearJointEncoderOffset.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/GearJointEncoderOffset.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,18 +32,13 @@ namespace calibration namespace parameter { - -bool GearJointEncoderOffset::operator==(const GearJointEncoderOffset & other) const +bool GearJointEncoderOffset::operator==(const GearJointEncoderOffset& other) const { - return - constant_ == other.constant_ && - sin1Amplitude_ == other.sin1Amplitude_ && - sin1Phaseshift_ == other.sin1Phaseshift_ && - sin2Amplitude_ == other.sin2Amplitude_ && - sin2Phaseshift_ == other.sin2Phaseshift_; + return constant_ == other.constant_ && sin1Amplitude_ == other.sin1Amplitude_ && + sin1Phaseshift_ == other.sin1Phaseshift_ && sin2Amplitude_ == other.sin2Amplitude_ && + sin2Phaseshift_ == other.sin2Phaseshift_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/ImuGyroscopeDcBias.cpp b/src/calibration/parameter/ImuGyroscopeDcBias.cpp index 53eb3ed..c8f380e 100644 --- a/src/calibration/parameter/ImuGyroscopeDcBias.cpp +++ b/src/calibration/parameter/ImuGyroscopeDcBias.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/ImuGyroscopeDcBias.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,16 +32,11 @@ namespace calibration namespace parameter { - -bool ImuGyroscopeDcBias::operator==(const ImuGyroscopeDcBias & other) const +bool ImuGyroscopeDcBias::operator==(const ImuGyroscopeDcBias& other) const { - return - x_ == other.x_ && - y_ == other.y_ && - z_ == other.z_; + return x_ == other.x_ && y_ == other.y_ && z_ == other.z_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/MotorEncoderOffset.cpp b/src/calibration/parameter/MotorEncoderOffset.cpp index 3b0a484..898d5b0 100644 --- a/src/calibration/parameter/MotorEncoderOffset.cpp +++ b/src/calibration/parameter/MotorEncoderOffset.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/MotorEncoderOffset.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,13 +32,11 @@ namespace calibration namespace parameter { - -bool MotorEncoderOffset::operator==(const MotorEncoderOffset & other) const +bool MotorEncoderOffset::operator==(const MotorEncoderOffset& other) const { - return value_ == other.value_; + return value_ == other.value_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/MotorEncoderParameters.cpp b/src/calibration/parameter/MotorEncoderParameters.cpp index 631e52b..7e1bc20 100644 --- a/src/calibration/parameter/MotorEncoderParameters.cpp +++ b/src/calibration/parameter/MotorEncoderParameters.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/MotorEncoderParameters.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,20 +32,12 @@ namespace calibration namespace parameter { - -bool MotorEncoderParameters::operator==(const MotorEncoderParameters & other) const +bool MotorEncoderParameters::operator==(const MotorEncoderParameters& other) const { - return - dGain_ == other.dGain_ && - dOffs_ == other.dOffs_ && - dOffc_ == other.dOffc_ && - dPh_ == other.dPh_ && - aGain_ == other.aGain_ && - aOffs_ == other.aOffs_ && - aOffc_ == other.aOffc_; + return dGain_ == other.dGain_ && dOffs_ == other.dOffs_ && dOffc_ == other.dOffc_ && dPh_ == other.dPh_ && + aGain_ == other.aGain_ && aOffs_ == other.aOffs_ && aOffc_ == other.aOffc_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/calibration/parameter/SpringStiffness.cpp b/src/calibration/parameter/SpringStiffness.cpp index c1949ab..60bbe65 100644 --- a/src/calibration/parameter/SpringStiffness.cpp +++ b/src/calibration/parameter/SpringStiffness.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/calibration/parameter/SpringStiffness.hpp" - namespace rsl_drive_sdk { namespace calibration @@ -31,15 +32,11 @@ namespace calibration namespace parameter { - -bool SpringStiffness::operator==(const SpringStiffness & other) const +bool SpringStiffness::operator==(const SpringStiffness& other) const { - return - neg_ == other.neg_ && - pos_ == other.pos_; + return neg_ == other.neg_ && pos_ == other.pos_; } - -} // parameter -} // calibration -} // rsl_drive_sdk +} // namespace parameter +} // namespace calibration +} // namespace rsl_drive_sdk diff --git a/src/common/BuildInfo.cpp b/src/common/BuildInfo.cpp index 5cc3641..ce2f026 100644 --- a/src/common/BuildInfo.cpp +++ b/src/common/BuildInfo.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rsl_drive_sdk/common/BuildInfo.h" @@ -27,8 +29,6 @@ namespace rsl_drive_sdk namespace common { - } - -} +} // namespace rsl_drive_sdk diff --git a/src/common/DriveInfo.cpp b/src/common/DriveInfo.cpp index 4488ecf..ac34752 100644 --- a/src/common/DriveInfo.cpp +++ b/src/common/DriveInfo.cpp @@ -2,81 +2,83 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/common/DriveInfo.hpp" - namespace rsl_drive_sdk { +DriveInfo::DriveInfo() +{ +} -DriveInfo::DriveInfo() {} - -DriveInfo::DriveInfo( - const std::string & serialNumber, - const std::string & model, - const std::string & name, - const uint16_t id, - const common::Version & bootloaderVersion, - const common::Version & firmwareVersion) -: serialNumber_(serialNumber), - model_(model), - name_(name), - id_(id), - bootloaderVersion_(bootloaderVersion), - firmwareVersion_(firmwareVersion) {} +DriveInfo::DriveInfo(const std::string& serialNumber, const std::string& model, const std::string& name, + const uint16_t id, const common::Version& bootloaderVersion, + const common::Version& firmwareVersion) + : serialNumber_(serialNumber) + , model_(model) + , name_(name) + , id_(id) + , bootloaderVersion_(bootloaderVersion) + , firmwareVersion_(firmwareVersion) +{ +} -DriveInfo::~DriveInfo() {} +DriveInfo::~DriveInfo() +{ +} -std::string & DriveInfo::getSerialNumber() +std::string& DriveInfo::getSerialNumber() { return serialNumber_; } -const std::string & DriveInfo::getSerialNumber() const +const std::string& DriveInfo::getSerialNumber() const { return serialNumber_; } -std::string & DriveInfo::getModel() +std::string& DriveInfo::getModel() { return model_; } -const std::string & DriveInfo::getModel() const +const std::string& DriveInfo::getModel() const { return model_; } -std::string & DriveInfo::getName() +std::string& DriveInfo::getName() { return name_; } -const std::string & DriveInfo::getName() const +const std::string& DriveInfo::getName() const { return name_; } -uint16_t & DriveInfo::getId() +uint16_t& DriveInfo::getId() { return id_; } @@ -86,27 +88,27 @@ uint16_t DriveInfo::getId() const return id_; } -common::Version & DriveInfo::getBootloaderVersion() +common::Version& DriveInfo::getBootloaderVersion() { return bootloaderVersion_; } -const common::Version & DriveInfo::getBootloaderVersion() const +const common::Version& DriveInfo::getBootloaderVersion() const { return bootloaderVersion_; } -common::Version & DriveInfo::getFirmwareVersion() +common::Version& DriveInfo::getFirmwareVersion() { return firmwareVersion_; } -const common::Version & DriveInfo::getFirmwareVersion() const +const common::Version& DriveInfo::getFirmwareVersion() const { return firmwareVersion_; } -std::ostream & operator<<(std::ostream & ostream, const DriveInfo & driveInfo) +std::ostream& operator<<(std::ostream& ostream, const DriveInfo& driveInfo) { ostream << "Serial number: " << driveInfo.getSerialNumber() << std::endl; ostream << "Model: " << driveInfo.getModel() << std::endl; @@ -117,5 +119,4 @@ std::ostream & operator<<(std::ostream & ostream, const DriveInfo & driveInfo) return ostream; } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk diff --git a/src/common/FirmwareInfo.cpp b/src/common/FirmwareInfo.cpp index 5eb1acd..56b33eb 100644 --- a/src/common/FirmwareInfo.cpp +++ b/src/common/FirmwareInfo.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std #include @@ -26,13 +28,12 @@ // rsl_drive_sdk #include "rsl_drive_sdk/common/FirmwareInfo.hpp" - namespace rsl_drive_sdk { namespace common { -std::ostream & operator<<(std::ostream & ostream, const FirmwareInfo & info) +std::ostream& operator<<(std::ostream& ostream, const FirmwareInfo& info) { ostream << "Firmware info version: " << (int)info.infoVersion << std::endl; ostream << "Firmware version: " << info.version << std::endl; @@ -44,5 +45,5 @@ std::ostream & operator<<(std::ostream & ostream, const FirmwareInfo & info) return ostream; } -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/src/common/Limits.cpp b/src/common/Limits.cpp index 305e416..d2def62 100644 --- a/src/common/Limits.cpp +++ b/src/common/Limits.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/common/Limits.hpp" @@ -29,20 +31,20 @@ namespace rsl_drive_sdk namespace common { +Limits::Limits() +{ +} -Limits::Limits() {} - -Limits::Limits(const double min, const double max) -: min_(min), - max_(max) +Limits::Limits(const double min, const double max) : min_(min), max_(max) { if (max < min) { - MELO_WARN_STREAM("The interval is an empty set, since max (" << max << - ") is smaller than min (" << min << ")."); + MELO_WARN_STREAM("The interval is an empty set, since max (" << max << ") is smaller than min (" << min << ")."); } } -Limits::~Limits() {} +Limits::~Limits() +{ +} bool Limits::areInf() const { @@ -54,7 +56,7 @@ double Limits::min() const return min_; } -double & Limits::min() +double& Limits::min() { return min_; } @@ -64,7 +66,7 @@ double Limits::max() const return max_; } -double & Limits::max() +double& Limits::max() { return max_; } @@ -79,7 +81,7 @@ Limits Limits::operator+(const double offset) const return areInf() ? *this : Limits(min_ + offset, max_ + offset); } -Limits & Limits::operator+=(const double offset) +Limits& Limits::operator+=(const double offset) { *this = *this + offset; return *this; @@ -90,7 +92,7 @@ Limits Limits::operator-(const double offset) const return *this + -offset; } -Limits & Limits::operator-=(const double offset) +Limits& Limits::operator-=(const double offset) { *this = *this - offset; return *this; @@ -107,12 +109,11 @@ Limits Limits::operator*(const double scaling) const } } -Limits & Limits::operator*=(const double scaling) +Limits& Limits::operator*=(const double scaling) { *this = *this * scaling; return *this; } - -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/src/common/Version.cpp b/src/common/Version.cpp index c04e740..30d1e24 100644 --- a/src/common/Version.cpp +++ b/src/common/Version.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std #include @@ -26,26 +28,25 @@ // rsl_drive_sdk #include "rsl_drive_sdk/common/Version.hpp" - namespace rsl_drive_sdk { namespace common { +Version::Version() +{ +} -Version::Version() {} - -Version::Version(const int major, const int minor, const int patch) -: major_(major), - minor_(minor), - patch_(patch) {} +Version::Version(const int major, const int minor, const int patch) : major_(major), minor_(minor), patch_(patch) +{ +} -Version::Version(const std::string & string) +Version::Version(const std::string& string) { sscanf(string.c_str(), "%d.%d.%d", &major_, &minor_, &patch_); } -Version & Version::fromString(const std::string & string) +Version& Version::fromString(const std::string& string) { *this = Version(string); return *this; @@ -58,20 +59,17 @@ std::string Version::toString() const return ss.str(); } -bool Version::operator==(const Version & other) const +bool Version::operator==(const Version& other) const { - return - major_ == other.major_ && - minor_ == other.minor_ && - patch_ == other.patch_; + return major_ == other.major_ && minor_ == other.minor_ && patch_ == other.patch_; } -bool Version::operator!=(const Version & other) const +bool Version::operator!=(const Version& other) const { return !(*this == other); } -bool Version::operator<(const Version & other) const +bool Version::operator<(const Version& other) const { if (major_ < other.major_) { return true; @@ -88,27 +86,26 @@ bool Version::operator<(const Version & other) const } } -bool Version::operator<=(const Version & other) const +bool Version::operator<=(const Version& other) const { - return *this == other || *this < other; + return *this == other || *this < other; } -bool Version::operator>(const Version & other) const +bool Version::operator>(const Version& other) const { return !(*this <= other); } -bool Version::operator>=(const Version & other) const +bool Version::operator>=(const Version& other) const { return !(*this < other); } -std::ostream & operator<<(std::ostream & ostream, const Version & version) +std::ostream& operator<<(std::ostream& ostream, const Version& version) { ostream << version.toString(); return ostream; } - -} // common -} // rsl_drive_sdk +} // namespace common +} // namespace rsl_drive_sdk diff --git a/src/configuration/DriveConfiguration.cpp b/src/configuration/DriveConfiguration.cpp index 53d7297..bc9ed95 100644 --- a/src/configuration/DriveConfiguration.cpp +++ b/src/configuration/DriveConfiguration.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/configuration/DriveConfiguration.hpp" @@ -43,19 +45,17 @@ namespace rsl_drive_sdk namespace configuration { - Configuration::Configuration() -: errorStateBehavior_(0), - maxCurrent_(28.0), - maxMotorVelocity_(680.7), - direction_(1), - imuEnable_(true), - imuAccelerometerRange_(1), - imuGyroscopeRange_(1), - fake_(false) -{ - - //std::lock_guard lock(mutex_); + : errorStateBehavior_(0) + , maxCurrent_(28.0) + , maxMotorVelocity_(680.7) + , direction_(1) + , imuEnable_(true) + , imuAccelerometerRange_(1) + , imuGyroscopeRange_(1) + , fake_(false) +{ + // std::lock_guard lock(mutex_); addMode(mode::ModeBasePtr(new mode::ModeCurrent())); addMode(mode::ModeBasePtr(new mode::ModeJointPositionTorque())); addMode(mode::ModeBasePtr(new mode::ModeDisable())); @@ -72,14 +72,13 @@ Configuration::Configuration() addMode(mode::ModeBasePtr(new mode::ModeMotorVelocity())); } -Configuration::Configuration(const Configuration & other) +Configuration::Configuration(const Configuration& other) { *this = other; } -Configuration & Configuration::operator=(const Configuration & other) +Configuration& Configuration::operator=(const Configuration& other) { - maxCommandAge_ = other.getMaxCommandAge(); autoStageLastCommand_ = other.getAutoStageLastCommand(); setReadingToNanOnDisconnect_ = other.getSetReadingToNanOnDisconnect(); @@ -98,9 +97,8 @@ Configuration & Configuration::operator=(const Configuration & other) jointPositionLimitsSoft_ = other.getJointPositionLimitsSoft(); jointPositionLimitsHard_ = other.getJointPositionLimitsHard(); - for (const auto & jointPositionConfiguration : other.getJointPositionConfigurations()) { - addJointPositionConfiguration(jointPositionConfiguration.first, - jointPositionConfiguration.second); + for (const auto& jointPositionConfiguration : other.getJointPositionConfigurations()) { + addJointPositionConfiguration(jointPositionConfiguration.first, jointPositionConfiguration.second); } imuEnable_ = other.getImuEnable(); @@ -112,8 +110,7 @@ Configuration & Configuration::operator=(const Configuration & other) fanLowerTemperature_ = other.getFanLowerTemperature(); fanUpperTemperature_ = other.getFanUpperTemperature(); - - for (const auto & mode : other.getModes()) { + for (const auto& mode : other.getModes()) { if (mode.second->getPidGains()) { getMode(mode.first)->setPidGains(mode.second->getPidGains().value()); } @@ -129,8 +126,7 @@ Configuration & Configuration::operator=(const Configuration & other) gearJointVelocityEmaAlpha_ = other.getGearJointVelocityEmaAlpha(); jointVelocityForAccelerationFilterType_ = other.getJointVelocityForAccelerationFilterType(); - jointVelocityForAccelerationKfNoiseVariance_ = - other.getJointVelocityForAccelerationKfNoiseVariance(); + jointVelocityForAccelerationKfNoiseVariance_ = other.getJointVelocityForAccelerationKfNoiseVariance(); jointVelocityForAccelerationKfLambda2_ = other.getJointVelocityForAccelerationKfLambda2(); jointVelocityForAccelerationKfGamma_ = other.getJointVelocityForAccelerationKfGamma(); jointVelocityForAccelerationEmaAlpha_ = other.getJointVelocityForAccelerationEmaAlpha(); @@ -145,7 +141,6 @@ Configuration & Configuration::operator=(const Configuration & other) return *this; } - void Configuration::setMaxCommandAge(const double maxCommandAge) { if (maxCommandAge < 0) { @@ -288,7 +283,7 @@ std::optional Configuration::getDirection() const return direction_; } -void Configuration::setJointPositionLimitsSdk(const common::Limits & limits) +void Configuration::setJointPositionLimitsSdk(const common::Limits& limits) { std::lock_guard lock(mutex_); jointPositionLimitsSdk_ = limits; @@ -300,7 +295,7 @@ std::optional Configuration::getJointPositionLimitsSdk() const return jointPositionLimitsSdk_; } -void Configuration::setJointPositionLimitsSoft(const common::Limits & limits) +void Configuration::setJointPositionLimitsSoft(const common::Limits& limits) { std::lock_guard lock(mutex_); jointPositionLimitsSoft_ = limits; @@ -312,7 +307,7 @@ std::optional Configuration::getJointPositionLimitsSoft() const return jointPositionLimitsSoft_; } -void Configuration::setJointPositionLimitsHard(const common::Limits & limits) +void Configuration::setJointPositionLimitsHard(const common::Limits& limits) { std::lock_guard lock(mutex_); jointPositionLimitsHard_ = limits; @@ -324,17 +319,15 @@ std::optional Configuration::getJointPositionLimitsHard() const return jointPositionLimitsHard_; } -void Configuration::addJointPositionConfiguration( - const std::string & jointPositionConfigurationName, double jointPositionConfigurationValue) +void Configuration::addJointPositionConfiguration(const std::string& jointPositionConfigurationName, + double jointPositionConfigurationValue) { std::lock_guard lock(mutex_); - jointPositionConfigurations_.insert({jointPositionConfigurationName, - jointPositionConfigurationValue}); + jointPositionConfigurations_.insert({ jointPositionConfigurationName, jointPositionConfigurationValue }); } -bool Configuration::getJointPositionConfigurationValue( - const std::string & jointPositionConfigurationName, - double & jointPositionConfigurationValue) const +bool Configuration::getJointPositionConfigurationValue(const std::string& jointPositionConfigurationName, + double& jointPositionConfigurationValue) const { std::lock_guard lock(mutex_); auto it = jointPositionConfigurations_.find(jointPositionConfigurationName); @@ -632,7 +625,6 @@ mode::ModeBasePtr Configuration::getMode(const mode::ModeEnum modeEnum) const std::lock_guard lock(mutex_); auto it = modes_.find(modeEnum); - if (it == modes_.end()) { MELO_ERROR_STREAM("Mode does not exist: " << modeEnum); return mode::ModeBasePtr(); @@ -710,12 +702,11 @@ bool Configuration::getFake() const return fake_; } -void Configuration::addMode(const mode::ModeBasePtr & mode) +void Configuration::addMode(const mode::ModeBasePtr& mode) { std::lock_guard lock(mutex_); - modes_.insert({mode->getModeEnum(), mode}); + modes_.insert({ mode->getModeEnum(), mode }); } - -} // configuration -} // rsl_drive_sdk +} // namespace configuration +} // namespace rsl_drive_sdk diff --git a/src/configuration/DriveConfigurationParser.cpp b/src/configuration/DriveConfigurationParser.cpp index ca8307b..5fe948f 100644 --- a/src/configuration/DriveConfigurationParser.cpp +++ b/src/configuration/DriveConfigurationParser.cpp @@ -2,141 +2,117 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ #include "rsl_drive_sdk/configuration/DriveConfigurationParser.hpp" - namespace rsl_drive_sdk { -configuration::Configuration DriveConfigurationParser::fromFile(const std::string & path) +configuration::Configuration DriveConfigurationParser::fromFile(const std::string& path) { YAML::Node yamlNode = YAML::LoadFile(path); return fromYamlNode(yamlNode); } -configuration::Configuration DriveConfigurationParser::fromYamlNode(const YAML::Node & yamlNode) +configuration::Configuration DriveConfigurationParser::fromYamlNode(const YAML::Node& yamlNode) { configuration::Configuration config; - if (yamlNode["max_command_age"].IsDefined()) { - config.setMaxCommandAge( - yamlNode["max_command_age"].as()); + config.setMaxCommandAge(yamlNode["max_command_age"].as()); } if (yamlNode["auto_stage_last_command"].IsDefined()) { - config.setAutoStageLastCommand( - yamlNode["auto_stage_last_command"].as()); + config.setAutoStageLastCommand(yamlNode["auto_stage_last_command"].as()); } if (yamlNode["set_reading_to_nan_on_disconnect"].IsDefined()) { - config.setSetReadingToNanOnDisconnect( - yamlNode["set_reading_to_nan_on_disconnect"].as()); + config.setSetReadingToNanOnDisconnect(yamlNode["set_reading_to_nan_on_disconnect"].as()); } if (yamlNode["error_state_behavior"].IsDefined()) { - config.setErrorStateBehavior( - yamlNode["error_state_behavior"].as()); + config.setErrorStateBehavior(yamlNode["error_state_behavior"].as()); } if (yamlNode["max_current"].IsDefined()) { - config.setMaxCurrent( - yamlNode["max_current"].as()); + config.setMaxCurrent(yamlNode["max_current"].as()); } if (yamlNode["max_freeze_current"].IsDefined()) { - config.setMaxFreezeCurrent( - yamlNode["max_freeze_current"].as()); + config.setMaxFreezeCurrent(yamlNode["max_freeze_current"].as()); } if (yamlNode["max_motor_velocity"].IsDefined()) { - config.setMaxMotorVelocity( - yamlNode["max_motor_velocity"].as()); + config.setMaxMotorVelocity(yamlNode["max_motor_velocity"].as()); } if (yamlNode["max_joint_torque"].IsDefined()) { - config.setMaxJointTorque( - yamlNode["max_joint_torque"].as()); + config.setMaxJointTorque(yamlNode["max_joint_torque"].as()); } if (yamlNode["current_integrator_saturation"].IsDefined()) { - config.setCurrentIntegratorSaturation( - yamlNode["current_integrator_saturation"].as()); + config.setCurrentIntegratorSaturation(yamlNode["current_integrator_saturation"].as()); } if (yamlNode["joint_torque_integrator_saturation"].IsDefined()) { - config.setJointTorqueIntegratorSaturation( - yamlNode["joint_torque_integrator_saturation"].as()); + config.setJointTorqueIntegratorSaturation(yamlNode["joint_torque_integrator_saturation"].as()); } if (yamlNode["direction"].IsDefined()) { - config.setDirection( - yamlNode["direction"].as()); + config.setDirection(yamlNode["direction"].as()); } if (yamlNode["joint_position_limits"].IsDefined()) { if (yamlNode["joint_position_limits"]["sdk"].IsDefined()) { - config.setJointPositionLimitsSdk(common::Limits( - yamlNode["joint_position_limits"]["sdk"]["min"].as(), - yamlNode["joint_position_limits"]["sdk"]["max"].as())); + config.setJointPositionLimitsSdk(common::Limits(yamlNode["joint_position_limits"]["sdk"]["min"].as(), + yamlNode["joint_position_limits"]["sdk"]["max"].as())); } if (yamlNode["joint_position_limits"]["soft"].IsDefined()) { - config.setJointPositionLimitsSoft(common::Limits( - yamlNode["joint_position_limits"]["soft"]["min"].as< - double>(), - yamlNode["joint_position_limits"]["soft"]["max"].as())); + config.setJointPositionLimitsSoft(common::Limits(yamlNode["joint_position_limits"]["soft"]["min"].as(), + yamlNode["joint_position_limits"]["soft"]["max"].as())); } if (yamlNode["joint_position_limits"]["hard"].IsDefined()) { - config.setJointPositionLimitsHard(common::Limits( - yamlNode["joint_position_limits"]["hard"]["min"].as< - double>(), - yamlNode["joint_position_limits"]["hard"]["max"].as())); + config.setJointPositionLimitsHard(common::Limits(yamlNode["joint_position_limits"]["hard"]["min"].as(), + yamlNode["joint_position_limits"]["hard"]["max"].as())); } } if (yamlNode["joint_position_configurations"].IsDefined()) { for (unsigned int i = 0; i < yamlNode["joint_position_configurations"].size(); i++) { - config.addJointPositionConfiguration( - yamlNode["joint_position_configurations"][i]["name"].as(), - yamlNode["joint_position_configurations"][i]["value"].as()); + config.addJointPositionConfiguration(yamlNode["joint_position_configurations"][i]["name"].as(), + yamlNode["joint_position_configurations"][i]["value"].as()); } } if (yamlNode["imu"].IsDefined()) { if (yamlNode["imu"]["enable"].IsDefined()) { - config.setImuEnable( - yamlNode["imu"]["enable"].as()); + config.setImuEnable(yamlNode["imu"]["enable"].as()); } if (yamlNode["imu"]["accelerometer_range"].IsDefined()) { - config.setImuAccelerometerRange( - yamlNode["imu"]["accelerometer_range"].as()); + config.setImuAccelerometerRange(yamlNode["imu"]["accelerometer_range"].as()); } if (yamlNode["imu"]["gyroscope_range"].IsDefined()) { - config.setImuGyroscopeRange( - yamlNode["imu"]["gyroscope_range"].as()); + config.setImuGyroscopeRange(yamlNode["imu"]["gyroscope_range"].as()); } } if (yamlNode["fan"].IsDefined()) { if (yamlNode["fan"]["mode"].IsDefined()) { - config.setFanMode( - yamlNode["fan"]["mode"].as()); + config.setFanMode(yamlNode["fan"]["mode"].as()); } if (yamlNode["fan"]["intensity"].IsDefined()) { - config.setFanIntensity( - yamlNode["fan"]["intensity"].as()); + config.setFanIntensity(yamlNode["fan"]["intensity"].as()); } if (yamlNode["fan"]["lower_temperature"].IsDefined()) { - config.setFanLowerTemperature( - yamlNode["fan"]["lower_temperature"].as()); + config.setFanLowerTemperature(yamlNode["fan"]["lower_temperature"].as()); } if (yamlNode["fan"]["upper_temperature"].IsDefined()) { - config.setFanUpperTemperature( - yamlNode["fan"]["upper_temperature"].as()); + config.setFanUpperTemperature(yamlNode["fan"]["upper_temperature"].as()); } } if (yamlNode["modes"].IsDefined()) { @@ -147,95 +123,84 @@ configuration::Configuration DriveConfigurationParser::fromYamlNode(const YAML:: MELO_ERROR_STREAM("Mode name '" << modeName << "' does not exist, cannot set gains."); continue; } - mode->setPidGains(mode::PidGainsF( - yamlNode["modes"][i]["gains"]["p"].as(), - yamlNode["modes"][i]["gains"]["i"].as(), - yamlNode["modes"][i]["gains"]["d"].as())); + mode->setPidGains(mode::PidGainsF(yamlNode["modes"][i]["gains"]["p"].as(), + yamlNode["modes"][i]["gains"]["i"].as(), + yamlNode["modes"][i]["gains"]["d"].as())); } } if (yamlNode["goal_states"].IsDefined()) { if (yamlNode["goal_states"]["startup"].IsDefined()) { - config.setGoalStateEnumStartup(fsm::stateNameToEnum( - yamlNode["goal_states"]["startup"].as())); + config.setGoalStateEnumStartup(fsm::stateNameToEnum(yamlNode["goal_states"]["startup"].as())); } if (yamlNode["goal_states"]["shutdown"].IsDefined()) { - config.setGoalStateEnumShutdown(fsm::stateNameToEnum( - yamlNode["goal_states"]["shutdown"].as())); + config.setGoalStateEnumShutdown(fsm::stateNameToEnum(yamlNode["goal_states"]["shutdown"].as())); } } if (yamlNode["gear_joint_velocity_filter"].IsDefined()) { - if(yamlNode["gear_joint_velocity_filter"]["type"].IsDefined()) { - config.setGearJointVelocityFilterType( - yamlNode["gear_joint_velocity_filter"]["type"].as()); + if (yamlNode["gear_joint_velocity_filter"]["type"].IsDefined()) { + config.setGearJointVelocityFilterType(yamlNode["gear_joint_velocity_filter"]["type"].as()); } - if(yamlNode["gear_joint_velocity_filter"]["kf_noise_variance"].IsDefined()) { + if (yamlNode["gear_joint_velocity_filter"]["kf_noise_variance"].IsDefined()) { config.setGearJointVelocityKfNoiseVariance( yamlNode["gear_joint_velocity_filter"]["kf_noise_variance"].as()); } - if(yamlNode["gear_joint_velocity_filter"]["kf_lambda_2"].IsDefined()) { - config.setGearJointVelocityKfLambda2( - yamlNode["gear_joint_velocity_filter"]["kf_lambda_2"].as()); + if (yamlNode["gear_joint_velocity_filter"]["kf_lambda_2"].IsDefined()) { + config.setGearJointVelocityKfLambda2(yamlNode["gear_joint_velocity_filter"]["kf_lambda_2"].as()); } - if(yamlNode["gear_joint_velocity_filter"]["kf_gamma"].IsDefined()) { - config.setGearJointVelocityKfGamma( - yamlNode["gear_joint_velocity_filter"]["kf_gamma"].as()); + if (yamlNode["gear_joint_velocity_filter"]["kf_gamma"].IsDefined()) { + config.setGearJointVelocityKfGamma(yamlNode["gear_joint_velocity_filter"]["kf_gamma"].as()); } - if(yamlNode["gear_joint_velocity_filter"]["ema_alpha"].IsDefined()) { - config.setGearJointVelocityEmaAlpha( - yamlNode["gear_joint_velocity_filter"]["ema_alpha"].as()); + if (yamlNode["gear_joint_velocity_filter"]["ema_alpha"].IsDefined()) { + config.setGearJointVelocityEmaAlpha(yamlNode["gear_joint_velocity_filter"]["ema_alpha"].as()); } } if (yamlNode["joint_velocity_filter_for_acceleration"].IsDefined()) { - if(yamlNode["joint_velocity_filter_for_acceleration"]["type"].IsDefined()) { + if (yamlNode["joint_velocity_filter_for_acceleration"]["type"].IsDefined()) { config.setJointVelocityForAccelerationFilterType( yamlNode["joint_velocity_filter_for_acceleration"]["type"].as()); } - if(yamlNode["joint_velocity_filter_for_acceleration"]["kf_noise_variance"].IsDefined()) { + if (yamlNode["joint_velocity_filter_for_acceleration"]["kf_noise_variance"].IsDefined()) { config.setJointVelocityForAccelerationKfNoiseVariance( yamlNode["joint_velocity_filter_for_acceleration"]["kf_noise_variance"].as()); } - if(yamlNode["joint_velocity_filter_for_acceleration"]["kf_lambda_2"].IsDefined()) { + if (yamlNode["joint_velocity_filter_for_acceleration"]["kf_lambda_2"].IsDefined()) { config.setJointVelocityForAccelerationKfLambda2( yamlNode["joint_velocity_filter_for_acceleration"]["kf_lambda_2"].as()); } - if(yamlNode["joint_velocity_filter_for_acceleration"]["kf_gamma"].IsDefined()) { + if (yamlNode["joint_velocity_filter_for_acceleration"]["kf_gamma"].IsDefined()) { config.setJointVelocityForAccelerationKfGamma( yamlNode["joint_velocity_filter_for_acceleration"]["kf_gamma"].as()); } - if(yamlNode["joint_velocity_filter_for_acceleration"]["ema_alpha"].IsDefined()) { + if (yamlNode["joint_velocity_filter_for_acceleration"]["ema_alpha"].IsDefined()) { config.setJointVelocityForAccelerationEmaAlpha( yamlNode["joint_velocity_filter_for_acceleration"]["ema_alpha"].as()); } } if (yamlNode["joint_acceleration_filter"].IsDefined()) { - if(yamlNode["joint_acceleration_filter"]["type"].IsDefined()) { - config.setJointAccelerationFilterType( - yamlNode["joint_acceleration_filter"]["type"].as()); + if (yamlNode["joint_acceleration_filter"]["type"].IsDefined()) { + config.setJointAccelerationFilterType(yamlNode["joint_acceleration_filter"]["type"].as()); } - if(yamlNode["joint_acceleration_filter"]["kf_noise_variance"].IsDefined()) { + if (yamlNode["joint_acceleration_filter"]["kf_noise_variance"].IsDefined()) { config.setJointAccelerationKfNoiseVariance( yamlNode["joint_acceleration_filter"]["kf_noise_variance"].as()); } - if(yamlNode["joint_acceleration_filter"]["kf_lambda_2"].IsDefined()) { - config.setJointAccelerationKfLambda2( - yamlNode["joint_acceleration_filter"]["kf_lambda_2"].as()); + if (yamlNode["joint_acceleration_filter"]["kf_lambda_2"].IsDefined()) { + config.setJointAccelerationKfLambda2(yamlNode["joint_acceleration_filter"]["kf_lambda_2"].as()); } - if(yamlNode["joint_acceleration_filter"]["kf_gamma"].IsDefined()) { - config.setJointAccelerationKfGamma( - yamlNode["joint_acceleration_filter"]["kf_gamma"].as()); + if (yamlNode["joint_acceleration_filter"]["kf_gamma"].IsDefined()) { + config.setJointAccelerationKfGamma(yamlNode["joint_acceleration_filter"]["kf_gamma"].as()); } - if(yamlNode["joint_acceleration_filter"]["ema_alpha"].IsDefined()) { - config.setJointAccelerationEmaAlpha( - yamlNode["joint_acceleration_filter"]["ema_alpha"].as()); + if (yamlNode["joint_acceleration_filter"]["ema_alpha"].IsDefined()) { + config.setJointAccelerationEmaAlpha(yamlNode["joint_acceleration_filter"]["ema_alpha"].as()); } } if (yamlNode["fake"].IsDefined()) { config.setFake(yamlNode["fake"].as()); } - if(yamlNode["dgainFilterCutoffFrequency"].IsDefined()) { + if (yamlNode["dgainFilterCutoffFrequency"].IsDefined()) { config.setDGainFilterCutoffFrequency(yamlNode["dgainFilterCutoffFrequency"].as()); } return config; } -} +} // namespace rsl_drive_sdk diff --git a/src/fsm/Controlword.cpp b/src/fsm/Controlword.cpp index 128fd34..74aedda 100644 --- a/src/fsm/Controlword.cpp +++ b/src/fsm/Controlword.cpp @@ -2,28 +2,29 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/Controlword.hpp" - namespace rsl_drive_sdk { namespace fsm @@ -70,7 +71,7 @@ std::string controlwordIdToString(const uint16_t controlwordId) return RSL_DRIVE_CW_NAME_NA; } -uint16_t controlwordStringToId(const std::string & controlwordString) +uint16_t controlwordStringToId(const std::string& controlwordString) { if (controlwordString == RSL_DRIVE_CW_NAME_CALIBRATE_TO_CONFIGURE) { return RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE; @@ -111,5 +112,5 @@ uint16_t controlwordStringToId(const std::string & controlwordString) return RSL_DRIVE_CW_ID_NA; } -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateBase.cpp b/src/fsm/StateBase.cpp index 2ee6ec7..79b7f79 100644 --- a/src/fsm/StateBase.cpp +++ b/src/fsm/StateBase.cpp @@ -2,47 +2,48 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateBase.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateBase::StateBase(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum, + const StateEnum stateEnum, const std::map& goalStateEnumToControlword) + : rsl_drive_sdk_(rsl_drive_sdk) + , goalStateEnum_(goalStateEnum) + , stateEnum_(stateEnum) + , name_(stateEnumToName(stateEnum)) + , goalStateEnumToControlword_(goalStateEnumToControlword) +{ +} -StateBase::StateBase( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum, - const StateEnum stateEnum, - const std::map & goalStateEnumToControlword) -: rsl_drive_sdk_(rsl_drive_sdk), - goalStateEnum_(goalStateEnum), - stateEnum_(stateEnum), - name_(stateEnumToName(stateEnum)), - goalStateEnumToControlword_(goalStateEnumToControlword) {} - -StateBase::~StateBase() {} +StateBase::~StateBase() +{ +} StateEnum StateBase::getStateEnum() const { @@ -74,8 +75,8 @@ std::string StateBase::getName() void StateBase::enterBase() { - MELO_INFO_STREAM("[" << rsl_drive_sdk_.getName() << "]: " << "Entered FSM state '" << name_ << - "'."); + MELO_INFO_STREAM("[" << rsl_drive_sdk_.getName() << "]: " + << "Entered FSM state '" << name_ << "'."); enteredCounter_++; controlwordSentForState_ = StateEnum::NA; @@ -98,7 +99,6 @@ void StateBase::updateBase() } if (controlwordSentForState_ == goalStateEnum_) { return; - } auto it = goalStateEnumToControlword_.find(goalStateEnum_); if (it == goalStateEnumToControlword_.end()) { @@ -106,7 +106,7 @@ void StateBase::updateBase() } rsl_drive_sdk_.setControlword(it->second); - //rsl_drive_sdk_.sendControlword(it->second); + // rsl_drive_sdk_.sendControlword(it->second); controlwordSentForState_ = goalStateEnum_; } @@ -118,6 +118,5 @@ void StateBase::leaveBase() MELO_DEBUG_STREAM("Left FSM state '" << name_ << "'."); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateCalibrate.cpp b/src/fsm/StateCalibrate.cpp index 8969005..9afcd61 100644 --- a/src/fsm/StateCalibrate.cpp +++ b/src/fsm/StateCalibrate.cpp @@ -2,46 +2,47 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateCalibrate.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateCalibrate::StateCalibrate(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Calibrate, + { { StateEnum::Configure, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE }, + { StateEnum::Standby, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE }, + { StateEnum::MotorOp, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE }, + { StateEnum::ControlOp, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE } }) +{ +} -StateCalibrate::StateCalibrate( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Calibrate, - {{StateEnum::Configure, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE}, - {StateEnum::Standby, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE}, - {StateEnum::MotorOp, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE}, - {StateEnum::ControlOp, RSL_DRIVE_CW_ID_CALIBRATE_TO_CONFIGURE}}) {} - -StateCalibrate::~StateCalibrate() {} - +StateCalibrate::~StateCalibrate() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateColdStart.cpp b/src/fsm/StateColdStart.cpp index 707c44e..6a796f0 100644 --- a/src/fsm/StateColdStart.cpp +++ b/src/fsm/StateColdStart.cpp @@ -2,42 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateColdStart.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateColdStart::StateColdStart(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::ColdStart) +{ +} -StateColdStart::StateColdStart( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::ColdStart) {} - -StateColdStart::~StateColdStart() {} - +StateColdStart::~StateColdStart() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateConfigure.cpp b/src/fsm/StateConfigure.cpp index 4ec4434..77025a2 100644 --- a/src/fsm/StateConfigure.cpp +++ b/src/fsm/StateConfigure.cpp @@ -2,45 +2,47 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateConfigure.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateConfigure::StateConfigure(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Configure, + { { StateEnum::Calibrate, RSL_DRIVE_CW_ID_CONFIGURE_TO_CALIBRATE }, + { StateEnum::Standby, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY }, + { StateEnum::MotorOp, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY }, + { StateEnum::ControlOp, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY } }) +{ +} -StateConfigure::StateConfigure( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Configure, - {{StateEnum::Calibrate, RSL_DRIVE_CW_ID_CONFIGURE_TO_CALIBRATE}, - {StateEnum::Standby, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY}, - {StateEnum::MotorOp, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY}, - {StateEnum::ControlOp, RSL_DRIVE_CW_ID_CONFIGURE_TO_STANDBY}}) {} - -StateConfigure::~StateConfigure() {} +StateConfigure::~StateConfigure() +{ +} void StateConfigure::enterDerived() { @@ -55,8 +57,7 @@ void StateConfigure::updateDerived() if (step_ == 0) { if (rsl_drive_sdk_.getConfiguration().getErrorStateBehavior()) { - rsl_drive_sdk_.setErrorStateBehavior( - rsl_drive_sdk_.getConfiguration().getErrorStateBehavior().value()); + rsl_drive_sdk_.setErrorStateBehavior(rsl_drive_sdk_.getConfiguration().getErrorStateBehavior().value()); } step_++; return; @@ -64,8 +65,7 @@ void StateConfigure::updateDerived() if (step_ == 1) { if (rsl_drive_sdk_.getConfiguration().getMaxCurrent()) { - rsl_drive_sdk_.setMaxCurrent( - rsl_drive_sdk_.getConfiguration().getMaxCurrent().value()); + rsl_drive_sdk_.setMaxCurrent(rsl_drive_sdk_.getConfiguration().getMaxCurrent().value()); } step_++; return; @@ -73,8 +73,7 @@ void StateConfigure::updateDerived() if (step_ == 2) { if (rsl_drive_sdk_.getConfiguration().getMaxMotorVelocity()) { - rsl_drive_sdk_.setMaxMotorVelocity( - rsl_drive_sdk_.getConfiguration().getMaxMotorVelocity().value()); + rsl_drive_sdk_.setMaxMotorVelocity(rsl_drive_sdk_.getConfiguration().getMaxMotorVelocity().value()); } step_++; return; @@ -82,8 +81,7 @@ void StateConfigure::updateDerived() if (step_ == 3) { if (rsl_drive_sdk_.getConfiguration().getDirection()) { - rsl_drive_sdk_.setDirection( - rsl_drive_sdk_.getConfiguration().getDirection().value()); + rsl_drive_sdk_.setDirection(rsl_drive_sdk_.getConfiguration().getDirection().value()); } step_++; return; @@ -91,8 +89,7 @@ void StateConfigure::updateDerived() if (step_ == 4) { if (rsl_drive_sdk_.getConfiguration().getJointPositionLimitsSoft()) { - rsl_drive_sdk_.setJointPositionLimitsSoft( - rsl_drive_sdk_.getConfiguration().getJointPositionLimitsSoft().value()); + rsl_drive_sdk_.setJointPositionLimitsSoft(rsl_drive_sdk_.getConfiguration().getJointPositionLimitsSoft().value()); } step_++; return; @@ -100,8 +97,7 @@ void StateConfigure::updateDerived() if (step_ == 5) { if (rsl_drive_sdk_.getConfiguration().getJointPositionLimitsHard()) { - rsl_drive_sdk_.setJointPositionLimitsHard( - rsl_drive_sdk_.getConfiguration().getJointPositionLimitsHard().value()); + rsl_drive_sdk_.setJointPositionLimitsHard(rsl_drive_sdk_.getConfiguration().getJointPositionLimitsHard().value()); } step_++; return; @@ -117,8 +113,7 @@ void StateConfigure::updateDerived() if (step_ == 7) { if (rsl_drive_sdk_.getConfiguration().getImuAccelerometerRange()) { - rsl_drive_sdk_.setImuAccelerometerRange( - rsl_drive_sdk_.getConfiguration().getImuAccelerometerRange().value()); + rsl_drive_sdk_.setImuAccelerometerRange(rsl_drive_sdk_.getConfiguration().getImuAccelerometerRange().value()); } step_++; return; @@ -126,8 +121,7 @@ void StateConfigure::updateDerived() if (step_ == 8) { if (rsl_drive_sdk_.getConfiguration().getImuGyroscopeRange()) { - rsl_drive_sdk_.setImuGyroscopeRange( - rsl_drive_sdk_.getConfiguration().getImuGyroscopeRange().value()); + rsl_drive_sdk_.setImuGyroscopeRange(rsl_drive_sdk_.getConfiguration().getImuGyroscopeRange().value()); } step_++; return; @@ -135,8 +129,7 @@ void StateConfigure::updateDerived() if (step_ == 9) { if (rsl_drive_sdk_.getConfiguration().getMaxJointTorque()) { - rsl_drive_sdk_.setMaxJointTorque( - rsl_drive_sdk_.getConfiguration().getMaxJointTorque().value()); + rsl_drive_sdk_.setMaxJointTorque(rsl_drive_sdk_.getConfiguration().getMaxJointTorque().value()); } step_++; return; @@ -162,8 +155,7 @@ void StateConfigure::updateDerived() if (step_ == 12) { if (rsl_drive_sdk_.getConfiguration().getFanMode()) { - rsl_drive_sdk_.setFanMode( - rsl_drive_sdk_.getConfiguration().getFanMode().value()); + rsl_drive_sdk_.setFanMode(rsl_drive_sdk_.getConfiguration().getFanMode().value()); } step_++; return; @@ -171,8 +163,7 @@ void StateConfigure::updateDerived() if (step_ == 13) { if (rsl_drive_sdk_.getConfiguration().getFanIntensity()) { - rsl_drive_sdk_.setFanIntensity( - rsl_drive_sdk_.getConfiguration().getFanIntensity().value()); + rsl_drive_sdk_.setFanIntensity(rsl_drive_sdk_.getConfiguration().getFanIntensity().value()); } step_++; return; @@ -180,8 +171,7 @@ void StateConfigure::updateDerived() if (step_ == 14) { if (rsl_drive_sdk_.getConfiguration().getFanLowerTemperature()) { - rsl_drive_sdk_.setFanLowerTemperature( - rsl_drive_sdk_.getConfiguration().getFanLowerTemperature().value()); + rsl_drive_sdk_.setFanLowerTemperature(rsl_drive_sdk_.getConfiguration().getFanLowerTemperature().value()); } step_++; return; @@ -189,8 +179,7 @@ void StateConfigure::updateDerived() if (step_ == 15) { if (rsl_drive_sdk_.getConfiguration().getFanUpperTemperature()) { - rsl_drive_sdk_.setFanUpperTemperature( - rsl_drive_sdk_.getConfiguration().getFanUpperTemperature().value()); + rsl_drive_sdk_.setFanUpperTemperature(rsl_drive_sdk_.getConfiguration().getFanUpperTemperature().value()); } step_++; return; @@ -198,8 +187,7 @@ void StateConfigure::updateDerived() if (step_ == 16) { if (rsl_drive_sdk_.getConfiguration().getMaxFreezeCurrent()) { - rsl_drive_sdk_.setMaxFreezeCurrent( - rsl_drive_sdk_.getConfiguration().getMaxFreezeCurrent().value()); + rsl_drive_sdk_.setMaxFreezeCurrent(rsl_drive_sdk_.getConfiguration().getMaxFreezeCurrent().value()); } step_++; step_mode_ = rsl_drive_sdk_.getConfiguration().getModes().begin(); @@ -207,14 +195,12 @@ void StateConfigure::updateDerived() } if (step_ == 17) { - const auto & modes = rsl_drive_sdk_.getConfiguration().getModes(); + const auto& modes = rsl_drive_sdk_.getConfiguration().getModes(); if (step_mode_ != modes.end()) { - - // std::advance(modeIt, stepMode_); + // std::advance(modeIt, stepMode_); if (step_mode_->second->getPidGains()) { - rsl_drive_sdk_.setControlGains(step_mode_->first, - step_mode_->second->getPidGains().value()); + rsl_drive_sdk_.setControlGains(step_mode_->first, step_mode_->second->getPidGains().value()); } step_mode_++; return; @@ -226,7 +212,7 @@ void StateConfigure::updateDerived() if (step_ == 18) { if (rsl_drive_sdk_.getConfiguration().getGearJointVelocityFilterType()) { rsl_drive_sdk_.setGearJointVelocityFilterType( - rsl_drive_sdk_.getConfiguration().getGearJointVelocityFilterType().value()); + rsl_drive_sdk_.getConfiguration().getGearJointVelocityFilterType().value()); } step_++; return; @@ -235,7 +221,7 @@ void StateConfigure::updateDerived() if (step_ == 19) { if (rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfNoiseVariance()) { rsl_drive_sdk_.setGearJointVelocityKfNoiseVariance( - rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfNoiseVariance().value()); + rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfNoiseVariance().value()); } step_++; return; @@ -244,7 +230,7 @@ void StateConfigure::updateDerived() if (step_ == 20) { if (rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfLambda2()) { rsl_drive_sdk_.setGearJointVelocityKfLambda2( - rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfLambda2().value()); + rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfLambda2().value()); } step_++; return; @@ -253,7 +239,7 @@ void StateConfigure::updateDerived() if (step_ == 21) { if (rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfGamma()) { rsl_drive_sdk_.setGearJointVelocityKfGamma( - rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfGamma().value()); + rsl_drive_sdk_.getConfiguration().getGearJointVelocityKfGamma().value()); } step_++; return; @@ -262,7 +248,7 @@ void StateConfigure::updateDerived() if (step_ == 22) { if (rsl_drive_sdk_.getConfiguration().getGearJointVelocityEmaAlpha()) { rsl_drive_sdk_.setGearJointVelocityEmaAlpha( - rsl_drive_sdk_.getConfiguration().getGearJointVelocityEmaAlpha().value()); + rsl_drive_sdk_.getConfiguration().getGearJointVelocityEmaAlpha().value()); } step_++; return; @@ -271,7 +257,7 @@ void StateConfigure::updateDerived() if (step_ == 23) { if (rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationFilterType()) { rsl_drive_sdk_.setJointVelocityForAccelerationFilterType( - rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationFilterType().value()); + rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationFilterType().value()); } step_++; return; @@ -280,7 +266,7 @@ void StateConfigure::updateDerived() if (step_ == 24) { if (rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfNoiseVariance()) { rsl_drive_sdk_.setJointVelocityForAccelerationKfNoiseVariance( - rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfNoiseVariance().value()); + rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfNoiseVariance().value()); } step_++; return; @@ -289,7 +275,7 @@ void StateConfigure::updateDerived() if (step_ == 25) { if (rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfLambda2()) { rsl_drive_sdk_.setJointVelocityForAccelerationKfLambda2( - rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfLambda2().value()); + rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfLambda2().value()); } step_++; return; @@ -298,7 +284,7 @@ void StateConfigure::updateDerived() if (step_ == 26) { if (rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfGamma()) { rsl_drive_sdk_.setJointVelocityForAccelerationKfGamma( - rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfGamma().value()); + rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationKfGamma().value()); } step_++; return; @@ -307,7 +293,7 @@ void StateConfigure::updateDerived() if (step_ == 27) { if (rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationEmaAlpha()) { rsl_drive_sdk_.setJointVelocityForAccelerationEmaAlpha( - rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationEmaAlpha().value()); + rsl_drive_sdk_.getConfiguration().getJointVelocityForAccelerationEmaAlpha().value()); } step_++; return; @@ -316,7 +302,7 @@ void StateConfigure::updateDerived() if (step_ == 28) { if (rsl_drive_sdk_.getConfiguration().getJointAccelerationFilterType()) { rsl_drive_sdk_.setJointAccelerationFilterType( - rsl_drive_sdk_.getConfiguration().getJointAccelerationFilterType().value()); + rsl_drive_sdk_.getConfiguration().getJointAccelerationFilterType().value()); } step_++; return; @@ -325,7 +311,7 @@ void StateConfigure::updateDerived() if (step_ == 29) { if (rsl_drive_sdk_.getConfiguration().getJointAccelerationKfNoiseVariance()) { rsl_drive_sdk_.setJointAccelerationKfNoiseVariance( - rsl_drive_sdk_.getConfiguration().getJointAccelerationKfNoiseVariance().value()); + rsl_drive_sdk_.getConfiguration().getJointAccelerationKfNoiseVariance().value()); } step_++; return; @@ -334,7 +320,7 @@ void StateConfigure::updateDerived() if (step_ == 30) { if (rsl_drive_sdk_.getConfiguration().getJointAccelerationKfLambda2()) { rsl_drive_sdk_.setJointAccelerationKfLambda2( - rsl_drive_sdk_.getConfiguration().getJointAccelerationKfLambda2().value()); + rsl_drive_sdk_.getConfiguration().getJointAccelerationKfLambda2().value()); } step_++; return; @@ -343,7 +329,7 @@ void StateConfigure::updateDerived() if (step_ == 31) { if (rsl_drive_sdk_.getConfiguration().getJointAccelerationKfGamma()) { rsl_drive_sdk_.setJointAccelerationKfGamma( - rsl_drive_sdk_.getConfiguration().getJointAccelerationKfGamma().value()); + rsl_drive_sdk_.getConfiguration().getJointAccelerationKfGamma().value()); } step_++; return; @@ -352,15 +338,15 @@ void StateConfigure::updateDerived() if (step_ == 32) { if (rsl_drive_sdk_.getConfiguration().getJointAccelerationEmaAlpha()) { rsl_drive_sdk_.setJointAccelerationEmaAlpha( - rsl_drive_sdk_.getConfiguration().getJointAccelerationEmaAlpha().value()); + rsl_drive_sdk_.getConfiguration().getJointAccelerationEmaAlpha().value()); } step_++; return; } - if(step_ == 33) { + if (step_ == 33) { if (rsl_drive_sdk_.getConfiguration().getDGainFilterCutoffFrequency()) { rsl_drive_sdk_.setDGainFilterCutoffFrequency( - rsl_drive_sdk_.getConfiguration().getDGainFilterCutoffFrequency().value()); + rsl_drive_sdk_.getConfiguration().getDGainFilterCutoffFrequency().value()); } step_++; return; @@ -369,6 +355,5 @@ void StateConfigure::updateDerived() isDone_ = true; } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateControlOp.cpp b/src/fsm/StateControlOp.cpp index 70f9b65..16b1c6d 100644 --- a/src/fsm/StateControlOp.cpp +++ b/src/fsm/StateControlOp.cpp @@ -2,49 +2,51 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateControlOp.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateControlOp::StateControlOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::ControlOp, + { { StateEnum::Configure, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY }, + { StateEnum::Standby, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY }, + { StateEnum::Calibrate, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY }, + { StateEnum::MotorOp, RSL_DRIVE_CW_ID_CONTROL_OP_TO_MOTOR_OP } }) +{ +} -StateControlOp::StateControlOp( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::ControlOp, - {{StateEnum::Configure, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY}, - {StateEnum::Standby, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY}, - {StateEnum::Calibrate, RSL_DRIVE_CW_ID_CONTROL_OP_TO_STANDBY}, - {StateEnum::MotorOp, RSL_DRIVE_CW_ID_CONTROL_OP_TO_MOTOR_OP}}) {} - -StateControlOp::~StateControlOp() {} +StateControlOp::~StateControlOp() +{ +} void StateControlOp::enterDerived() { -// rsl_drive_sdk_.getCommunicationInterface()->configureHeartBeat(false); + // rsl_drive_sdk_.getCommunicationInterface()->configureHeartBeat(false); } void StateControlOp::leaveDerived() @@ -57,9 +59,8 @@ void StateControlOp::leaveDerived() rsl_drive_sdk_.stageFreeze(); } -// rsl_drive_sdk_.getCommunicationInterface()->configureHeartBeat(true); + // rsl_drive_sdk_.getCommunicationInterface()->configureHeartBeat(true); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateDeviceMissing.cpp b/src/fsm/StateDeviceMissing.cpp index 9119498..62371a6 100644 --- a/src/fsm/StateDeviceMissing.cpp +++ b/src/fsm/StateDeviceMissing.cpp @@ -2,54 +2,55 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateDeviceMissing.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateDeviceMissing::StateDeviceMissing(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::DeviceMissing) +{ +} -StateDeviceMissing::StateDeviceMissing( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::DeviceMissing) {} - -StateDeviceMissing::~StateDeviceMissing() {} +StateDeviceMissing::~StateDeviceMissing() +{ +} void StateDeviceMissing::enterDerived() { MELO_WARN("The device is disconnected."); - //rsl_drive_sdk_.deviceDisconnectedCb(); + // rsl_drive_sdk_.deviceDisconnectedCb(); } void StateDeviceMissing::leaveDerived() { MELO_WARN("The device is reconnected."); - //rsl_drive_sdk_.deviceReconnectedCb(); + // rsl_drive_sdk_.deviceReconnectedCb(); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateEnum.cpp b/src/fsm/StateEnum.cpp index 76133f1..7992228 100644 --- a/src/fsm/StateEnum.cpp +++ b/src/fsm/StateEnum.cpp @@ -2,34 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateEnum.hpp" - namespace rsl_drive_sdk { namespace fsm { - uint16_t stateEnumToId(const StateEnum stateEnum) { if (stateEnum == StateEnum::Calibrate) { @@ -144,7 +144,7 @@ std::string stateEnumToName(const StateEnum stateEnum) return RSL_DRIVE_STATE_NAME_NA; } -StateEnum stateNameToEnum(const std::string & string) +StateEnum stateNameToEnum(const std::string& string) { if (string == RSL_DRIVE_STATE_NAME_CALIBRATE) { return StateEnum::Calibrate; @@ -182,11 +182,10 @@ StateEnum stateNameToEnum(const std::string & string) return StateEnum::NA; } -std::ostream & operator<<(std::ostream & out, const StateEnum stateEnum) +std::ostream& operator<<(std::ostream& out, const StateEnum stateEnum) { return out << stateEnumToName(stateEnum); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateError.cpp b/src/fsm/StateError.cpp index 77ef0a4..0556d30 100644 --- a/src/fsm/StateError.cpp +++ b/src/fsm/StateError.cpp @@ -2,50 +2,53 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateError.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateError::StateError(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Error) +{ +} -StateError::StateError(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Error) {} - -StateError::~StateError() {} +StateError::~StateError() +{ +} void StateError::enterDerived() { - // rsl_drive_sdk_.errorCb(); + // rsl_drive_sdk_.errorCb(); } void StateError::leaveDerived() { - //rsl_drive_sdk_.errorRecoveredCb(); + // rsl_drive_sdk_.errorRecoveredCb(); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateFatal.cpp b/src/fsm/StateFatal.cpp index 87302bd..c41067d 100644 --- a/src/fsm/StateFatal.cpp +++ b/src/fsm/StateFatal.cpp @@ -2,50 +2,53 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateFatal.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateFatal::StateFatal(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Fatal) +{ +} -StateFatal::StateFatal(DriveEthercatDevice & rsl_drive_sdk, std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Fatal) {} - -StateFatal::~StateFatal() {} +StateFatal::~StateFatal() +{ +} void StateFatal::enterDerived() { - //rsl_drive_sdk_.fatalCb(); + // rsl_drive_sdk_.fatalCb(); } void StateFatal::leaveDerived() { - //rsl_drive_sdk_.fatalRecoveredCb(); + // rsl_drive_sdk_.fatalRecoveredCb(); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateMachine.cpp b/src/fsm/StateMachine.cpp index a24ba59..237375c 100644 --- a/src/fsm/StateMachine.cpp +++ b/src/fsm/StateMachine.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "message_logger/message_logger.hpp" @@ -36,15 +38,12 @@ #include "rsl_drive_sdk/fsm/StateWarmStart.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { - -StateMachine::StateMachine(DriveEthercatDevice & rsl_drive_sdk) -: rsl_drive_sdk_(rsl_drive_sdk) +StateMachine::StateMachine(DriveEthercatDevice& rsl_drive_sdk) : rsl_drive_sdk_(rsl_drive_sdk) { std::lock_guard lock(mutex_); activeStateEnum_ = StateEnum::NA; @@ -62,20 +61,21 @@ StateMachine::StateMachine(DriveEthercatDevice & rsl_drive_sdk) addState(StateBasePtr(new StateWarmStart(rsl_drive_sdk_, goalStateEnum_))); } -StateMachine::~StateMachine() {} +StateMachine::~StateMachine() +{ +} void StateMachine::updateActiveState(const StateEnum newActiveStateEnum) { // Check if the active state changed. std::lock_guard lock(mutex_); auto activeStateIt = states_.find(activeStateEnum_); - StateBasePtr activeState = (activeStateIt == - states_.end()) ? StateBasePtr() : activeStateIt->second; + StateBasePtr activeState = (activeStateIt == states_.end()) ? StateBasePtr() : activeStateIt->second; if (newActiveStateEnum != activeStateEnum_) { auto newActiveStateIt = states_.find(newActiveStateEnum); if (newActiveStateIt == states_.end()) { - MELO_WARN_STREAM("New active FSM state '" << stateEnumToName(newActiveStateEnum) << - "' has not been found in the list of states."); + MELO_WARN_STREAM("New active FSM state '" << stateEnumToName(newActiveStateEnum) + << "' has not been found in the list of states."); } else { if (activeState) { activeState->leave(); @@ -116,18 +116,13 @@ void StateMachine::setGoalStateEnum(const StateEnum goalStateEnum) MELO_WARN_STREAM("Cannot set goal FSM state to DeviceMissing."); return; } - if (goalStateEnum == StateEnum::Error || - goalStateEnum == StateEnum::Fatal) - { + if (goalStateEnum == StateEnum::Error || goalStateEnum == StateEnum::Fatal) { MELO_WARN_STREAM("Cannot set goal FSM state to Error or Fatal."); return; } - if (goalStateEnum == StateEnum::ColdStart || - goalStateEnum == StateEnum::WarmStart || - goalStateEnum == StateEnum::MotorPreOp) - { - MELO_WARN_STREAM( - "Cannot set goal FSM state to ColdStart, WarmStart or MotorPreOp (auto-transition states)."); + if (goalStateEnum == StateEnum::ColdStart || goalStateEnum == StateEnum::WarmStart || + goalStateEnum == StateEnum::MotorPreOp) { + MELO_WARN_STREAM("Cannot set goal FSM state to ColdStart, WarmStart or MotorPreOp (auto-transition states)."); return; } if (goalStateEnum == getActiveStateEnum()) { @@ -135,8 +130,8 @@ void StateMachine::setGoalStateEnum(const StateEnum goalStateEnum) return; } - MELO_DEBUG_STREAM("[" << rsl_drive_sdk_.getName() << "]Setting goal FSM state to '" << - stateEnumToName(goalStateEnum) << "'."); + MELO_DEBUG_STREAM("[" << rsl_drive_sdk_.getName() << "]Setting goal FSM state to '" << stateEnumToName(goalStateEnum) + << "'."); std::lock_guard lock(mutex_); goalStateEnum_ = goalStateEnum; } @@ -152,11 +147,10 @@ std::string StateMachine::getName() const return rsl_drive_sdk_.getName(); } -void StateMachine::addState(const StateBasePtr & state) +void StateMachine::addState(const StateBasePtr& state) { - states_.insert({state->getStateEnum(), state}); + states_.insert({ state->getStateEnum(), state }); } - -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateMotorOp.cpp b/src/fsm/StateMotorOp.cpp index e061c05..260391f 100644 --- a/src/fsm/StateMotorOp.cpp +++ b/src/fsm/StateMotorOp.cpp @@ -2,46 +2,47 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateMotorOp.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateMotorOp::StateMotorOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::MotorOp, + { { StateEnum::Configure, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY }, + { StateEnum::Standby, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY }, + { StateEnum::Calibrate, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY }, + { StateEnum::ControlOp, RSL_DRIVE_CW_ID_MOTOR_OP_TO_CONTROL_OP } }) +{ +} -StateMotorOp::StateMotorOp( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::MotorOp, - {{StateEnum::Configure, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY}, - {StateEnum::Standby, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY}, - {StateEnum::Calibrate, RSL_DRIVE_CW_ID_MOTOR_OP_TO_STANDBY}, - {StateEnum::ControlOp, RSL_DRIVE_CW_ID_MOTOR_OP_TO_CONTROL_OP}}) {} - -StateMotorOp::~StateMotorOp() {} - +StateMotorOp::~StateMotorOp() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateMotorPreOp.cpp b/src/fsm/StateMotorPreOp.cpp index 4f8d4ac..759c2cb 100644 --- a/src/fsm/StateMotorPreOp.cpp +++ b/src/fsm/StateMotorPreOp.cpp @@ -2,42 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateMotorPreOp.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateMotorPreOp::StateMotorPreOp(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::MotorPreOp) +{ +} -StateMotorPreOp::StateMotorPreOp( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::MotorPreOp) {} - -StateMotorPreOp::~StateMotorPreOp() {} - +StateMotorPreOp::~StateMotorPreOp() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateStandby.cpp b/src/fsm/StateStandby.cpp index 9a305cf..af3b31b 100644 --- a/src/fsm/StateStandby.cpp +++ b/src/fsm/StateStandby.cpp @@ -2,46 +2,47 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateStandby.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateStandby::StateStandby(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Standby, + { { StateEnum::Configure, RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE }, + { StateEnum::Calibrate, RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE }, + { StateEnum::MotorOp, RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP }, + { StateEnum::ControlOp, RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP } }) +{ +} -StateStandby::StateStandby( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::Standby, - {{StateEnum::Configure, RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE}, - {StateEnum::Calibrate, RSL_DRIVE_CW_ID_STANDBY_TO_CONFIGURE}, - {StateEnum::MotorOp, RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP}, - {StateEnum::ControlOp, RSL_DRIVE_CW_ID_STANDBY_TO_MOTOR_PREOP}}) {} - -StateStandby::~StateStandby() {} - +StateStandby::~StateStandby() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/fsm/StateWarmStart.cpp b/src/fsm/StateWarmStart.cpp index 2dd6e76..b1f6ca4 100644 --- a/src/fsm/StateWarmStart.cpp +++ b/src/fsm/StateWarmStart.cpp @@ -2,42 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/fsm/StateWarmStart.hpp" #include "rsl_drive_sdk/Drive.hpp" - namespace rsl_drive_sdk { namespace fsm { +StateWarmStart::StateWarmStart(DriveEthercatDevice& rsl_drive_sdk, std::atomic& goalStateEnum) + : StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::WarmStart) +{ +} -StateWarmStart::StateWarmStart( - DriveEthercatDevice & rsl_drive_sdk, - std::atomic & goalStateEnum) -: StateBase(rsl_drive_sdk, goalStateEnum, StateEnum::WarmStart) {} - -StateWarmStart::~StateWarmStart() {} - +StateWarmStart::~StateWarmStart() +{ +} -} // fsm -} // rsl_drive_sdk +} // namespace fsm +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeBase.cpp b/src/mode/ModeBase.cpp index 7ae83fc..7f91a4e 100644 --- a/src/mode/ModeBase.cpp +++ b/src/mode/ModeBase.cpp @@ -2,38 +2,41 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeBase.hpp" - namespace rsl_drive_sdk { namespace mode { +ModeBase::ModeBase(const ModeEnum modeEnum) : modeEnum_(modeEnum) +{ +} -ModeBase::ModeBase(const ModeEnum modeEnum) -: modeEnum_(modeEnum) {} - -ModeBase::~ModeBase() {} +ModeBase::~ModeBase() +{ +} ModeEnum ModeBase::getModeEnum() const { @@ -47,7 +50,7 @@ std::string ModeBase::getName() const return modeEnumToName(modeEnum_); } -void ModeBase::setPidGains(const PidGainsF & pidGains) +void ModeBase::setPidGains(const PidGainsF& pidGains) { std::lock_guard lock(mutex_); pidGains_ = pidGains; @@ -59,6 +62,5 @@ std::optional ModeBase::getPidGains() const return pidGains_; } - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeCurrent.cpp b/src/mode/ModeCurrent.cpp index 9208eab..5a7f320 100644 --- a/src/mode/ModeCurrent.cpp +++ b/src/mode/ModeCurrent.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeCurrent.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeCurrent::ModeCurrent() -: ModeBase(ModeEnum::Current) +ModeCurrent::ModeCurrent() : ModeBase(ModeEnum::Current) { controlCurrent_ = true; } -ModeCurrent::~ModeCurrent() {} - +ModeCurrent::~ModeCurrent() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeDisable.cpp b/src/mode/ModeDisable.cpp index 29ec2cd..479d282 100644 --- a/src/mode/ModeDisable.cpp +++ b/src/mode/ModeDisable.cpp @@ -2,39 +2,41 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeDisable.hpp" - namespace rsl_drive_sdk { namespace mode { +ModeDisable::ModeDisable() : ModeBase(ModeEnum::Disable) +{ +} -ModeDisable::ModeDisable() -: ModeBase(ModeEnum::Disable) {} - -ModeDisable::~ModeDisable() {} - +ModeDisable::~ModeDisable() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeEnum.cpp b/src/mode/ModeEnum.cpp index 07b9dff..1c75578 100644 --- a/src/mode/ModeEnum.cpp +++ b/src/mode/ModeEnum.cpp @@ -2,34 +2,34 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeEnum.hpp" - namespace rsl_drive_sdk { namespace mode { - uint16_t modeEnumToId(const ModeEnum modeEnum) { if (modeEnum == ModeEnum::Current) { @@ -218,7 +218,7 @@ std::string modeEnumToShortName(const ModeEnum modeEnum) return RSL_DRIVE_MODE_NAME_SHORT_NA; } -ModeEnum modeNameToEnum(const std::string & string) +ModeEnum modeNameToEnum(const std::string& string) { if (string == RSL_DRIVE_MODE_NAME_CURRENT) { return ModeEnum::Current; @@ -265,11 +265,10 @@ ModeEnum modeNameToEnum(const std::string & string) return ModeEnum::NA; } -std::ostream & operator<<(std::ostream & out, const ModeEnum modeEnum) +std::ostream& operator<<(std::ostream& out, const ModeEnum modeEnum) { return out << modeEnumToName(modeEnum); } - -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeFreeze.cpp b/src/mode/ModeFreeze.cpp index 9afc6c8..6a47f21 100644 --- a/src/mode/ModeFreeze.cpp +++ b/src/mode/ModeFreeze.cpp @@ -2,39 +2,41 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeFreeze.hpp" - namespace rsl_drive_sdk { namespace mode { +ModeFreeze::ModeFreeze() : ModeBase(ModeEnum::Freeze) +{ +} -ModeFreeze::ModeFreeze() -: ModeBase(ModeEnum::Freeze) {} - -ModeFreeze::~ModeFreeze() {} - +ModeFreeze::~ModeFreeze() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeGearPosition.cpp b/src/mode/ModeGearPosition.cpp index 078ae85..7a1f68d 100644 --- a/src/mode/ModeGearPosition.cpp +++ b/src/mode/ModeGearPosition.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeGearPosition.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeGearPosition::ModeGearPosition() -: ModeBase(ModeEnum::GearPosition) +ModeGearPosition::ModeGearPosition() : ModeBase(ModeEnum::GearPosition) { controlGearPosition_ = true; } -ModeGearPosition::~ModeGearPosition() {} - +ModeGearPosition::~ModeGearPosition() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeGearVelocity.cpp b/src/mode/ModeGearVelocity.cpp index b9af571..c4bb168 100644 --- a/src/mode/ModeGearVelocity.cpp +++ b/src/mode/ModeGearVelocity.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeGearVelocity.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeGearVelocity::ModeGearVelocity() -: ModeBase(ModeEnum::GearVelocity) +ModeGearVelocity::ModeGearVelocity() : ModeBase(ModeEnum::GearVelocity) { controlGearVelocity_ = true; } -ModeGearVelocity::~ModeGearVelocity() {} - +ModeGearVelocity::~ModeGearVelocity() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointPosition.cpp b/src/mode/ModeJointPosition.cpp index 89ab05f..a92de84 100644 --- a/src/mode/ModeJointPosition.cpp +++ b/src/mode/ModeJointPosition.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointPosition.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointPosition::ModeJointPosition() -: ModeBase(ModeEnum::JointPosition) +ModeJointPosition::ModeJointPosition() : ModeBase(ModeEnum::JointPosition) { controlJointPosition_ = true; } -ModeJointPosition::~ModeJointPosition() {} - +ModeJointPosition::~ModeJointPosition() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointPositionTorque.cpp b/src/mode/ModeJointPositionTorque.cpp index 2b4ff16..d103af2 100644 --- a/src/mode/ModeJointPositionTorque.cpp +++ b/src/mode/ModeJointPositionTorque.cpp @@ -2,43 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointPositionTorque.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointPositionTorque::ModeJointPositionTorque() -: ModeBase(ModeEnum::JointPositionTorque) +ModeJointPositionTorque::ModeJointPositionTorque() : ModeBase(ModeEnum::JointPositionTorque) { controlJointPosition_ = true; controlJointTorque_ = true; } -ModeJointPositionTorque::~ModeJointPositionTorque() {} - +ModeJointPositionTorque::~ModeJointPositionTorque() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointPositionVelocity.cpp b/src/mode/ModeJointPositionVelocity.cpp index 0f7e352..a1ee4e2 100644 --- a/src/mode/ModeJointPositionVelocity.cpp +++ b/src/mode/ModeJointPositionVelocity.cpp @@ -2,43 +2,43 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointPositionVelocity.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointPositionVelocity::ModeJointPositionVelocity() -: ModeBase(ModeEnum::JointPositionVelocity) +ModeJointPositionVelocity::ModeJointPositionVelocity() : ModeBase(ModeEnum::JointPositionVelocity) { controlJointPosition_ = true; controlJointVelocity_ = true; } -ModeJointPositionVelocity::~ModeJointPositionVelocity() {} - +ModeJointPositionVelocity::~ModeJointPositionVelocity() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointPositionVelocityTorque.cpp b/src/mode/ModeJointPositionVelocityTorque.cpp index cd1a8f3..531bb17 100644 --- a/src/mode/ModeJointPositionVelocityTorque.cpp +++ b/src/mode/ModeJointPositionVelocityTorque.cpp @@ -2,44 +2,44 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointPositionVelocityTorque.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointPositionVelocityTorque::ModeJointPositionVelocityTorque() -: ModeBase(ModeEnum::JointPositionVelocityTorque) +ModeJointPositionVelocityTorque::ModeJointPositionVelocityTorque() : ModeBase(ModeEnum::JointPositionVelocityTorque) { controlJointPosition_ = true; controlJointVelocity_ = true; controlJointTorque_ = true; } -ModeJointPositionVelocityTorque::~ModeJointPositionVelocityTorque() {} - +ModeJointPositionVelocityTorque::~ModeJointPositionVelocityTorque() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointPositionVelocityTorquePidGains.cpp b/src/mode/ModeJointPositionVelocityTorquePidGains.cpp index 11998c6..ae5bf94 100644 --- a/src/mode/ModeJointPositionVelocityTorquePidGains.cpp +++ b/src/mode/ModeJointPositionVelocityTorquePidGains.cpp @@ -2,36 +2,36 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointPositionVelocityTorquePidGains.hpp" - namespace rsl_drive_sdk { namespace mode { - ModeJointPositionVelocityTorquePidGains::ModeJointPositionVelocityTorquePidGains() -: ModeBase(ModeEnum::JointPositionVelocityTorquePidGains) + : ModeBase(ModeEnum::JointPositionVelocityTorquePidGains) { controlJointPosition_ = true; controlJointVelocity_ = true; @@ -39,8 +39,9 @@ ModeJointPositionVelocityTorquePidGains::ModeJointPositionVelocityTorquePidGains customGains_ = true; } -ModeJointPositionVelocityTorquePidGains::~ModeJointPositionVelocityTorquePidGains() {} - +ModeJointPositionVelocityTorquePidGains::~ModeJointPositionVelocityTorquePidGains() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointTorque.cpp b/src/mode/ModeJointTorque.cpp index d0a74b8..8aa1170 100644 --- a/src/mode/ModeJointTorque.cpp +++ b/src/mode/ModeJointTorque.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointTorque.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointTorque::ModeJointTorque() -: ModeBase(ModeEnum::JointTorque) +ModeJointTorque::ModeJointTorque() : ModeBase(ModeEnum::JointTorque) { controlJointTorque_ = true; } -ModeJointTorque::~ModeJointTorque() {} - +ModeJointTorque::~ModeJointTorque() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeJointVelocity.cpp b/src/mode/ModeJointVelocity.cpp index b446f13..6c99aeb 100644 --- a/src/mode/ModeJointVelocity.cpp +++ b/src/mode/ModeJointVelocity.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeJointVelocity.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeJointVelocity::ModeJointVelocity() -: ModeBase(ModeEnum::JointVelocity) +ModeJointVelocity::ModeJointVelocity() : ModeBase(ModeEnum::JointVelocity) { controlJointVelocity_ = true; } -ModeJointVelocity::~ModeJointVelocity() {} - +ModeJointVelocity::~ModeJointVelocity() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeMotorPosition.cpp b/src/mode/ModeMotorPosition.cpp index bd2f4eb..5e9a512 100644 --- a/src/mode/ModeMotorPosition.cpp +++ b/src/mode/ModeMotorPosition.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeMotorPosition.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeMotorPosition::ModeMotorPosition() -: ModeBase(ModeEnum::MotorPosition) +ModeMotorPosition::ModeMotorPosition() : ModeBase(ModeEnum::MotorPosition) { controlMotorPosition_ = true; } -ModeMotorPosition::~ModeMotorPosition() {} - +ModeMotorPosition::~ModeMotorPosition() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/mode/ModeMotorVelocity.cpp b/src/mode/ModeMotorVelocity.cpp index f27c7f0..6296ecd 100644 --- a/src/mode/ModeMotorVelocity.cpp +++ b/src/mode/ModeMotorVelocity.cpp @@ -2,42 +2,42 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // rsl_drive_sdk #include "rsl_drive_sdk/mode/ModeMotorVelocity.hpp" - namespace rsl_drive_sdk { namespace mode { - -ModeMotorVelocity::ModeMotorVelocity() -: ModeBase(ModeEnum::MotorVelocity) +ModeMotorVelocity::ModeMotorVelocity() : ModeBase(ModeEnum::MotorVelocity) { controlMotorVelocity_ = true; } -ModeMotorVelocity::~ModeMotorVelocity() {} - +ModeMotorVelocity::~ModeMotorVelocity() +{ +} -} // mode -} // rsl_drive_sdk +} // namespace mode +} // namespace rsl_drive_sdk diff --git a/src/thread_sleep.cpp b/src/thread_sleep.cpp index da4dc0a..35b25db 100644 --- a/src/thread_sleep.cpp +++ b/src/thread_sleep.cpp @@ -2,23 +2,25 @@ ** Copyright 2024 Robotic Systems Lab - ETH Zurich: ** Remo Diethelm, Christian Gehring, Samuel Bachmann, Philipp Leeman, Lennart Nachtigall, Jonas Junger, Jan Preisig, ** Fabian Tischhauser, Johannes Pankert - ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the following conditions - *are met: + ** Redistribution and use in source and binary forms, with or without modification, are permitted provided that the + *following conditions are met: ** - ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following disclaimer. + ** 1. Redistributions of source code must retain the above copyright notice, this list of conditions and the following + *disclaimer. ** - ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the following disclaimer in the - *documentation and/or other materials provided with the distribution. + ** 2. Redistributions in binary form must reproduce the above copyright notice, this list of conditions and the + *following disclaimer in the documentation and/or other materials provided with the distribution. ** - ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote products derived from - *this software without specific prior written permission. + ** 3. Neither the name of the copyright holder nor the names of its contributors may be used to endorse or promote + *products derived from this software without specific prior written permission. ** - ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT - *LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT - *HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT - *LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON - *ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE - *USE OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. + ** THIS SOFTWARE IS PROVIDED BY THE COPYRIGHT HOLDERS AND CONTRIBUTORS "AS IS" AND ANY EXPRESS OR IMPLIED WARRANTIES, + *INCLUDING, BUT NOT LIMITED TO, THE IMPLIED WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE + *DISCLAIMED. IN NO EVENT SHALL THE COPYRIGHT HOLDER OR CONTRIBUTORS BE LIABLE FOR ANY DIRECT, INDIRECT, INCIDENTAL, + *SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR + *SERVICES; LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND ON ANY THEORY OF LIABILITY, + *WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE + *OF THIS SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE. */ // std @@ -28,15 +30,12 @@ // rsl_drive_sdk #include "rsl_drive_sdk/thread_sleep.hpp" - namespace rsl_drive_sdk { - void thread_sleep(const double duration) { std::this_thread::sleep_for(std::chrono::nanoseconds(static_cast(1e9 * duration))); } - -} // rsl_drive_sdk +} // namespace rsl_drive_sdk