From da3a1aca913b04a0ac3eb54d22d06d9664542c87 Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 12:16:58 +0200 Subject: [PATCH 01/28] Add usb communication option --- .../include/march_hardware/march_robot.h | 26 ++-- march_hardware/src/march_robot.cpp | 114 +++++++++--------- .../march_hardware_builder/allowed_robot.h | 12 ++ .../march_hardware_builder/hardware_builder.h | 5 + .../robots/odrive_test_joint_rotational.yaml | 18 +++ .../src/hardware_builder.cpp | 56 +++++++-- 6 files changed, 157 insertions(+), 74 deletions(-) create mode 100644 march_hardware_builder/robots/odrive_test_joint_rotational.yaml diff --git a/march_hardware/include/march_hardware/march_robot.h b/march_hardware/include/march_hardware/march_robot.h index 12c745efc..39b079750 100644 --- a/march_hardware/include/march_hardware/march_robot.h +++ b/march_hardware/include/march_hardware/march_robot.h @@ -20,18 +20,20 @@ class MarchRobot private: ::std::vector jointList; urdf::Model urdf_; - EthercatMaster ethercatMaster; + std::unique_ptr ethercatMaster; std::unique_ptr pdb_; public: using iterator = std::vector::iterator; - MarchRobot(::std::vector jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout); + MarchRobot(::std::vector jointList, urdf::Model urdf, std::unique_ptr ethercatMaster); MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout); + std::unique_ptr powerDistributionBoard, + std::unique_ptr ethercatMaster); + + MarchRobot(::std::vector jointList, urdf::Model urdf, + std::unique_ptr powerDistributionBoard); ~MarchRobot(); @@ -45,21 +47,21 @@ class MarchRobot void resetMotorControllers(); - void startEtherCAT(bool reset_motor_controllers); + void startCommunication(bool reset_motor_controllers); - void stopEtherCAT(); - - int getMaxSlaveIndex(); + void stopCommunication(); bool hasValidSlaves(); bool isEthercatOperational(); - std::exception_ptr getLastEthercatException() const noexcept; + bool isCommunicationOperational(); + + std::exception_ptr getLastCommunicationException() const noexcept; - void waitForPdo(); + void waitForUpdate(); - int getEthercatCycleTime() const; + int getCycleTime() const; Joint& getJoint(::std::string jointName); diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index 980329911..e2f3f61a6 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -14,26 +14,34 @@ namespace march { -MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout) +MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, std::unique_ptr ethercatMaster) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) + , ethercatMaster(std::move(ethercatMaster)) , pdb_(nullptr) { } MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, - int ecatCycleTime, int ecatSlaveTimeout) + std::unique_ptr powerDistributionBoard, + std::unique_ptr ethercatMaster) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) + , ethercatMaster(std::move(ethercatMaster)) , pdb_(std::move(powerDistributionBoard)) { } -void MarchRobot::startEtherCAT(bool reset_motor_controllers) +MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, + std::unique_ptr powerDistributionBoard) + : jointList(std::move(jointList)) + , urdf_(std::move(urdf)) + , ethercatMaster(nullptr) + , pdb_(std::move(powerDistributionBoard)) +{ +} + +void MarchRobot::startCommunication(bool reset_motor_controllers) { if (!hasValidSlaves()) { @@ -42,35 +50,40 @@ void MarchRobot::startEtherCAT(bool reset_motor_controllers) ROS_INFO("Slave configuration is non-conflicting"); - if (ethercatMaster.isOperational()) + if (this->isCommunicationOperational()) { - ROS_WARN("Trying to start EtherCAT while it is already active."); + ROS_WARN("Trying to start Communication while it is already active."); return; } - bool config_overwritten = ethercatMaster.start(this->jointList); - - if (reset_motor_controllers || config_overwritten) + if (ethercatMaster != nullptr) { - ROS_DEBUG("Resetting all motor controllers due to either: reset arg: %d or downloading of configuration file: %d", - reset_motor_controllers, config_overwritten); - resetMotorControllers(); + bool sw_reset = ethercatMaster->start(this->jointList); - ROS_INFO("Restarting the EtherCAT Master"); - ethercatMaster.stop(); - config_overwritten = ethercatMaster.start(this->jointList); + if (reset_motor_controllers || sw_reset) + { + ROS_DEBUG("Resetting all MotorControllers due to either: reset arg: %d or downloading of .sw fie: %d", + reset_motor_controllers, sw_reset); + resetMotorControllers(); + + ROS_INFO("Restarting the EtherCAT Master"); + ethercatMaster->stop(); + sw_reset = ethercatMaster->start(this->jointList); + } } } -void MarchRobot::stopEtherCAT() +void MarchRobot::stopCommunication() { - if (!ethercatMaster.isOperational()) + if (!this->isCommunicationOperational()) { - ROS_WARN("Trying to stop EtherCAT while it is not active."); + ROS_WARN("Trying to stop Communication while it is not active."); return; } - ethercatMaster.stop(); + if (ethercatMaster != nullptr) { + ethercatMaster->stop(); + } } void MarchRobot::resetMotorControllers() @@ -81,28 +94,6 @@ void MarchRobot::resetMotorControllers() } } -int MarchRobot::getMaxSlaveIndex() -{ - int maxSlaveIndex = -1; - - for (Joint& joint : jointList) - { - int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); - if (temperatureSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = temperatureSlaveIndex; - } - - int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; - - if (motorControllerSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = motorControllerSlaveIndex; - } - } - return maxSlaveIndex; -} - bool MarchRobot::hasValidSlaves() { ::std::vector motorControllerIndices; @@ -154,27 +145,40 @@ bool MarchRobot::hasValidSlaves() bool MarchRobot::isEthercatOperational() { - return ethercatMaster.isOperational(); + return ethercatMaster != nullptr && ethercatMaster->isOperational(); } -std::exception_ptr MarchRobot::getLastEthercatException() const noexcept +bool MarchRobot::isCommunicationOperational() { - return this->ethercatMaster.getLastException(); + return this->isEthercatOperational(); } -void MarchRobot::waitForPdo() +std::exception_ptr MarchRobot::getLastCommunicationException() const noexcept { - this->ethercatMaster.waitForPdo(); + if (ethercatMaster != nullptr) { + return this->ethercatMaster->getLastException(); + } + return nullptr; } -int MarchRobot::getEthercatCycleTime() const +void MarchRobot::waitForUpdate() { - return this->ethercatMaster.getCycleTime(); + if (ethercatMaster != nullptr) { + this->ethercatMaster->waitForPdo(); + } +} + +int MarchRobot::getCycleTime() const +{ + if (ethercatMaster != nullptr) { + return this->ethercatMaster->getCycleTime(); + } + return 0; } Joint& MarchRobot::getJoint(::std::string jointName) { - if (!ethercatMaster.isOperational()) + if (!this->isCommunicationOperational()) { ROS_WARN("Trying to access joints while ethercat is not operational. This " "may lead to incorrect sensor data."); @@ -192,7 +196,7 @@ Joint& MarchRobot::getJoint(::std::string jointName) Joint& MarchRobot::getJoint(size_t index) { - if (!ethercatMaster.isOperational()) + if (!this->isCommunicationOperational()) { ROS_WARN("Trying to access joints while ethercat is not operational. This " "may lead to incorrect sensor data."); @@ -207,7 +211,7 @@ size_t MarchRobot::size() const MarchRobot::iterator MarchRobot::begin() { - if (!ethercatMaster.isOperational()) + if (!this->isCommunicationOperational()) { ROS_WARN("Trying to access joints while ethercat is not operational. This " "may lead to incorrect sensor data."); @@ -232,7 +236,7 @@ PowerDistributionBoard* MarchRobot::getPowerDistributionBoard() const MarchRobot::~MarchRobot() { - stopEtherCAT(); + stopCommunication(); } const urdf::Model& MarchRobot::getUrdf() const diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index 6264f4bf7..09f52bb02 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -15,6 +15,7 @@ class AllowedRobot march4, march3, test_joint_rotational, + odrive_test_joint_rotational, test_joint_linear, pdb, }; @@ -34,6 +35,10 @@ class AllowedRobot { this->value = test_joint_rotational; } + else if (robot_name == "odrive_test_joint_rotational") + { + this->value = odrive_test_joint_rotational; + } else if (robot_name == "test_joint_linear") { this->value = test_joint_linear; @@ -64,6 +69,10 @@ class AllowedRobot { return base_path.append("/robots/test_joint_rotational.yaml"); } + else if (this->value == AllowedRobot::odrive_test_joint_rotational) + { + return base_path.append("/robots/odrive_test_joint_rotational.yaml"); + } else if (this->value == AllowedRobot::test_joint_linear) { return base_path.append("/robots/test_joint_linear.yaml"); @@ -105,6 +114,9 @@ class AllowedRobot case test_joint_rotational: out << "test_joint_rotational"; break; + case odrive_test_joint_rotational: + out << "odrive_test_joint_rotational"; + break; case pdb: out << "pdb"; break; diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index 823b8c785..cd2dad271 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -117,4 +117,9 @@ class HardwareBuilder */ std::string convertSWFileToString(std::ifstream& sw_file); +/** + * Returns the highest slave index of motor controllers and GESs in joints + */ +int getMaxSlaveIndex(std::vector jointList); + #endif // MARCH_HARDWARE_BUILDER_HARDWARE_BUILDER_H diff --git a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml new file mode 100644 index 000000000..c8f09231d --- /dev/null +++ b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml @@ -0,0 +1,18 @@ +testsetup: + ifName: enp2s0 + cycleTime: 4 + slaveTimeout: 50 + joints: + - rotational_joint: + actuationMode: torque + allowActuation: true + odrive: + serial_number: 0x2084387E304E + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 97242 + maxPositionIU: 119617 + incrementalEncoder: + resolution: 12 + transmission: 101 diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 5972dc57a..520bd0c4f 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -14,6 +14,7 @@ #include #include #include +#include "march_hardware/ethercat/ethercat_master.h" #include // clang-format off @@ -55,25 +56,36 @@ HardwareBuilder::HardwareBuilder(const std::string& yaml_path, urdf::Model urdf) std::unique_ptr HardwareBuilder::createMarchRobot() { this->initUrdf(); - auto pdo_interface = march::PdoInterfaceImpl::create(); - auto sdo_interface = march::SdoInterfaceImpl::create(); const auto robot_name = this->robot_config_.begin()->first.as(); ROS_DEBUG_STREAM("Starting creation of robot " << robot_name); // Remove top level robot name key YAML::Node config = this->robot_config_[robot_name]; - const auto if_name = config["ifName"].as(); - const auto cycle_time = config["ecatCycleTime"].as(); - const auto slave_timeout = config["ecatSlaveTimeout"].as(); + const auto cycle_time = config["cycleTime"].as(); + const auto slave_timeout = config["slaveTimeout"].as(); + + auto pdo_interface = march::PdoInterfaceImpl::create(); + auto sdo_interface = march::SdoInterfaceImpl::create(); std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface); ROS_INFO_STREAM("Robot config:\n" << config); YAML::Node pdb_config = config["powerDistributionBoard"]; auto pdb = HardwareBuilder::createPowerDistributionBoard(pdb_config, pdo_interface, sdo_interface); - return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), if_name, cycle_time, - slave_timeout); + + if (config["ifName"]) + { + const auto if_name = config["ifName"].as(); + auto ethercat_master = std::make_unique(if_name, getMaxSlaveIndex(joints), cycle_time, slave_timeout); + + return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), + std::move(ethercat_master)); + } + else + { + return std::make_unique(std::move(joints), this->urdf_, std::move(pdb)); + } } march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, @@ -112,6 +124,11 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const controller = HardwareBuilder::createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); } + if (joint_config["odrive"]) + { + controller = + HardwareBuilder::createODrive(joint_config["odrive"], mode, urdf_joint); + } if (!controller) { ROS_FATAL("Joint %s does not have a configuration for a motor controller", joint_name.c_str()); @@ -317,3 +334,28 @@ std::string convertSWFileToString(std::ifstream& sw_file) { return std::string(std::istreambuf_iterator(sw_file), std::istreambuf_iterator()); } + +/** + * Returns the highest slave index of motor controllers and GESs in joints + */ +int getMaxSlaveIndex(std::vector jointList) +{ + int maxSlaveIndex = -1; + + for (march::Joint& joint : jointList) + { + int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); + if (temperatureSlaveIndex > maxSlaveIndex) + { + maxSlaveIndex = temperatureSlaveIndex; + } + + int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; + + if (motorControllerSlaveIndex > maxSlaveIndex) + { + maxSlaveIndex = motorControllerSlaveIndex; + } + } + return maxSlaveIndex; +} From 5b2897ad8d891598810770a8b4a041a3ae655d20 Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 14:13:49 +0200 Subject: [PATCH 02/28] Pass usb_master to create_joints --- march_hardware/src/march_robot.cpp | 39 ++++++++------- .../march_hardware_builder/hardware_builder.h | 14 ++++-- .../src/hardware_builder.cpp | 49 ++++++++++--------- 3 files changed, 58 insertions(+), 44 deletions(-) diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index e2f3f61a6..dff4d2298 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -15,10 +15,7 @@ namespace march { MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, std::unique_ptr ethercatMaster) - : jointList(std::move(jointList)) - , urdf_(std::move(urdf)) - , ethercatMaster(std::move(ethercatMaster)) - , pdb_(nullptr) + : jointList(std::move(jointList)), urdf_(std::move(urdf)), ethercatMaster(std::move(ethercatMaster)), pdb_(nullptr) { } @@ -81,9 +78,10 @@ void MarchRobot::stopCommunication() return; } - if (ethercatMaster != nullptr) { - ethercatMaster->stop(); - } + if (ethercatMaster != nullptr) + { + ethercatMaster->stop(); + } } void MarchRobot::resetMotorControllers() @@ -145,7 +143,7 @@ bool MarchRobot::hasValidSlaves() bool MarchRobot::isEthercatOperational() { - return ethercatMaster != nullptr && ethercatMaster->isOperational(); + return ethercatMaster != nullptr && ethercatMaster->isOperational(); } bool MarchRobot::isCommunicationOperational() @@ -155,25 +153,28 @@ bool MarchRobot::isCommunicationOperational() std::exception_ptr MarchRobot::getLastCommunicationException() const noexcept { - if (ethercatMaster != nullptr) { - return this->ethercatMaster->getLastException(); - } - return nullptr; + if (ethercatMaster != nullptr) + { + return this->ethercatMaster->getLastException(); + } + return nullptr; } void MarchRobot::waitForUpdate() { - if (ethercatMaster != nullptr) { - this->ethercatMaster->waitForPdo(); - } + if (ethercatMaster != nullptr) + { + this->ethercatMaster->waitForPdo(); + } } int MarchRobot::getCycleTime() const { - if (ethercatMaster != nullptr) { - return this->ethercatMaster->getCycleTime(); - } - return 0; + if (ethercatMaster != nullptr) + { + return this->ethercatMaster->getCycleTime(); + } + return 0; } Joint& MarchRobot::getJoint(::std::string jointName) diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index cd2dad271..6fc48b932 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -21,6 +21,9 @@ #include #include +#include +#include + /** * @brief Creates a MarchRobot from a robot yaml and URDF. */ @@ -68,7 +71,7 @@ class HardwareBuilder static march::Joint createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface); + march::SdoInterfacePtr sdo_interface, UsbMaster usb_master); static std::unique_ptr createAbsoluteEncoder(const YAML::Node& absolute_encoder_config, const urdf::JointConstSharedPtr& urdf_joint); static std::unique_ptr @@ -84,6 +87,11 @@ class HardwareBuilder createPowerDistributionBoard(const YAML::Node& power_distribution_board_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface); + static std::unique_ptr createOdrive(const YAML::Node& imc_config, march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::PdoInterfacePtr pdo_interface, + march::SdoInterfacePtr sdo_interface); + static const std::vector INCREMENTAL_ENCODER_REQUIRED_KEYS; static const std::vector ABSOLUTE_ENCODER_REQUIRED_KEYS; static const std::vector IMOTIONCUBE_REQUIRED_KEYS; @@ -105,7 +113,7 @@ class HardwareBuilder * @return list of created joints */ std::vector createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface) const; + march::SdoInterfacePtr sdo_interface, UsbMaster usb_master) const; YAML::Node robot_config_; urdf::Model urdf_; @@ -120,6 +128,6 @@ std::string convertSWFileToString(std::ifstream& sw_file); /** * Returns the highest slave index of motor controllers and GESs in joints */ -int getMaxSlaveIndex(std::vector jointList); +int getMaxSlaveIndex(std::vector& jointList); #endif // MARCH_HARDWARE_BUILDER_HARDWARE_BUILDER_H diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 520bd0c4f..e5a1b5922 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -68,7 +68,9 @@ std::unique_ptr HardwareBuilder::createMarchRobot() auto pdo_interface = march::PdoInterfaceImpl::create(); auto sdo_interface = march::SdoInterfaceImpl::create(); - std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface); + auto usb_master = UsbMaster(); + + std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface, usb_master); ROS_INFO_STREAM("Robot config:\n" << config); YAML::Node pdb_config = config["powerDistributionBoard"]; @@ -77,7 +79,8 @@ std::unique_ptr HardwareBuilder::createMarchRobot() if (config["ifName"]) { const auto if_name = config["ifName"].as(); - auto ethercat_master = std::make_unique(if_name, getMaxSlaveIndex(joints), cycle_time, slave_timeout); + auto ethercat_master = + std::make_unique(if_name, getMaxSlaveIndex(joints), cycle_time, slave_timeout); return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), std::move(ethercat_master)); @@ -90,7 +93,8 @@ std::unique_ptr HardwareBuilder::createMarchRobot() march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, - march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface) + march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, + UsbMaster usb_master) { ROS_DEBUG("Starting creation of joint %s", joint_name.c_str()); if (!urdf_joint) @@ -298,7 +302,8 @@ void HardwareBuilder::initUrdf() std::vector HardwareBuilder::createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface) const + march::SdoInterfacePtr sdo_interface, + UsbMaster usb_master) const { std::vector joints; for (const YAML::Node& joint_config : joints_config) @@ -309,8 +314,8 @@ std::vector HardwareBuilder::createJoints(const YAML::Node& joints { ROS_WARN("Joint %s is fixed in the URDF, but defined in the robot yaml", joint_name.c_str()); } - joints.push_back( - HardwareBuilder::createJoint(joint_config[joint_name], joint_name, urdf_joint, pdo_interface, sdo_interface)); + joints.push_back(HardwareBuilder::createJoint(joint_config[joint_name], joint_name, urdf_joint, pdo_interface, + sdo_interface, usb_master)); } for (const auto& urdf_joint : this->urdf_.joints_) @@ -338,24 +343,24 @@ std::string convertSWFileToString(std::ifstream& sw_file) /** * Returns the highest slave index of motor controllers and GESs in joints */ -int getMaxSlaveIndex(std::vector jointList) +int getMaxSlaveIndex(std::vector& jointList) { - int maxSlaveIndex = -1; + int maxSlaveIndex = -1; - for (march::Joint& joint : jointList) + for (march::Joint& joint : jointList) + { + int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); + if (temperatureSlaveIndex > maxSlaveIndex) { - int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); - if (temperatureSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = temperatureSlaveIndex; - } - - int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; - - if (motorControllerSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = motorControllerSlaveIndex; - } + maxSlaveIndex = temperatureSlaveIndex; } - return maxSlaveIndex; + + int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; + + if (motorControllerSlaveIndex > maxSlaveIndex) + { + maxSlaveIndex = motorControllerSlaveIndex; + } + } + return maxSlaveIndex; } From eafdcb0269ab04a970c9eac264f8f8d06f36bd3a Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 17:44:57 +0200 Subject: [PATCH 03/28] createOdrive and move odrive objects to march_hardware --- march_hardware/CMakeLists.txt | 32 +- march_hardware/config/march_odrive.json | 72 ++++ .../motor_controller/odrive/odrive.h | 85 ++++ .../motor_controller/odrive/odrive_endpoint.h | 187 +++++++++ .../motor_controller/odrive/odrive_enums.h | 76 ++++ .../motor_controller/odrive/odrive_motor.h | 29 ++ .../motor_controller/odrive/usb_master.h | 28 ++ march_hardware/package.xml | 4 + march_hardware/src/odrive/odrive.cpp | 389 ++++++++++++++++++ march_hardware/src/odrive/odrive_endpoint.cpp | 338 +++++++++++++++ march_hardware/src/odrive/odrive_motor.cpp | 31 ++ march_hardware/src/odrive/usb_master.cpp | 23 ++ march_hardware_builder/CMakeLists.txt | 40 +- .../march_hardware_builder/hardware_builder.h | 12 +- .../robots/odrive_test_joint_rotational.yaml | 16 +- .../src/hardware_builder.cpp | 30 +- 16 files changed, 1354 insertions(+), 38 deletions(-) create mode 100644 march_hardware/config/march_odrive.json create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/odrive.h create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h create mode 100644 march_hardware/src/odrive/odrive.cpp create mode 100644 march_hardware/src/odrive/odrive_endpoint.cpp create mode 100644 march_hardware/src/odrive/odrive_motor.cpp create mode 100644 march_hardware/src/odrive/usb_master.cpp diff --git a/march_hardware/CMakeLists.txt b/march_hardware/CMakeLists.txt index 23190f008..799f92e0c 100644 --- a/march_hardware/CMakeLists.txt +++ b/march_hardware/CMakeLists.txt @@ -20,6 +20,20 @@ catkin_package( ${PROJECT_NAME}-extras.cmake ) + +find_package(PkgConfig REQUIRED) + +pkg_check_modules(JSONCPP jsoncpp) +pkg_check_modules(LIBUSB1 libusb-1.0) + +catkin_package( + INCLUDE_DIRS include + CATKIN_DEPENDS message_runtime roscpp roslib +) + +link_libraries(${JSONCPP_LIBRARIES} ${LIBUSB1_LIBRARIES}) +include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) + include(cmake/${PROJECT_NAME}-extras.cmake) include_directories( @@ -62,6 +76,11 @@ add_library(${PROJECT_NAME} include/${PROJECT_NAME}/motor_controller/motor_controller_states.h include/${PROJECT_NAME}/joint.h include/${PROJECT_NAME}/march_robot.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_endpoint.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_enums.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_motor.h + include/${PROJECT_NAME}/motor_controller/odrive/usb_master.h include/${PROJECT_NAME}/power/boot_shutdown_offsets.h include/${PROJECT_NAME}/power/high_voltage.h include/${PROJECT_NAME}/power/low_voltage.h @@ -83,6 +102,10 @@ add_library(${PROJECT_NAME} src/imotioncube/imotioncube_target_state.cpp src/joint.cpp src/march_robot.cpp + src/odrive/odrive.cpp + src/odrive/odrive_endpoint.cpp + src/odrive/odrive_motor.cpp + src/odrive/usb_master.cpp src/power/high_voltage.cpp src/power/low_voltage.cpp src/power/power_distribution_board.cpp @@ -91,6 +114,13 @@ add_library(${PROJECT_NAME} target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} pthread) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + ${LIBUSB1_LIBRARIES} + ${JSONCPP_LIBRARIES} + yaml-cpp + ) + add_executable(slave_count_check check/slave_count.cpp) target_link_libraries(slave_count_check ${PROJECT_NAME}) ros_enable_rpath(slave_count_check) @@ -108,7 +138,7 @@ install(TARGETS slave_count_check RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} ) -install(DIRECTORY launch +install(DIRECTORY launch config DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} ) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json new file mode 100644 index 000000000..dd5398520 --- /dev/null +++ b/march_hardware/config/march_odrive.json @@ -0,0 +1,72 @@ +[ + { + "name":"axis0.motor.config.current_lim", + "type":"float", + "value":10.0 + }, + { + "name":"axis0.encoder.config.cpr", + "type":"uint32", + "value":4096 + }, + { + "name":"config.brake_resistance", + "type":"float", + "value":0.0 + }, + { + "name":"axis0.motor.config.pole_pairs", + "type":"uint32", + "value":21 + }, + { + "name":"axis0.motor.config.calibration_current", + "type":"float", + "value":10.0 + }, + { + "name":"axis0.controller.config.vel_limit", + "type":"float", + "value":5000.0 + }, + { + "name":"axis0.motor.config.motor_type", + "type":"uint8", + "value":0 + }, + { + "name":"axis0.controller.config.vel_gain", + "type":"float", + "value":0.01 + }, + { + "name":"axis0.controller.config.vel_integrator_gain", + "type":"float", + "value":0.05 + }, + { + "name":"axis0.controller.config.control_mode", + "type":"uint8", + "value":1 + }, + { + "name":"axis0.motor.config.direction", + "type":"uint8", + "value":1 + }, + { + "name":"axis0.sensorless_estimator.config.pm_flux_linkage", + "type":"float", + "value":0.001944723 + }, + { + "name":"axis0.config.watchdog_timeout", + "type":"float", + "value":0 + }, + { + "name":"axis1.config.watchdog_timeout", + "type":"float", + "value":0 + } +] diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h new file mode 100644 index 000000000..a833e640c --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -0,0 +1,85 @@ +#ifndef ODRIVE_HPP_ +#define ODRIVE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros/ros.h" +#include "odrive_endpoint.h" +#include "odrive_enums.h" + +typedef struct odrive_json_object +{ + int id; + std::string name; + std::string type; + std::string access; +} odrive_json_object; + +namespace march { + class Odrive { + public: + /** + * Initialize the odrive with specified axis + */ + Odrive(const std::string &axis_number, std::shared_ptr odrive_endpoint, + bool import_json = true); + + /** + * Check if given value type matched value type of odrive variable + */ + template + int validateType(const odrive_json_object &json_object, TT &value); + + /** + * Read parameter from the odrive object + */ + template + int read(const std::string ¶meter_name, TT &value); + + /** + * Write parameter to the odrive object + */ + template + int write(const std::string ¶meter_name, TT &value); + + /** + * Execute function on the odrive object + */ + int function(const std::string &function_name); + + /** + * Set configurations in the Json file to the Odrive + */ + int setConfigurations(const std::string &configuration_json_path); + + std::string axis_number; + + private: + int importOdriveJson(); + + int json_string_read(const Json::Value &json_parameter_object); + + int json_string_write(const Json::Value &json_parameter_object); + + static std::vector split_string(const std::string &str, char delimiter = '.'); + + odrive_json_object getJsonObject(const std::string ¶meter_name); + + Json::Value odrive_json_; + Json::Value odrive_configuration_json_; + + std::shared_ptr odrive_endpoint_; + }; +} // namespace march +#endif diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h new file mode 100644 index 000000000..5ed5f90de --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h @@ -0,0 +1,187 @@ +#ifndef ODRIVE_ENDPOINT_HPP_ +#define ODRIVE_ENDPOINT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ODrive Device Info +#define ODRIVE_USB_VENDORID 0x1209 +#define ODRIVE_USB_PRODUCTID 0x0D32 + +// ODrive USB Protocol +#define ODRIVE_TIMEOUT 200 +#define ODRIVE_MAX_BYTES_TO_RECEIVE 64 +#define ODRIVE_MAX_RESULT_LENGTH 100 +#define ODRIVE_DEFAULT_CRC_VALUE 0x7411 +#define ODRIVE_PROTOCOL_VERION 1 + +// ODrive Comm +#define ODRIVE_COMM_SUCCESS 0 +#define ODRIVE_COMM_ERROR 1 + +// Endpoints (from target) +#define CDC_IN_EP 0x81 /* EP1 for data IN (target) */ +#define CDC_OUT_EP 0x01 /* EP1 for data OUT (target) */ +#define CDC_CMD_EP 0x82 /* EP2 for CDC commands */ +#define ODRIVE_IN_EP 0x83 /* EP3 IN: ODrive device TX endpoint */ +#define ODRIVE_OUT_EP 0x03 /* EP3 OUT: ODrive device RX endpoint */ + +// CDC Endpoints parameters +#define CDC_DATA_HS_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_DATA_FS_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SIZE 8 /* Control Endpoint Packet size */ + +#define USB_CDC_CONFIG_DESC_SIZ (67 + 39) +#define CDC_DATA_HS_IN_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE +#define CDC_DATA_HS_OUT_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE + +#define CDC_DATA_FS_IN_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE +#define CDC_DATA_FS_OUT_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE + +#define CDC_SEND_ENCAPSULATED_COMMAND 0x00 +#define CDC_GET_ENCAPSULATED_RESPONSE 0x01 +#define CDC_SET_COMM_FEATURE 0x02 +#define CDC_GET_COMM_FEATURE 0x03 +#define CDC_CLEAR_COMM_FEATURE 0x04 +#define CDC_SET_LINE_CODING 0x20 +#define CDC_GET_LINE_CODING 0x21 +#define CDC_SET_CONTROL_LINE_STATE 0x22 +#define CDC_SEND_BREAK 0x23 + +typedef std::vector commBuffer; + +namespace march +{ +class OdriveEndpoint +{ +public: + /** + * initialize USB library and local variables + */ + OdriveEndpoint(); + + /** + * release USB library + */ + ~OdriveEndpoint(); + + /** + * enumerate ODrive hardware + * @param serialNumber odrive serial number + * @return ODRIVE_COMM_SUCCESS on success else ODRIVE_COMM_ERROR + */ + int open_connection(const std::string& serialNumber); + + /** + * close ODrive device + */ + void remove(); + + /** + * Read value from ODrive + * @param id odrive ID + * @param value Data read + * @return ODRIVE_COMM_SUCCESS on success else ODRIVE_COMM_ERROR + */ + template + int getData(int id, T& value); + + /** + * Write value to Odrive + * @param id odrive ID + * @param value Data to be written + * @return ODRIVE_COMM_SUCCESS on success + */ + template + int setData(int id, const TT& value); + + /** + * Request function to ODrive + * @param id odrive ID + * @return ODRIVE_COMM_SUCCESS on success + */ + int execFunc(int id); + + /** + * Request to USB endpoint + * @param handle USB device handler + * @param endpoint_id odrive ID + * @param received_payload receive buffer + * @param received_length receive length + * @param payload data read + * @param ack request acknowledge + * @param length data length + * @param read send read address + * @param address read address + * @return LIBUSB_SUCCESS on success + */ + int endpointRequest(int endpoint_id, commBuffer& received_payload, int& received_length, const commBuffer& payload, + bool ack, int length, bool read = false, int address = 0); + + /** + * Getter for the serial number + * @return the serial number of the odrive connection + */ + std::string getSerialNumber(); + + +private: + /** + * Append short data to data buffer + * @param buf data buffer + * @param value data to append + */ + static void appendShortToCommBuffer(commBuffer& buf, short value); + + /** + * Append int data to data buffer + * @param buf data buffer + * @param value data to append + */ + static void appendIntToCommBuffer(commBuffer& buf, int value); + + /** + * Decode odrive packet + * @param buf data buffer + * @param seq_no packet sequence number + * @param received_packet received buffer + * @return data buffer + */ + static commBuffer decodeODrivePacket(commBuffer& buf, short& seq_no); + + /** + * Read data buffer from Odrive hardware + * @param seq_no next sequence number + * @param endpoint_id USB endpoint ID + * @param response_size maximum data length to be read + * @param read append request address + * @param address destination address + * @param input data buffer to send + * @return data buffer read + */ + commBuffer createODrivePacket(short seq_no, int endpoint_id, short response_size, bool read, int address, + const commBuffer& input); + + libusb_context* lib_usb_context_ = nullptr; + libusb_device_handle* odrive_handle_ = nullptr; + + bool attached_to_handle_; + int crc_; + + int outbound_seq_no_ = 0; + std::string odrive_serial_number; + + std::mutex ep_lock_; +}; + +} // namespace march +#endif // ODRIVE_ENDPOINT_HPP_ diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h new file mode 100644 index 000000000..aa28e9652 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -0,0 +1,76 @@ +#ifndef ODRIVE_ENUMS_HPP_ +#define ODRIVE_ENUMS_HPP_ + + +// axis command names +#define PM_ENCODER_COUNTS ".encoder.shadow_count" +#define ODRIVE_INPUT_VOLTAGE "vbus_voltage" + +// axis states +#define AXIS_STATE_UNDEFINED 0 +#define AXIS_STATE_IDLE 1 +#define AXIS_STATE_STARTUP_SEQUENCE 2 +#define AXIS_STATE_FULL_CALIBRATION_SEQUENCE 3 +#define AXIS_STATE_MOTOR_CALIBRATION 4 +#define AXIS_STATE_SENSORLESS_CONTROL 5 +#define AXIS_STATE_ENCODER_INDEX_SEARCH 6 +#define AXIS_STATE_ENCODER_OFFSET_CALIBRATION 7 +#define AXIS_STATE_CLOSED_LOOP_CONTROL 8 +#define AXIS_STATE_LOCKIN_SPIN 9 +#define AXIS_STATE_ENCODER_DIR_FIND 10 + +#define ERROR_NONE 0x00 + +// class axis: +#define ERROR_INVALID_STATE 0x01 // + +#include "ros/ros.h" +#include "odrive.h" +#include "odrive_enums.h" +#include + +namespace march +{ +class OdriveMotor : public Odrive +{ +public: + OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode); + + int get_position(); + + float get_input_voltage(); + + int get_axis_error(); + +private: + ActuationMode mode; +}; + +} // namespace march +#endif // ODRIVE_INTERFACE_ODRIVE_MOTOR_H diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h new file mode 100644 index 000000000..9dea280c0 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h @@ -0,0 +1,28 @@ +// +// Created by roel on 16-08-20. +// + +#ifndef MARCH_HARDWARE_USB_MASTER_H +#define MARCH_HARDWARE_USB_MASTER_H + +#include "odrive_endpoint.h" +#include +#include +#include + +namespace march +{ +class UsbMaster +{ +public: + UsbMaster() = default; + std::shared_ptr getSerialConnection(const std::string& serial_number); + +private: + std::vector> odrive_endpoints; + + +}; + +} // namespace march +#endif //MARCH_HARDWARE_USB_MASTER_H diff --git a/march_hardware/package.xml b/march_hardware/package.xml index 1dae600ad..53f87be21 100644 --- a/march_hardware/package.xml +++ b/march_hardware/package.xml @@ -16,6 +16,10 @@ roscpp soem urdf + roslib + yaml-cpp + + message_runtime rosunit code_coverage diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp new file mode 100644 index 000000000..b85a4e0e8 --- /dev/null +++ b/march_hardware/src/odrive/odrive.cpp @@ -0,0 +1,389 @@ +#include "march_hardware/motor_controller/odrive/odrive.h" + +#include + +#define ODRIVE_OK 0; +#define ODRIVE_ERROR 1; + +namespace march +{ +Odrive::Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json) +{ + this->axis_number = axis_number; + this->odrive_endpoint_ = std::move(odrive_endpoint); + + if (import_json) + { + if (this->importOdriveJson()) + { + ROS_ERROR("Odrive %s error getting JSON", odrive_endpoint_->getSerialNumber().c_str()); + } + } +} + +std::vector Odrive::split_string(const std::string& string_name, char delimiter) +{ + std::vector split_string; + + std::stringstream ss(string_name); + std::string token; + + while (std::getline(ss, token, delimiter)) + { + split_string.push_back(token); + } + + return split_string; +} + +odrive_json_object Odrive::getJsonObject(const std::string& parameter_name) +{ + odrive_json_object json_object; + std::vector parameter_list = this->split_string(parameter_name); + + Json::Value odrive_json_member_object; + Json::Value odrive_json_member_list = this->odrive_json_; + + for (std::string& split_parameter_name : parameter_list) + { + for (auto& json_parameter : odrive_json_member_list) + { + if (json_parameter["name"].asCString() == split_parameter_name) + { + odrive_json_member_object = json_parameter; + odrive_json_member_list = json_parameter["members"]; + break; + } + } + } + + if (!odrive_json_member_object or odrive_json_member_object["name"].asCString() != parameter_list.back()) + { + json_object.id = -1; + ROS_ERROR("Odrive object with name %s is not found in parsed json file", parameter_name.c_str()); + } + else + { + json_object.id = odrive_json_member_object["id"].asInt(); + json_object.name = odrive_json_member_object["name"].asString(); + json_object.type = odrive_json_member_object["type"].asString(); + json_object.access = odrive_json_member_object["access"].asString(); + } + + return json_object; +} + +int Odrive::importOdriveJson() +{ + commBuffer rx; + commBuffer tx; + + int len; + int address = 0; + + std::string json; + + do + { + this->odrive_endpoint_->endpointRequest(0, rx, len, tx, true, 512, true, address); + address = address + len; + json.append((const char*)&rx[0], (size_t)len); + } while (len > 0); + + Json::Reader reader; + bool res = reader.parse(json, this->odrive_json_); + + if (!res) + { + return ODRIVE_ERROR; + } + return ODRIVE_OK; +} + +int Odrive::function(const std::string& function_name) +{ + odrive_json_object json_object = this->getJsonObject(function_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.type != "function") + { + ROS_ERROR("Error: Given parameter %s is not a function on the Odrive", function_name.c_str()); + return ODRIVE_ERROR; + } + + this->odrive_endpoint_->execFunc(json_object.id); + return 0; +} + +template +int Odrive::validateType(const odrive_json_object& json_object, TT& value) +{ + if (json_object.type == "float") + { + if (sizeof(value) != sizeof(float)) + { + ROS_ERROR("Error value for %s is not float", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint8") + { + if (sizeof(value) != sizeof(uint8_t)) + { + ROS_ERROR("Error value for %s is not uint8_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint16") + { + if (sizeof(value) != sizeof(uint16_t)) + { + ROS_ERROR("Error value for %s is not uint16_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint32") + { + if (sizeof(value) != sizeof(uint32_t)) + { + ROS_ERROR("Error value for %s is not uint32_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint64") + { + if (sizeof(value) != sizeof(uint64_t)) + { + ROS_ERROR("Error value for %s is not uint64_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "int32") + { + if (sizeof(value) != sizeof(int)) + { + ROS_ERROR("Error value for %s is not int", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "int16") + { + if (sizeof(value) != sizeof(short)) + { + ROS_ERROR("Error value for %s is not short", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "bool") + { + if (sizeof(value) != sizeof(bool)) + { + ROS_ERROR("Error value for %s is not bool", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else + { + ROS_ERROR("Error: invalid type for %s", json_object.name.c_str()); + return ODRIVE_ERROR; + } + + return ODRIVE_OK; +} + +template +int Odrive::read(const std::string& parameter_name, TT& value) +{ + odrive_json_object json_object = this->getJsonObject(parameter_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.access.find('r') == std::string::npos) + { + ROS_ERROR("Error: invalid read access for %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } + + if (this->validateType(json_object, value) == 1) + { + return ODRIVE_ERROR; + } + + return this->odrive_endpoint_->getData(json_object.id, value); +} + +template +int Odrive::write(const std::string& parameter_name, TT& value) +{ + odrive_json_object json_object = this->getJsonObject(parameter_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.access.find('w') == std::string::npos) + { + ROS_ERROR("Error: invalid read access for %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } + + if (this->validateType(json_object, value) == 1) + { + return ODRIVE_ERROR; + } + + return this->odrive_endpoint_->setData(json_object.id, value); +} + +int Odrive::json_string_read(const Json::Value& json_parameter_object) +{ + std::string parameter_name = json_parameter_object["name"].asString(); + std::string type_name = json_parameter_object["type"].asString(); + Json::Value value = json_parameter_object["value"]; + + if (type_name == "uint8") + { + uint8_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "uint16") + { + uint16_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "uint32") + { + uint32_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "float") + { + float casted_value = value.asFloat(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "bool") + { + bool casted_value = value.asBool(); + return this->read(parameter_name, casted_value); + } + else + { + ROS_ERROR("Error converting string for reading, invalid type %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } +} + +int Odrive::json_string_write(const Json::Value& json_parameter_object) +{ + std::string parameter_name = json_parameter_object["name"].asString(); + std::string type_name = json_parameter_object["type"].asString(); + Json::Value value = json_parameter_object["value"]; + + if (type_name == "uint8") + { + uint8_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "uint16") + { + uint16_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "uint32") + { + uint32_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "float") + { + float casted_value = value.asFloat(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "bool") + { + bool casted_value = value.asBool(); + return this->write(parameter_name, casted_value); + } + else + { + ROS_ERROR("Error converting string for writing, invalid type %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } +} + +int Odrive::setConfigurations(const std::string& configuration_json_path) +{ + std::ifstream cfg; + std::string line, json; + cfg.open(configuration_json_path, std::ios::in); + + if (cfg.is_open()) + { + while (getline(cfg, line)) + { + json.append(line); + } + } + cfg.close(); + + Json::Reader reader; + bool res = reader.parse(json, this->odrive_configuration_json_); + + if (!res) + { + ROS_INFO("Error parsing odrive configuration, error"); + return ODRIVE_ERROR; + } + + for (auto& parameter : this->odrive_configuration_json_) + { + ROS_INFO("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + int result = this->json_string_write(parameter); + + if (result != LIBUSB_SUCCESS) + { + ROS_INFO("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + continue; + } + + ROS_INFO("Setting succeeded %s", parameter["name"].asString().c_str()); + } + return ODRIVE_OK; +} + +template int Odrive::validateType(const odrive_json_object& json_object, uint8_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint16_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint32_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint64_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int&); +template int Odrive::validateType(const odrive_json_object& json_object, short&); +template int Odrive::validateType(const odrive_json_object& json_object, float&); +template int Odrive::validateType(const odrive_json_object& json_object, bool&); + +template int Odrive::read(const std::string& parameter_name, uint8_t&); +template int Odrive::read(const std::string& parameter_name, uint16_t&); +template int Odrive::read(const std::string& parameter_name, uint32_t&); +template int Odrive::read(const std::string& parameter_name, uint64_t&); +template int Odrive::read(const std::string& parameter_name, int&); +template int Odrive::read(const std::string& parameter_name, short&); +template int Odrive::read(const std::string& parameter_name, float&); +template int Odrive::read(const std::string& parameter_name, bool&); + +template int Odrive::write(const std::string& parameter_name, uint8_t&); +template int Odrive::write(const std::string& parameter_name, uint16_t&); +template int Odrive::write(const std::string& parameter_name, uint32_t&); +template int Odrive::write(const std::string& parameter_name, uint64_t&); +template int Odrive::write(const std::string& parameter_name, int&); +template int Odrive::write(const std::string& parameter_name, short&); +template int Odrive::write(const std::string& parameter_name, float&); +template int Odrive::write(const std::string& parameter_name, bool&); +} // namespace march + diff --git a/march_hardware/src/odrive/odrive_endpoint.cpp b/march_hardware/src/odrive/odrive_endpoint.cpp new file mode 100644 index 000000000..2e205b45f --- /dev/null +++ b/march_hardware/src/odrive/odrive_endpoint.cpp @@ -0,0 +1,338 @@ +#include "march_hardware/motor_controller/odrive/odrive.h" +#include "march_hardware/motor_controller/odrive/odrive_endpoint.h" + +using namespace std; + +namespace march +{ +OdriveEndpoint::OdriveEndpoint() +{ + if (libusb_init(&lib_usb_context_) != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive could not connect to endpoint USB."); + } +} + +OdriveEndpoint::~OdriveEndpoint() +{ + this->remove(); + + if (lib_usb_context_ != nullptr) + { + libusb_exit(lib_usb_context_); + lib_usb_context_ = nullptr; + } +} + +void OdriveEndpoint::remove() +{ + if (odrive_handle_ != nullptr) + { + libusb_release_interface(odrive_handle_, 2); + libusb_close(odrive_handle_); + odrive_handle_ = nullptr; + } +} + +int OdriveEndpoint::open_connection(const std::string& serial_number) +{ + libusb_device** usb_device_list; + + ssize_t device_count = libusb_get_device_list(lib_usb_context_, &usb_device_list); + if (device_count <= 0) + { + ROS_WARN("No attached odrives found"); + return ODRIVE_COMM_ERROR; + } + + for (size_t i = 0; i < size_t(device_count); ++i) + { + libusb_device* device = usb_device_list[i]; + libusb_device_descriptor desc = {}; + + int result = libusb_get_device_descriptor(device, &desc); + if (result != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error getting device descriptor", serial_number.c_str()); + continue; + } + + if (desc.idVendor == ODRIVE_USB_VENDORID && desc.idProduct == ODRIVE_USB_PRODUCTID) + { + libusb_device_handle* device_handle; + struct libusb_config_descriptor* config; + + this->attached_to_handle_ = false; + unsigned char buf[128]; + + if (libusb_open(device, &device_handle) != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error opening USB device", serial_number.c_str()); + continue; + } + + result = libusb_get_config_descriptor(device, 0, &config); + if (result != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error getting device descriptor", serial_number.c_str()); + continue; + } + + int ifNumber = 2; // config->bNumInterfaces; + + if ((libusb_kernel_driver_active(device_handle, ifNumber) != LIBUSB_SUCCESS) && + (libusb_detach_kernel_driver(device_handle, ifNumber) != LIBUSB_SUCCESS)) + { + libusb_close(device_handle); + + ROS_ERROR("Odrive %s driver error", serial_number.c_str()); + continue; + } + + if (libusb_claim_interface(device_handle, ifNumber) != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error claiming device", serial_number.c_str()); + libusb_close(device_handle); + continue; + } + else + { + result = libusb_get_string_descriptor_ascii(device_handle, desc.iSerialNumber, buf, 127); + if (result <= 0) + { + libusb_release_interface(device_handle, ifNumber); + libusb_close(device_handle); + + ROS_ERROR("Odrive %s error getting data", serial_number.c_str()); + continue; + } + else + { + std::stringstream stream; + stream << uppercase << std::hex << stoull(serial_number, nullptr, 16); + std::string sn(stream.str()); + + if (sn.compare(0, strlen((const char*)buf), (const char*)buf) == 0) + { + ROS_INFO("Odrive with serial number; %s found", serial_number.c_str()); + this->attached_to_handle_ = true; + this->odrive_serial_number = serial_number; + + odrive_handle_ = device_handle; + break; + } + } + if (!this->attached_to_handle_) + { + libusb_release_interface(device_handle, ifNumber); + libusb_close(device_handle); + } + } + } + } + + libusb_free_device_list(usb_device_list, 1); + + return ODRIVE_COMM_SUCCESS; +} + +template +int OdriveEndpoint::getData(int id, T& value) +{ + commBuffer tx; + commBuffer rx; + int rx_size; + + int result = this->endpointRequest(id, rx, rx_size, tx, true, sizeof(value)); + if (result != ODRIVE_COMM_SUCCESS) + { + return result; + } + + memcpy(&value, &rx[0], sizeof(value)); + + return ODRIVE_COMM_SUCCESS; +} + +template +int OdriveEndpoint::setData(int endpoint_id, const TT& value) +{ + commBuffer tx; + commBuffer rx; + int rx_length; + + for (int i = 0; i < int(sizeof(value)); i++) + { + tx.push_back(((unsigned char*)&value)[i]); + } + + return this->endpointRequest(endpoint_id, rx, rx_length, tx, true, 0); +} + +int OdriveEndpoint::execFunc(int endpoint_id) +{ + commBuffer tx; + commBuffer rx; + int rx_length; + + return this->endpointRequest(endpoint_id, rx, rx_length, tx, false, 0); +} + +int OdriveEndpoint::endpointRequest(int endpoint_id, commBuffer& received_payload, int& received_length, + const commBuffer& payload, bool ack, int length, bool read, int address) +{ + commBuffer send_buffer; + commBuffer receive_buffer; + + unsigned char receive_bytes[ODRIVE_MAX_RESULT_LENGTH] = { 0 }; + + int sent_bytes = 0; + int received_bytes = 0; + short received_seq_no = 0; + + this->ep_lock_.lock(); + + // Prepare sequence number + if (ack) + { + endpoint_id |= 0x8000; + } + outbound_seq_no_ = (outbound_seq_no_ + 1) & 0x7fff; + outbound_seq_no_ |= LIBUSB_ENDPOINT_IN; + short seq_no = outbound_seq_no_; + + // Create request packet + commBuffer packet = createODrivePacket(seq_no, endpoint_id, length, read, address, payload); + + // Transfer packet to target + int transfer_result = + libusb_bulk_transfer(odrive_handle_, ODRIVE_OUT_EP, packet.data(), packet.size(), &sent_bytes, ODRIVE_TIMEOUT); + if (transfer_result != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error in transferring data to USB, error id %i", this->odrive_serial_number.c_str(), + transfer_result); + + this->ep_lock_.unlock(); + return transfer_result; + } + + else if (int(packet.size()) != sent_bytes) + { + ROS_ERROR("Odrive %s error not all data transferring to USB was successful", this->odrive_serial_number.c_str()); + } + + // Get response + if (ack) + { + int ack_result = libusb_bulk_transfer(odrive_handle_, ODRIVE_IN_EP, receive_bytes, ODRIVE_MAX_BYTES_TO_RECEIVE, + &received_bytes, ODRIVE_TIMEOUT); + if (ack_result != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error in acknowledging response from USB, error id %i", this->odrive_serial_number.c_str(), + ack_result); + + this->ep_lock_.unlock(); + return ack_result; + } + + // Push received data to buffer + for (int i = 0; i < received_bytes; i++) + { + receive_buffer.push_back(receive_bytes[i]); + } + + received_payload = decodeODrivePacket(receive_buffer, received_seq_no); + if (received_seq_no != seq_no) + { + ROS_ERROR("Odrive %s error received data out of order", this->odrive_serial_number.c_str()); + } + received_length = received_payload.size(); + } + + this->ep_lock_.unlock(); + return LIBUSB_SUCCESS; +} + +std::string OdriveEndpoint::getSerialNumber() { + return this->odrive_serial_number; +} + +void OdriveEndpoint::appendShortToCommBuffer(commBuffer& buf, short value) +{ + buf.push_back((value >> 0) & 0xFF); + buf.push_back((value >> 8) & 0xFF); +} + +void OdriveEndpoint::appendIntToCommBuffer(commBuffer& buf, const int value) +{ + buf.push_back((value >> 0) & 0xFF); + buf.push_back((value >> 8) & 0xFF); + buf.push_back((value >> 16) & 0xFF); + buf.push_back((value >> 24) & 0xFF); +} + +commBuffer OdriveEndpoint::decodeODrivePacket(commBuffer& buf, short& seq_no) +{ + commBuffer payload; + + memcpy(&seq_no, &buf[0], sizeof(short)); + seq_no &= 0x7fff; + + for (commBuffer::size_type i = 2; i < buf.size(); ++i) + { + payload.push_back(buf[i]); + } + return payload; +} + +commBuffer OdriveEndpoint::createODrivePacket(short seq_no, int endpoint_id, short response_size, bool read, + int address, const commBuffer& input) +{ + commBuffer packet; + this->crc_ = 0; + + if ((endpoint_id & 0x7fff) == 0) + { + this->crc_ = ODRIVE_PROTOCOL_VERION; + } + else + { + this->crc_ = ODRIVE_DEFAULT_CRC_VALUE; + } + + appendShortToCommBuffer(packet, seq_no); + appendShortToCommBuffer(packet, endpoint_id); + appendShortToCommBuffer(packet, response_size); + if (read) + { + appendIntToCommBuffer(packet, address); + } + + for (uint8_t b : input) + { + packet.push_back(b); + } + + appendShortToCommBuffer(packet, this->crc_); + + return packet; +} + +template int OdriveEndpoint::getData(int, bool&); +template int OdriveEndpoint::getData(int, short&); +template int OdriveEndpoint::getData(int, int&); +template int OdriveEndpoint::getData(int, float&); +template int OdriveEndpoint::getData(int, uint8_t&); +template int OdriveEndpoint::getData(int, uint16_t&); +template int OdriveEndpoint::getData(int, uint32_t&); +template int OdriveEndpoint::getData(int, uint64_t&); + +template int OdriveEndpoint::setData(int, const bool&); +template int OdriveEndpoint::setData(int, const short&); +template int OdriveEndpoint::setData(int, const int&); +template int OdriveEndpoint::setData(int, const float&); +template int OdriveEndpoint::setData(int, const uint8_t&); +template int OdriveEndpoint::setData(int, const uint16_t&); +template int OdriveEndpoint::setData(int, const uint32_t&); +template int OdriveEndpoint::setData(int, const uint64_t&); +} // namespace march diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp new file mode 100644 index 000000000..9c25eb74a --- /dev/null +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -0,0 +1,31 @@ +#include "march_hardware/motor_controller/odrive/odrive_motor.h" + +#include + +namespace march +{ +OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, + ActuationMode mode) + : Odrive(axisNumber, std::move(odriveEndpoint), false) +{ + this->mode = mode; +} + +int OdriveMotor::get_position() +{ + int position; + std::string parameter_name = this->axis_number + PM_ENCODER_COUNTS; + + this->read(parameter_name, position); + return position; +} + +float OdriveMotor::get_input_voltage() +{ + float odrive_input_voltage; + + this->read(ODRIVE_INPUT_VOLTAGE, odrive_input_voltage); + return odrive_input_voltage; +} + +} // namespace march diff --git a/march_hardware/src/odrive/usb_master.cpp b/march_hardware/src/odrive/usb_master.cpp new file mode 100644 index 000000000..af1898b43 --- /dev/null +++ b/march_hardware/src/odrive/usb_master.cpp @@ -0,0 +1,23 @@ +// Copyright 2019 Project March. +#include "march_hardware/motor_controller/odrive/usb_master.h" +#include + + +namespace march +{ +std::shared_ptr UsbMaster::getSerialConnection(const std::string& serial_number) { + for (uint i = 0; i < this->odrive_endpoints.size(); ++i) { + if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0) { + return odrive_endpoints[i]; + } + } + + std::shared_ptr odrive_endpoint = std::make_shared(); + ROS_INFO("Open serial connection %s", serial_number); + odrive_endpoint->open_connection(serial_number); + + odrive_endpoints.push_back(odrive_endpoint); + odrive_endpoint->getSerialNumber(); + return odrive_endpoint; +} +} // namespace march diff --git a/march_hardware_builder/CMakeLists.txt b/march_hardware_builder/CMakeLists.txt index 12a6d119b..f4562d876 100644 --- a/march_hardware_builder/CMakeLists.txt +++ b/march_hardware_builder/CMakeLists.txt @@ -51,23 +51,23 @@ install(TARGETS ${PROJECT_NAME} ) ## Add gtest based cpp test target and link libraries -if(CATKIN_ENABLE_TESTING) - catkin_add_gtest(${PROJECT_NAME}_test - test/absolute_encoder_builder_test.cpp - test/allowed_robot_test.cpp - test/imc_builder_test.cpp - test/incremental_encoder_builder_test.cpp - test/joint_builder_test.cpp - test/pdb_builder_test.cpp - test/test_runner.cpp - ) - target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) - - if(ENABLE_COVERAGE_TESTING) - set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*") - add_code_coverage( - NAME coverage_report - DEPENDENCIES ${PROJECT_NAME}_test - ) - endif() -endif() +#if(CATKIN_ENABLE_TESTING) +# catkin_add_gtest(${PROJECT_NAME}_test +# test/absolute_encoder_builder_test.cpp +# test/allowed_robot_test.cpp +# test/imc_builder_test.cpp +# test/incremental_encoder_builder_test.cpp +# test/joint_builder_test.cpp +# test/pdb_builder_test.cpp +# test/test_runner.cpp +# ) +# target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) +# +# if(ENABLE_COVERAGE_TESTING) +# set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*") +# add_code_coverage( +# NAME coverage_report +# DEPENDENCIES ${PROJECT_NAME}_test +# ) +# endif() +#endif() diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index 6fc48b932..707f14fcd 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -21,8 +21,8 @@ #include #include -#include -#include +#include +#include /** * @brief Creates a MarchRobot from a robot yaml and URDF. @@ -71,7 +71,7 @@ class HardwareBuilder static march::Joint createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface, UsbMaster usb_master); + march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master); static std::unique_ptr createAbsoluteEncoder(const YAML::Node& absolute_encoder_config, const urdf::JointConstSharedPtr& urdf_joint); static std::unique_ptr @@ -89,12 +89,12 @@ class HardwareBuilder static std::unique_ptr createOdrive(const YAML::Node& imc_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, - march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface); + march::UsbMaster usb_master); static const std::vector INCREMENTAL_ENCODER_REQUIRED_KEYS; static const std::vector ABSOLUTE_ENCODER_REQUIRED_KEYS; static const std::vector IMOTIONCUBE_REQUIRED_KEYS; + static const std::vector ODRIVE_REQUIRED_KEYS; static const std::vector TEMPERATUREGES_REQUIRED_KEYS; static const std::vector POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS; static const std::vector JOINT_REQUIRED_KEYS; @@ -113,7 +113,7 @@ class HardwareBuilder * @return list of created joints */ std::vector createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface, UsbMaster usb_master) const; + march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master) const; YAML::Node robot_config_; urdf::Model urdf_; diff --git a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml index c8f09231d..525c4ec15 100644 --- a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml +++ b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml @@ -7,12 +7,12 @@ testsetup: actuationMode: torque allowActuation: true odrive: - serial_number: 0x2084387E304E + serial_number: "0x2084387E304E" axis: "axis0" - absoluteEncoder: - resolution: 17 - minPositionIU: 97242 - maxPositionIU: 119617 - incrementalEncoder: - resolution: 12 - transmission: 101 + absoluteEncoder: + resolution: 17 + minPositionIU: 97242 + maxPositionIU: 119617 + incrementalEncoder: + resolution: 12 + transmission: 101 diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index e5a1b5922..7cd28683c 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -27,6 +27,10 @@ const std::vector HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS = { "slaveIndex", "incrementalEncoder", "absoluteEncoder" }; +const std::vector HardwareBuilder::ODRIVE_REQUIRED_KEYS = + { + "serial_number", "axis", "incrementalEncoder", "absoluteEncoder" + }; const std::vector HardwareBuilder::TEMPERATUREGES_REQUIRED_KEYS = { "slaveIndex", "byteOffset" }; const std::vector HardwareBuilder::POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS = { @@ -68,7 +72,7 @@ std::unique_ptr HardwareBuilder::createMarchRobot() auto pdo_interface = march::PdoInterfaceImpl::create(); auto sdo_interface = march::SdoInterfaceImpl::create(); - auto usb_master = UsbMaster(); + auto usb_master = march::UsbMaster(); std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface, usb_master); @@ -94,7 +98,7 @@ std::unique_ptr HardwareBuilder::createMarchRobot() march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, - UsbMaster usb_master) + march::UsbMaster usb_master) { ROS_DEBUG("Starting creation of joint %s", joint_name.c_str()); if (!urdf_joint) @@ -146,6 +150,26 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const return { joint_name, net_number, allow_actuation, std::move(controller), std::move(ges) }; } +std::unique_ptr HardwareBuilder::createOdrive(const YAML::Node& odrive_config, march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::UsbMaster usb_master) +{ + if (!odrive_config || !urdf_joint) + { + return nullptr; + } + + HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS, "odrive"); + + YAML::Node incremental_encoder_config = odrive_config["incrementalEncoder"]; + YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; + std::string axis = odrive_config["axis"].as(); + std::string serial_number = odrive_config["serial_number"].as(); + auto odrive_endpoint = usb_master.getSerialConnection(serial_number); + + return std::make_unique(axis, odrive_endpoint, mode); +} + std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, @@ -303,7 +327,7 @@ void HardwareBuilder::initUrdf() std::vector HardwareBuilder::createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, - UsbMaster usb_master) const + march::UsbMaster usb_master) const { std::vector joints; for (const YAML::Node& joint_config : joints_config) From 8f01c1159de6959973b8854d775def034da264e3 Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 19:54:04 +0200 Subject: [PATCH 04/28] Fix tests and cmake --- march_hardware/CMakeLists.txt | 238 +++++++++--------- march_hardware_builder/CMakeLists.txt | 40 +-- .../test/joint_builder_test.cpp | 32 ++- 3 files changed, 159 insertions(+), 151 deletions(-) diff --git a/march_hardware/CMakeLists.txt b/march_hardware/CMakeLists.txt index 799f92e0c..af1089498 100644 --- a/march_hardware/CMakeLists.txt +++ b/march_hardware/CMakeLists.txt @@ -4,43 +4,37 @@ project(march_hardware) add_compile_options(-std=c++14 -Wall -Wextra -Werror) find_package(catkin REQUIRED COMPONENTS - roscpp - soem - urdf -) + roscpp + soem + urdf + ) +find_package(PkgConfig REQUIRED) catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - soem - urdf - LIBRARIES ${PROJECT_NAME} - CFG_EXTRAS - ${PROJECT_NAME}-extras.cmake + INCLUDE_DIRS include + CATKIN_DEPENDS + message_runtime + roscpp + roslib + soem + urdf + LIBRARIES ${PROJECT_NAME} + CFG_EXTRAS + ${PROJECT_NAME}-extras.cmake ) - -find_package(PkgConfig REQUIRED) - pkg_check_modules(JSONCPP jsoncpp) pkg_check_modules(LIBUSB1 libusb-1.0) -catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS message_runtime roscpp roslib -) - link_libraries(${JSONCPP_LIBRARIES} ${LIBUSB1_LIBRARIES}) -include_directories(include SYSTEM ${catkin_INCLUDE_DIRS}) include(cmake/${PROJECT_NAME}-extras.cmake) include_directories( - include - SYSTEM - ${catkin_INCLUDE_DIRS} - ${soem_INCLUDE_DIRS}/soem + include + SYSTEM + ${catkin_INCLUDE_DIRS} + ${soem_INCLUDE_DIRS}/soem ) # needed to circumvent LD_LIBRARY_PATH being emptied through ethercat_grant @@ -55,130 +49,128 @@ if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) endif() add_library(${PROJECT_NAME} - include/${PROJECT_NAME}/encoder/absolute_encoder.h - include/${PROJECT_NAME}/encoder/encoder.h - include/${PROJECT_NAME}/encoder/incremental_encoder.h - include/${PROJECT_NAME}/error/error_type.h - include/${PROJECT_NAME}/error/hardware_exception.h - include/${PROJECT_NAME}/error/motion_error.h - include/${PROJECT_NAME}/ethercat/ethercat_master.h - include/${PROJECT_NAME}/ethercat/pdo_interface.h - include/${PROJECT_NAME}/ethercat/pdo_map.h - include/${PROJECT_NAME}/ethercat/pdo_types.h - include/${PROJECT_NAME}/ethercat/sdo_interface.h - include/${PROJECT_NAME}/ethercat/slave.h - include/${PROJECT_NAME}/motor_controller/actuation_mode.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h - include/${PROJECT_NAME}/motor_controller/motor_controller.h - include/${PROJECT_NAME}/motor_controller/motor_controller_states.h - include/${PROJECT_NAME}/joint.h - include/${PROJECT_NAME}/march_robot.h - include/${PROJECT_NAME}/motor_controller/odrive/odrive.h - include/${PROJECT_NAME}/motor_controller/odrive/odrive_endpoint.h - include/${PROJECT_NAME}/motor_controller/odrive/odrive_enums.h - include/${PROJECT_NAME}/motor_controller/odrive/odrive_motor.h - include/${PROJECT_NAME}/motor_controller/odrive/usb_master.h - include/${PROJECT_NAME}/power/boot_shutdown_offsets.h - include/${PROJECT_NAME}/power/high_voltage.h - include/${PROJECT_NAME}/power/low_voltage.h - include/${PROJECT_NAME}/power/net_driver_offsets.h - include/${PROJECT_NAME}/power/net_monitor_offsets.h - include/${PROJECT_NAME}/power/power_distribution_board.h - include/${PROJECT_NAME}/temperature/temperature_ges.h - include/${PROJECT_NAME}/temperature/temperature_sensor.h - src/encoder/absolute_encoder.cpp - src/encoder/encoder.cpp - src/encoder/incremental_encoder.cpp - src/error/error_type.cpp - src/error/motion_error.cpp - src/ethercat/ethercat_master.cpp - src/ethercat/pdo_interface.cpp - src/ethercat/pdo_map.cpp - src/ethercat/sdo_interface.cpp - src/imotioncube/imotioncube.cpp - src/imotioncube/imotioncube_target_state.cpp - src/joint.cpp - src/march_robot.cpp - src/odrive/odrive.cpp - src/odrive/odrive_endpoint.cpp - src/odrive/odrive_motor.cpp - src/odrive/usb_master.cpp - src/power/high_voltage.cpp - src/power/low_voltage.cpp - src/power/power_distribution_board.cpp - src/temperature/temperature_ges.cpp -) - -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} pthread) + include/${PROJECT_NAME}/encoder/absolute_encoder.h + include/${PROJECT_NAME}/encoder/encoder.h + include/${PROJECT_NAME}/encoder/incremental_encoder.h + include/${PROJECT_NAME}/error/error_type.h + include/${PROJECT_NAME}/error/hardware_exception.h + include/${PROJECT_NAME}/error/motion_error.h + include/${PROJECT_NAME}/ethercat/ethercat_master.h + include/${PROJECT_NAME}/ethercat/pdo_interface.h + include/${PROJECT_NAME}/ethercat/pdo_map.h + include/${PROJECT_NAME}/ethercat/pdo_types.h + include/${PROJECT_NAME}/ethercat/sdo_interface.h + include/${PROJECT_NAME}/ethercat/slave.h + include/${PROJECT_NAME}/motor_controller/actuation_mode.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h + include/${PROJECT_NAME}/motor_controller/motor_controller.h + include/${PROJECT_NAME}/motor_controller/motor_controller_states.h + include/${PROJECT_NAME}/joint.h + include/${PROJECT_NAME}/march_robot.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_endpoint.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_enums.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_motor.h + include/${PROJECT_NAME}/motor_controller/odrive/usb_master.h + include/${PROJECT_NAME}/power/boot_shutdown_offsets.h + include/${PROJECT_NAME}/power/high_voltage.h + include/${PROJECT_NAME}/power/low_voltage.h + include/${PROJECT_NAME}/power/net_driver_offsets.h + include/${PROJECT_NAME}/power/net_monitor_offsets.h + include/${PROJECT_NAME}/power/power_distribution_board.h + include/${PROJECT_NAME}/temperature/temperature_ges.h + include/${PROJECT_NAME}/temperature/temperature_sensor.h + src/encoder/absolute_encoder.cpp + src/encoder/encoder.cpp + src/encoder/incremental_encoder.cpp + src/error/error_type.cpp + src/error/motion_error.cpp + src/ethercat/ethercat_master.cpp + src/ethercat/pdo_interface.cpp + src/ethercat/pdo_map.cpp + src/ethercat/sdo_interface.cpp + src/imotioncube/imotioncube.cpp + src/imotioncube/imotioncube_target_state.cpp + src/joint.cpp + src/march_robot.cpp + src/odrive/odrive.cpp + src/odrive/odrive_endpoint.cpp + src/odrive/odrive_motor.cpp + src/odrive/usb_master.cpp + src/power/high_voltage.cpp + src/power/low_voltage.cpp + src/power/power_distribution_board.cpp + src/temperature/temperature_ges.cpp + ) target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} + pthread ${LIBUSB1_LIBRARIES} ${JSONCPP_LIBRARIES} - yaml-cpp - ) + yaml-cpp) add_executable(slave_count_check check/slave_count.cpp) target_link_libraries(slave_count_check ${PROJECT_NAME}) ros_enable_rpath(slave_count_check) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) install(TARGETS slave_count_check - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) install(DIRECTORY launch config - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) ## Add gtest based cpp test target and link libraries if(CATKIN_ENABLE_TESTING) catkin_add_gmock(${PROJECT_NAME}_test - test/encoder/absolute_encoder_test.cpp - test/encoder/encoder_test.cpp - test/encoder/incremental_encoder_test.cpp - test/error/hardware_exception_test.cpp - test/error/motion_error_test.cpp - test/ethercat/pdo_map_test.cpp - test/ethercat/slave_test.cpp - test/imotioncube/imotioncube_test.cpp - test/joint_test.cpp - test/mocks/mock_absolute_encoder.h - test/mocks/mock_encoder.h - test/mocks/mock_motor_controller.h - test/mocks/mock_incremental_encoder.h - test/mocks/mock_joint.h - test/mocks/mock_pdo_interface.h - test/mocks/mock_sdo_interface.h - test/mocks/mock_slave.h - test/mocks/mock_temperature_ges.h - test/power/boot_shutdown_offsets_test.cpp - test/power/high_voltage_test.cpp - test/power/low_voltage_test.cpp - test/power/net_driver_offsets_test.cpp - test/power/net_monitor_offsets_test.cpp - test/power/power_distribution_board_test.cpp - test/temperature/temperature_ges_test.cpp - test/test_runner.cpp - ) + test/encoder/absolute_encoder_test.cpp + test/encoder/encoder_test.cpp + test/encoder/incremental_encoder_test.cpp + test/error/hardware_exception_test.cpp + test/error/motion_error_test.cpp + test/ethercat/pdo_map_test.cpp + test/ethercat/slave_test.cpp + test/imotioncube/imotioncube_test.cpp + test/joint_test.cpp + test/mocks/mock_absolute_encoder.h + test/mocks/mock_encoder.h + test/mocks/mock_motor_controller.h + test/mocks/mock_incremental_encoder.h + test/mocks/mock_joint.h + test/mocks/mock_pdo_interface.h + test/mocks/mock_sdo_interface.h + test/mocks/mock_slave.h + test/mocks/mock_temperature_ges.h + test/power/boot_shutdown_offsets_test.cpp + test/power/high_voltage_test.cpp + test/power/low_voltage_test.cpp + test/power/net_driver_offsets_test.cpp + test/power/net_monitor_offsets_test.cpp + test/power/power_distribution_board_test.cpp + test/temperature/temperature_ges_test.cpp + test/test_runner.cpp + ) target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) if(ENABLE_COVERAGE_TESTING) set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*" "*/${PROJECT_NAME}/check/*") add_code_coverage( - NAME coverage_report - DEPENDENCIES ${PROJECT_NAME}_test + NAME coverage_report + DEPENDENCIES ${PROJECT_NAME}_test ) endif() endif() diff --git a/march_hardware_builder/CMakeLists.txt b/march_hardware_builder/CMakeLists.txt index f4562d876..12a6d119b 100644 --- a/march_hardware_builder/CMakeLists.txt +++ b/march_hardware_builder/CMakeLists.txt @@ -51,23 +51,23 @@ install(TARGETS ${PROJECT_NAME} ) ## Add gtest based cpp test target and link libraries -#if(CATKIN_ENABLE_TESTING) -# catkin_add_gtest(${PROJECT_NAME}_test -# test/absolute_encoder_builder_test.cpp -# test/allowed_robot_test.cpp -# test/imc_builder_test.cpp -# test/incremental_encoder_builder_test.cpp -# test/joint_builder_test.cpp -# test/pdb_builder_test.cpp -# test/test_runner.cpp -# ) -# target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) -# -# if(ENABLE_COVERAGE_TESTING) -# set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*") -# add_code_coverage( -# NAME coverage_report -# DEPENDENCIES ${PROJECT_NAME}_test -# ) -# endif() -#endif() +if(CATKIN_ENABLE_TESTING) + catkin_add_gtest(${PROJECT_NAME}_test + test/absolute_encoder_builder_test.cpp + test/allowed_robot_test.cpp + test/imc_builder_test.cpp + test/incremental_encoder_builder_test.cpp + test/joint_builder_test.cpp + test/pdb_builder_test.cpp + test/test_runner.cpp + ) + target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) + + if(ENABLE_COVERAGE_TESTING) + set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*") + add_code_coverage( + NAME coverage_report + DEPENDENCIES ${PROJECT_NAME}_test + ) + endif() +endif() diff --git a/march_hardware_builder/test/joint_builder_test.cpp b/march_hardware_builder/test/joint_builder_test.cpp index 3d22b4ea0..c5ca9190e 100644 --- a/march_hardware_builder/test/joint_builder_test.cpp +++ b/march_hardware_builder/test/joint_builder_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#include class JointBuilderTest : public ::testing::Test { @@ -45,9 +46,10 @@ TEST_F(JointBuilderTest, ValidJointHip) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; + auto usb_master = march::UsbMaster(); const std::string name = "test_joint_hip"; march::Joint created = - HardwareBuilder::createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface); + HardwareBuilder::createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface, usb_master); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -70,8 +72,10 @@ TEST_F(JointBuilderTest, ValidNotActuated) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; + + auto usb_master = march::UsbMaster(); march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); + HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface, usb_master); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -90,16 +94,20 @@ TEST_F(JointBuilderTest, NoActuate) { YAML::Node config = this->loadTestYaml("/joint_no_actuate.yaml"); + auto usb_master = march::UsbMaster(); + ASSERT_THROW(HardwareBuilder::createJoint(config, "test_joint_no_actuate", this->joint, this->pdo_interface, - this->sdo_interface), + this->sdo_interface, usb_master), MissingKeyException); } TEST_F(JointBuilderTest, NoIMotionCube) { + + auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_no_imotioncube.yaml"); march::Joint joint = HardwareBuilder::createJoint(config, "test_joint_no_imotioncube", this->joint, - this->pdo_interface, this->sdo_interface); + this->pdo_interface, this->sdo_interface, usb_master); ASSERT_FALSE(joint.hasMotorController()); } @@ -112,8 +120,10 @@ TEST_F(JointBuilderTest, NoTemperatureGES) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 0.15; + auto usb_master = march::UsbMaster(); + ASSERT_NO_THROW(HardwareBuilder::createJoint(config, "test_joint_no_temperature_ges", this->joint, - this->pdo_interface, this->sdo_interface)); + this->pdo_interface, this->sdo_interface, usb_master)); } TEST_F(JointBuilderTest, ValidActuationMode) @@ -124,8 +134,10 @@ TEST_F(JointBuilderTest, ValidActuationMode) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; + + auto usb_master = march::UsbMaster(); march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); + HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface, usb_master); march::Joint expected("test_joint_hip", -1, false, std::make_unique( @@ -141,14 +153,18 @@ TEST_F(JointBuilderTest, ValidActuationMode) TEST_F(JointBuilderTest, EmptyJoint) { YAML::Node config; + + auto usb_master = march::UsbMaster(); ASSERT_THROW( - HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface), + HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface, usb_master), MissingKeyException); } TEST_F(JointBuilderTest, NoUrdfJoint) { + + auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_correct.yaml"); - ASSERT_THROW(HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface), + ASSERT_THROW(HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface, usb_master), march::error::HardwareException); } From f58ea511e71663140c8e46fb133a854ac0af19fb Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 20:24:31 +0200 Subject: [PATCH 05/28] Create Motor Controller interface methods --- .../motor_controller/odrive/odrive.h | 89 +++++++------- .../motor_controller/odrive/odrive_endpoint.h | 11 +- .../motor_controller/odrive/odrive_enums.h | 34 +++--- .../motor_controller/odrive/odrive_motor.h | 21 +++- .../motor_controller/odrive/usb_master.h | 10 +- march_hardware/src/odrive/odrive.cpp | 69 ++++++++--- march_hardware/src/odrive/odrive_endpoint.cpp | 17 ++- march_hardware/src/odrive/odrive_motor.cpp | 114 ++++++++++++++++-- march_hardware/src/odrive/usb_master.cpp | 26 ++-- .../march_hardware_builder/hardware_builder.h | 2 +- .../src/hardware_builder.cpp | 38 +++--- .../test/joint_builder_test.cpp | 23 ++-- 12 files changed, 306 insertions(+), 148 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h index a833e640c..14fd197f7 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -17,7 +17,13 @@ #include "ros/ros.h" #include "odrive_endpoint.h" #include "odrive_enums.h" +#include "march_hardware/motor_controller/motor_controller.h" +#define ODRIVE_OK 0; +#define ODRIVE_ERROR 1; + +namespace march +{ typedef struct odrive_json_object { int id; @@ -26,60 +32,59 @@ typedef struct odrive_json_object std::string access; } odrive_json_object; -namespace march { - class Odrive { - public: - /** - * Initialize the odrive with specified axis - */ - Odrive(const std::string &axis_number, std::shared_ptr odrive_endpoint, - bool import_json = true); +class Odrive +{ +public: + /** + * Initialize the odrive with specified axis + */ + Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json = true); - /** - * Check if given value type matched value type of odrive variable - */ - template - int validateType(const odrive_json_object &json_object, TT &value); + /** + * Check if given value type matched value type of odrive variable + */ + template + int validateType(const odrive_json_object& json_object, TT& value); - /** - * Read parameter from the odrive object - */ - template - int read(const std::string ¶meter_name, TT &value); + /** + * Read parameter from the odrive object + */ + template + int read(const std::string& parameter_name, TT& value); - /** - * Write parameter to the odrive object - */ - template - int write(const std::string ¶meter_name, TT &value); + /** + * Write parameter to the odrive object + */ + template + int write(const std::string& parameter_name, TT& value); - /** - * Execute function on the odrive object - */ - int function(const std::string &function_name); + /** + * Execute function on the odrive object + */ + int function(const std::string& function_name); - /** - * Set configurations in the Json file to the Odrive - */ - int setConfigurations(const std::string &configuration_json_path); + /** + * Set configurations in the Json file to the Odrive + */ + int setConfigurations(const std::string& configuration_json_path); - std::string axis_number; + std::string axis_number; - private: - int importOdriveJson(); +private: + int importOdriveJson(); - int json_string_read(const Json::Value &json_parameter_object); + int json_string_read(const Json::Value& json_parameter_object); - int json_string_write(const Json::Value &json_parameter_object); + int json_string_write(const Json::Value& json_parameter_object); - static std::vector split_string(const std::string &str, char delimiter = '.'); + static std::vector split_string(const std::string& str, char delimiter = '.'); - odrive_json_object getJsonObject(const std::string ¶meter_name); + odrive_json_object getJsonObject(const std::string& parameter_name); - Json::Value odrive_json_; - Json::Value odrive_configuration_json_; + Json::Value odrive_json_; + Json::Value odrive_configuration_json_; - std::shared_ptr odrive_endpoint_; - }; + std::shared_ptr odrive_endpoint_; +}; } // namespace march #endif diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h index 5ed5f90de..b5ecc8449 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h @@ -127,12 +127,11 @@ class OdriveEndpoint int endpointRequest(int endpoint_id, commBuffer& received_payload, int& received_length, const commBuffer& payload, bool ack, int length, bool read = false, int address = 0); - /** - * Getter for the serial number - * @return the serial number of the odrive connection - */ - std::string getSerialNumber(); - + /** + * Getter for the serial number + * @return the serial number of the odrive connection + */ + std::string getSerialNumber(); private: /** diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h index aa28e9652..0dd2510d4 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -1,23 +1,29 @@ #ifndef ODRIVE_ENUMS_HPP_ #define ODRIVE_ENUMS_HPP_ - // axis command names -#define PM_ENCODER_COUNTS ".encoder.shadow_count" -#define ODRIVE_INPUT_VOLTAGE "vbus_voltage" +#define O_PM_REQUEST_STATE ".requested_state" +#define O_PM_CURRENT_STATE ".current_state" +#define O_PM_ENCODER_POSITION_UI ".encoder.pos_estimate" +#define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate" +#define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage" +#define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" // axis states -#define AXIS_STATE_UNDEFINED 0 -#define AXIS_STATE_IDLE 1 -#define AXIS_STATE_STARTUP_SEQUENCE 2 -#define AXIS_STATE_FULL_CALIBRATION_SEQUENCE 3 -#define AXIS_STATE_MOTOR_CALIBRATION 4 -#define AXIS_STATE_SENSORLESS_CONTROL 5 -#define AXIS_STATE_ENCODER_INDEX_SEARCH 6 -#define AXIS_STATE_ENCODER_OFFSET_CALIBRATION 7 -#define AXIS_STATE_CLOSED_LOOP_CONTROL 8 -#define AXIS_STATE_LOCKIN_SPIN 9 -#define AXIS_STATE_ENCODER_DIR_FIND 10 +enum States : int +{ + AXIS_STATE_UNDEFINED = 0, + AXIS_STATE_IDLE = 1, + AXIS_STATE_STARTUP_SEQUENCE = 2, + AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, + AXIS_STATE_MOTOR_CALIBRATION = 4, + AXIS_STATE_SENSORLESS_CONTROL = 5, + AXIS_STATE_ENCODER_INDEX_SEARCH = 6, + AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, + AXIS_STATE_CLOSED_LOOP_CONTROL = 8, + AXIS_STATE_LOCKIN_SPIN = 9, + AXIS_STATE_ENCODER_DIR_FIND = 10 +}; #define ERROR_NONE 0x00 diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index 4349acfef..856f43f83 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -2,12 +2,17 @@ #define ODRIVE_INTERFACE_ODRIVE_MOTOR_H #include +#include #include "ros/ros.h" #include "odrive.h" #include "odrive_enums.h" #include +static constexpr double PI_2 = 2 * M_PI; +static constexpr double MOTOR_KV = 100; +static constexpr double CURRENT_TO_TORQUE_CONVERSION = 8.27; + namespace march { class OdriveMotor : public Odrive @@ -15,13 +20,23 @@ class OdriveMotor : public Odrive public: OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode); - int get_position(); + int initialize(); + + int setState(uint8_t state); + int getState(); + + float getMotorControllerVoltage(); + float getMotorCurrent(); + float getTorque(); - float get_input_voltage(); + float getAngleRadAbsolute(); + float getVelocityRadAbsolute(); - int get_axis_error(); + float getAngleRadIncremental(); + float getVelocityRadIncremental(); private: + std::string create_command(std::string command_name); ActuationMode mode; }; diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h index 9dea280c0..be414e184 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h @@ -15,14 +15,12 @@ namespace march class UsbMaster { public: - UsbMaster() = default; - std::shared_ptr getSerialConnection(const std::string& serial_number); + UsbMaster() = default; + std::shared_ptr getSerialConnection(const std::string& serial_number); private: - std::vector> odrive_endpoints; - - + std::vector> odrive_endpoints; }; } // namespace march -#endif //MARCH_HARDWARE_USB_MASTER_H +#endif // MARCH_HARDWARE_USB_MASTER_H diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp index b85a4e0e8..d7c1a5bb8 100644 --- a/march_hardware/src/odrive/odrive.cpp +++ b/march_hardware/src/odrive/odrive.cpp @@ -1,10 +1,5 @@ #include "march_hardware/motor_controller/odrive/odrive.h" -#include - -#define ODRIVE_OK 0; -#define ODRIVE_ERROR 1; - namespace march { Odrive::Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json) @@ -162,11 +157,11 @@ int Odrive::validateType(const odrive_json_object& json_object, TT& value) return ODRIVE_ERROR; } } - else if (json_object.type == "int32") + else if (json_object.type == "int8") { - if (sizeof(value) != sizeof(int)) + if (sizeof(value) != sizeof(short)) { - ROS_ERROR("Error value for %s is not int", json_object.name.c_str()); + ROS_ERROR("Error value for %s is not short", json_object.name.c_str()); return ODRIVE_ERROR; } } @@ -178,6 +173,15 @@ int Odrive::validateType(const odrive_json_object& json_object, TT& value) return ODRIVE_ERROR; } } + else if (json_object.type == "int32") + { + if (sizeof(value) != sizeof(int)) + { + ROS_ERROR("Error value for %s is not int", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "bool") { if (sizeof(value) != sizeof(bool)) @@ -264,6 +268,21 @@ int Odrive::json_string_read(const Json::Value& json_parameter_object) uint32_t casted_value = value.asUInt(); return this->read(parameter_name, casted_value); } + else if (type_name == "int8") + { + int8_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "int16") + { + int16_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "int32") + { + int32_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } else if (type_name == "float") { float casted_value = value.asFloat(); @@ -302,6 +321,21 @@ int Odrive::json_string_write(const Json::Value& json_parameter_object) uint32_t casted_value = value.asUInt(); return this->write(parameter_name, casted_value); } + else if (type_name == "int8") + { + int8_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "int16") + { + int16_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "int32") + { + int32_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } else if (type_name == "float") { float casted_value = value.asFloat(); @@ -359,31 +393,36 @@ int Odrive::setConfigurations(const std::string& configuration_json_path) return ODRIVE_OK; } +template int Odrive::validateType(const odrive_json_object& json_object, int8_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int16_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int32_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int64_t&); template int Odrive::validateType(const odrive_json_object& json_object, uint8_t&); template int Odrive::validateType(const odrive_json_object& json_object, uint16_t&); template int Odrive::validateType(const odrive_json_object& json_object, uint32_t&); template int Odrive::validateType(const odrive_json_object& json_object, uint64_t&); -template int Odrive::validateType(const odrive_json_object& json_object, int&); -template int Odrive::validateType(const odrive_json_object& json_object, short&); template int Odrive::validateType(const odrive_json_object& json_object, float&); template int Odrive::validateType(const odrive_json_object& json_object, bool&); +template int Odrive::read(const std::string& parameter_name, int8_t&); +template int Odrive::read(const std::string& parameter_name, int16_t&); +template int Odrive::read(const std::string& parameter_name, int32_t&); +template int Odrive::read(const std::string& parameter_name, int64_t&); template int Odrive::read(const std::string& parameter_name, uint8_t&); template int Odrive::read(const std::string& parameter_name, uint16_t&); template int Odrive::read(const std::string& parameter_name, uint32_t&); template int Odrive::read(const std::string& parameter_name, uint64_t&); -template int Odrive::read(const std::string& parameter_name, int&); -template int Odrive::read(const std::string& parameter_name, short&); template int Odrive::read(const std::string& parameter_name, float&); template int Odrive::read(const std::string& parameter_name, bool&); +template int Odrive::write(const std::string& parameter_name, int8_t&); +template int Odrive::write(const std::string& parameter_name, int16_t&); +template int Odrive::write(const std::string& parameter_name, int32_t&); +template int Odrive::write(const std::string& parameter_name, int64_t&); template int Odrive::write(const std::string& parameter_name, uint8_t&); template int Odrive::write(const std::string& parameter_name, uint16_t&); template int Odrive::write(const std::string& parameter_name, uint32_t&); template int Odrive::write(const std::string& parameter_name, uint64_t&); -template int Odrive::write(const std::string& parameter_name, int&); -template int Odrive::write(const std::string& parameter_name, short&); template int Odrive::write(const std::string& parameter_name, float&); template int Odrive::write(const std::string& parameter_name, bool&); } // namespace march - diff --git a/march_hardware/src/odrive/odrive_endpoint.cpp b/march_hardware/src/odrive/odrive_endpoint.cpp index 2e205b45f..b2cd6a8ff 100644 --- a/march_hardware/src/odrive/odrive_endpoint.cpp +++ b/march_hardware/src/odrive/odrive_endpoint.cpp @@ -253,8 +253,9 @@ int OdriveEndpoint::endpointRequest(int endpoint_id, commBuffer& received_payloa return LIBUSB_SUCCESS; } -std::string OdriveEndpoint::getSerialNumber() { - return this->odrive_serial_number; +std::string OdriveEndpoint::getSerialNumber() +{ + return this->odrive_serial_number; } void OdriveEndpoint::appendShortToCommBuffer(commBuffer& buf, short value) @@ -319,20 +320,24 @@ commBuffer OdriveEndpoint::createODrivePacket(short seq_no, int endpoint_id, sho } template int OdriveEndpoint::getData(int, bool&); -template int OdriveEndpoint::getData(int, short&); -template int OdriveEndpoint::getData(int, int&); template int OdriveEndpoint::getData(int, float&); template int OdriveEndpoint::getData(int, uint8_t&); template int OdriveEndpoint::getData(int, uint16_t&); template int OdriveEndpoint::getData(int, uint32_t&); template int OdriveEndpoint::getData(int, uint64_t&); +template int OdriveEndpoint::getData(int, int8_t&); +template int OdriveEndpoint::getData(int, int16_t&); +template int OdriveEndpoint::getData(int, int32_t&); +template int OdriveEndpoint::getData(int, int64_t&); template int OdriveEndpoint::setData(int, const bool&); -template int OdriveEndpoint::setData(int, const short&); -template int OdriveEndpoint::setData(int, const int&); template int OdriveEndpoint::setData(int, const float&); template int OdriveEndpoint::setData(int, const uint8_t&); template int OdriveEndpoint::setData(int, const uint16_t&); template int OdriveEndpoint::setData(int, const uint32_t&); template int OdriveEndpoint::setData(int, const uint64_t&); +template int OdriveEndpoint::setData(int, const int8_t&); +template int OdriveEndpoint::setData(int, const int16_t&); +template int OdriveEndpoint::setData(int, const int32_t&); +template int OdriveEndpoint::setData(int, const int64_t&); } // namespace march diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 9c25eb74a..5eb2c4e16 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -6,26 +6,118 @@ namespace march { OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode) - : Odrive(axisNumber, std::move(odriveEndpoint), false) + : Odrive(axisNumber, std::move(odriveEndpoint), false), mode(mode) { - this->mode = mode; } -int OdriveMotor::get_position() +int OdriveMotor::initialize() { - int position; - std::string parameter_name = this->axis_number + PM_ENCODER_COUNTS; + if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) + { + ROS_ERROR("Calibration sequence was not finished successfully"); + return ODRIVE_ERROR; + } - this->read(parameter_name, position); - return position; + return ODRIVE_OK; } -float OdriveMotor::get_input_voltage() +int OdriveMotor::setState(uint8_t state) { - float odrive_input_voltage; + std::string command_name_ = this->create_command(O_PM_REQUEST_STATE); + if (this->write(command_name_, state) == 1) + { + ROS_ERROR("Could net set state; %i to the axis", state); + return ODRIVE_ERROR; + } + return ODRIVE_OK +} +int OdriveMotor::getState() +{ + int axis_state; + std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); + if (this->read(command_name_, axis_state) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + + return axis_state; +} + +float OdriveMotor::getMotorControllerVoltage() +{ + float motor_controller_voltage; + std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE); + if (this->read(command_name_, motor_controller_voltage) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + return motor_controller_voltage; +} +float OdriveMotor::getMotorCurrent() +{ + float motor_current; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); + if (this->read(command_name_, motor_current) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + + return motor_current; +} +float OdriveMotor::getTorque() +{ + float motor_current = this->getMotorCurrent(); + float motor_torque = CURRENT_TO_TORQUE_CONVERSION * motor_current / MOTOR_KV; + + return motor_torque; +} - this->read(ODRIVE_INPUT_VOLTAGE, odrive_input_voltage); - return odrive_input_voltage; +float OdriveMotor::getAngleRadAbsolute() +{ + return 0; +} +float OdriveMotor::getVelocityRadAbsolute() +{ + return 0; +} + +float OdriveMotor::getAngleRadIncremental() +{ + float iu_position; + std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); + if (this->read(command_name_, iu_position) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + + float angle_rad = iu_position * PI_2 / std::pow(2, 12); + return angle_rad; +} +float OdriveMotor::getVelocityRadIncremental() +{ + float iu_velocity; + std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); + if (this->read(command_name_, iu_velocity) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + + float angle_rad = iu_velocity * PI_2 / std::pow(2, 12); + return angle_rad; +} + +std::string OdriveMotor::create_command(std::string command_name) +{ + if (command_name.at(0) == '.') + { + return this->axis_number + command_name; + } + return command_name; } } // namespace march diff --git a/march_hardware/src/odrive/usb_master.cpp b/march_hardware/src/odrive/usb_master.cpp index af1898b43..aa90522b8 100644 --- a/march_hardware/src/odrive/usb_master.cpp +++ b/march_hardware/src/odrive/usb_master.cpp @@ -2,22 +2,24 @@ #include "march_hardware/motor_controller/odrive/usb_master.h" #include - namespace march { -std::shared_ptr UsbMaster::getSerialConnection(const std::string& serial_number) { - for (uint i = 0; i < this->odrive_endpoints.size(); ++i) { - if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0) { - return odrive_endpoints[i]; - } +std::shared_ptr UsbMaster::getSerialConnection(const std::string& serial_number) +{ + for (uint i = 0; i < this->odrive_endpoints.size(); ++i) + { + if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0) + { + return odrive_endpoints[i]; } + } - std::shared_ptr odrive_endpoint = std::make_shared(); - ROS_INFO("Open serial connection %s", serial_number); - odrive_endpoint->open_connection(serial_number); + std::shared_ptr odrive_endpoint = std::make_shared(); + ROS_INFO("Open serial connection %s", serial_number); + odrive_endpoint->open_connection(serial_number); - odrive_endpoints.push_back(odrive_endpoint); - odrive_endpoint->getSerialNumber(); - return odrive_endpoint; + odrive_endpoints.push_back(odrive_endpoint); + odrive_endpoint->getSerialNumber(); + return odrive_endpoint; } } // namespace march diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index 707f14fcd..4fdb84cba 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -94,7 +94,7 @@ class HardwareBuilder static const std::vector INCREMENTAL_ENCODER_REQUIRED_KEYS; static const std::vector ABSOLUTE_ENCODER_REQUIRED_KEYS; static const std::vector IMOTIONCUBE_REQUIRED_KEYS; - static const std::vector ODRIVE_REQUIRED_KEYS; + static const std::vector ODRIVE_REQUIRED_KEYS; static const std::vector TEMPERATUREGES_REQUIRED_KEYS; static const std::vector POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS; static const std::vector JOINT_REQUIRED_KEYS; diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 7cd28683c..312988aaf 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -132,11 +132,10 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const controller = HardwareBuilder::createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); } - if (joint_config["odrive"]) - { - controller = - HardwareBuilder::createODrive(joint_config["odrive"], mode, urdf_joint); - } + if (joint_config["odrive"]) + { + HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); + } if (!controller) { ROS_FATAL("Joint %s does not have a configuration for a motor controller", joint_name.c_str()); @@ -150,24 +149,25 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const return { joint_name, net_number, allow_actuation, std::move(controller), std::move(ges) }; } -std::unique_ptr HardwareBuilder::createOdrive(const YAML::Node& odrive_config, march::ActuationMode mode, - const urdf::JointConstSharedPtr& urdf_joint, - march::UsbMaster usb_master) +std::unique_ptr HardwareBuilder::createOdrive(const YAML::Node& odrive_config, + march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::UsbMaster usb_master) { - if (!odrive_config || !urdf_joint) - { - return nullptr; - } + if (!odrive_config || !urdf_joint) + { + return nullptr; + } - HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS, "odrive"); + HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS, "odrive"); - YAML::Node incremental_encoder_config = odrive_config["incrementalEncoder"]; - YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; - std::string axis = odrive_config["axis"].as(); - std::string serial_number = odrive_config["serial_number"].as(); - auto odrive_endpoint = usb_master.getSerialConnection(serial_number); + YAML::Node incremental_encoder_config = odrive_config["incrementalEncoder"]; + YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; + std::string axis = odrive_config["axis"].as(); + std::string serial_number = odrive_config["serial_number"].as(); + auto odrive_endpoint = usb_master.getSerialConnection(serial_number); - return std::make_unique(axis, odrive_endpoint, mode); + return std::make_unique(axis, odrive_endpoint, mode); } std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, diff --git a/march_hardware_builder/test/joint_builder_test.cpp b/march_hardware_builder/test/joint_builder_test.cpp index c5ca9190e..04e6cc843 100644 --- a/march_hardware_builder/test/joint_builder_test.cpp +++ b/march_hardware_builder/test/joint_builder_test.cpp @@ -72,10 +72,9 @@ TEST_F(JointBuilderTest, ValidNotActuated) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; - auto usb_master = march::UsbMaster(); - march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface, usb_master); + march::Joint created = HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, + this->sdo_interface, usb_master); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -103,7 +102,6 @@ TEST_F(JointBuilderTest, NoActuate) TEST_F(JointBuilderTest, NoIMotionCube) { - auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_no_imotioncube.yaml"); march::Joint joint = HardwareBuilder::createJoint(config, "test_joint_no_imotioncube", this->joint, @@ -134,10 +132,9 @@ TEST_F(JointBuilderTest, ValidActuationMode) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; - auto usb_master = march::UsbMaster(); - march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface, usb_master); + march::Joint created = HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, + this->sdo_interface, usb_master); march::Joint expected("test_joint_hip", -1, false, std::make_unique( @@ -155,16 +152,16 @@ TEST_F(JointBuilderTest, EmptyJoint) YAML::Node config; auto usb_master = march::UsbMaster(); - ASSERT_THROW( - HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface, usb_master), - MissingKeyException); + ASSERT_THROW(HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, + this->sdo_interface, usb_master), + MissingKeyException); } TEST_F(JointBuilderTest, NoUrdfJoint) { - auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_correct.yaml"); - ASSERT_THROW(HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface, usb_master), - march::error::HardwareException); + ASSERT_THROW( + HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface, usb_master), + march::error::HardwareException); } From b2a89796d6159db7157b3ccfa83a65e1f2e9fad0 Mon Sep 17 00:00:00 2001 From: roel Date: Sun, 16 Aug 2020 20:38:46 +0200 Subject: [PATCH 06/28] fix return type of methods --- .../motor_controller/odrive/odrive.h | 2 +- .../motor_controller/odrive/odrive_motor.h | 10 +++++----- march_hardware/src/odrive/odrive_motor.cpp | 18 +++++++++--------- 3 files changed, 15 insertions(+), 15 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h index 14fd197f7..0cf88b458 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -32,7 +32,7 @@ typedef struct odrive_json_object std::string access; } odrive_json_object; -class Odrive +class Odrive : public MotorController { public: /** diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index 856f43f83..28c4ac71f 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -27,13 +27,13 @@ class OdriveMotor : public Odrive float getMotorControllerVoltage(); float getMotorCurrent(); - float getTorque(); + double getTorque(); - float getAngleRadAbsolute(); - float getVelocityRadAbsolute(); + double getAngleRadAbsolute(); + double getVelocityRadAbsolute(); - float getAngleRadIncremental(); - float getVelocityRadIncremental(); + double getAngleRadIncremental(); + double getVelocityRadIncremental(); private: std::string create_command(std::string command_name); diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 5eb2c4e16..59a30fd05 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -67,24 +67,24 @@ float OdriveMotor::getMotorCurrent() return motor_current; } -float OdriveMotor::getTorque() +double OdriveMotor::getTorque() { - float motor_current = this->getMotorCurrent(); - float motor_torque = CURRENT_TO_TORQUE_CONVERSION * motor_current / MOTOR_KV; + double motor_current = this->getMotorCurrent(); + double motor_torque = CURRENT_TO_TORQUE_CONVERSION * motor_current / MOTOR_KV; return motor_torque; } -float OdriveMotor::getAngleRadAbsolute() +double OdriveMotor::getAngleRadAbsolute() { return 0; } -float OdriveMotor::getVelocityRadAbsolute() +double OdriveMotor::getVelocityRadAbsolute() { return 0; } -float OdriveMotor::getAngleRadIncremental() +double OdriveMotor::getAngleRadIncremental() { float iu_position; std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); @@ -94,10 +94,10 @@ float OdriveMotor::getAngleRadIncremental() return ODRIVE_ERROR; } - float angle_rad = iu_position * PI_2 / std::pow(2, 12); + double angle_rad = iu_position * PI_2 / std::pow(2, 12); return angle_rad; } -float OdriveMotor::getVelocityRadIncremental() +double OdriveMotor::getVelocityRadIncremental() { float iu_velocity; std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); @@ -107,7 +107,7 @@ float OdriveMotor::getVelocityRadIncremental() return ODRIVE_ERROR; } - float angle_rad = iu_velocity * PI_2 / std::pow(2, 12); + double angle_rad = iu_velocity * PI_2 / std::pow(2, 12); return angle_rad; } From 14324c50dbd1476a860a64ccf7ef5ceec200d31c Mon Sep 17 00:00:00 2001 From: roel Date: Mon, 17 Aug 2020 08:53:53 +0200 Subject: [PATCH 07/28] Make (empty) implementation of the remaining template methods --- .../motor_controller/odrive/odrive_enums.h | 2 + .../motor_controller/odrive/odrive_motor.h | 43 ++++-- .../motor_controller/odrive/odrive_states.h | 35 +++++ march_hardware/src/odrive/odrive_motor.cpp | 144 +++++++++++++----- .../src/hardware_builder.cpp | 8 +- 5 files changed, 182 insertions(+), 50 deletions(-) create mode 100644 march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h index 0dd2510d4..720085fc5 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -8,6 +8,8 @@ #define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate" #define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage" #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" +#define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d" +#define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q" // axis states enum States : int diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index 28c4ac71f..eda5f5f27 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -20,24 +20,45 @@ class OdriveMotor : public Odrive public: OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode); - int initialize(); + bool initialize(int cycle_time) override; + void prepareActuation() override; + void reset() override; - int setState(uint8_t state); - int getState(); + void actuateRad(double target_rad) override; + void actuateTorque(int16_t target_torque) override; + + MotorControllerStates& getStates() override; + + float getMotorControllerVoltage() override; + float getMotorCurrent() override; + float getMotorVoltage() override; + double getTorque() override; - float getMotorControllerVoltage(); - float getMotorCurrent(); - double getTorque(); + double getAngleRadAbsolute() override; + double getVelocityRadAbsolute() override; - double getAngleRadAbsolute(); - double getVelocityRadAbsolute(); + double getAngleRadIncremental() override; + double getVelocityRadIncremental() override; - double getAngleRadIncremental(); - double getVelocityRadIncremental(); + bool getIncrementalMorePrecise() const override; + ActuationMode getActuationMode() const + { + return this->mode_; + } + int getSlaveIndex() const + { + return -1; + } private: std::string create_command(std::string command_name); - ActuationMode mode; + int setState(uint8_t state); + int getState(); + + int getAngleCountsAbsolute(); + int getAngleCountsIncremental(); + + ActuationMode mode_; }; } // namespace march diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h new file mode 100644 index 000000000..1426118b8 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h @@ -0,0 +1,35 @@ +// Copyright 2019 Project March. +#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATES_H +#define MARCH_HARDWARE_IMOTIONCUBE_STATES_H + +#include +#include "march_hardware/motor_controller/motor_controller_states.h" +#include "odrive_enums.h" + +namespace march +{ +struct OdriveStates : public MotorControllerStates +{ +public: + OdriveStates() = default; + + States state; + + virtual bool checkState() + { + return true; + } + + virtual std::string getErrorStatus() + { + std::ostringstream error_stream; +// std::string state = IMCStateOfOperation(this->statusWord).getString().c_str(); + + error_stream << "State: " << state; + return error_stream.str(); + } +}; + +} // namespace march + +#endif // MARCH_HARDWARE_IMOTIONCUBE_STATES_H diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 59a30fd05..f3f8d0643 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -1,4 +1,5 @@ #include "march_hardware/motor_controller/odrive/odrive_motor.h" +#include "march_hardware/motor_controller/odrive/odrive_states.h" #include @@ -6,42 +7,59 @@ namespace march { OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode) - : Odrive(axisNumber, std::move(odriveEndpoint), false), mode(mode) + : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode) { } -int OdriveMotor::initialize() +bool OdriveMotor::initialize(int cycle_time) +{ + return cycle_time; +} + +void OdriveMotor::prepareActuation() { if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) { - ROS_ERROR("Calibration sequence was not finished successfully"); - return ODRIVE_ERROR; + ROS_FATAL("Calibration sequence was not finished successfully"); } +} - return ODRIVE_OK; +// to be implemented + void OdriveMotor::reset() + { + return; + } + +void OdriveMotor::actuateRad(double target_rad) +{ + ROS_INFO("Actuating rad %f", target_rad); + return; } -int OdriveMotor::setState(uint8_t state) +void OdriveMotor::actuateTorque(int16_t target_torque) { - std::string command_name_ = this->create_command(O_PM_REQUEST_STATE); - if (this->write(command_name_, state) == 1) - { - ROS_ERROR("Could net set state; %i to the axis", state); - return ODRIVE_ERROR; - } - return ODRIVE_OK + ROS_INFO("Actuating torque %d", target_torque); + return; } -int OdriveMotor::getState() + +MotorControllerStates& OdriveMotor::getStates() { - int axis_state; - std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); - if (this->read(command_name_, axis_state) == 1) - { - ROS_ERROR("Could not retrieve incremental position of the encoder"); - return ODRIVE_ERROR; - } + static OdriveStates states; + + // Common states + states.motorCurrent = this->getMotorCurrent(); + states.controllerVoltage = this->getMotorControllerVoltage(); + states.motorVoltage = this->getMotorVoltage(); + + states.absoluteEncoderValue = this->getAngleCountsAbsolute(); + states.incrementalEncoderValue = this->getAngleCountsIncremental(); + states.absoluteVelocity = this->getVelocityRadAbsolute(); + states.incrementalVelocity = this->getVelocityRadIncremental(); + + states.state = States(this->getState()); + + return states; - return axis_state; } float OdriveMotor::getMotorControllerVoltage() @@ -50,23 +68,38 @@ float OdriveMotor::getMotorControllerVoltage() std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE); if (this->read(command_name_, motor_controller_voltage) == 1) { - ROS_ERROR("Could not retrieve incremental position of the encoder"); + ROS_ERROR("Could not retrieve motor controller voltage"); return ODRIVE_ERROR; } return motor_controller_voltage; } + float OdriveMotor::getMotorCurrent() { float motor_current; std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); if (this->read(command_name_, motor_current) == 1) { - ROS_ERROR("Could not retrieve incremental position of the encoder"); + ROS_ERROR("Could not retrieve motor courrent"); return ODRIVE_ERROR; } return motor_current; } + +float OdriveMotor::getMotorVoltage() +{ + float motor_voltage; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_VOLTAGE_D); + if (this->read(command_name_, motor_voltage) == 1) + { + ROS_ERROR("Could not retrieve motor voltage"); + return ODRIVE_ERROR; + } + + return motor_voltage; +} + double OdriveMotor::getTorque() { double motor_current = this->getMotorCurrent(); @@ -75,35 +108,52 @@ double OdriveMotor::getTorque() return motor_torque; } +int OdriveMotor::getAngleCountsAbsolute() +{ + return 0; +} + double OdriveMotor::getAngleRadAbsolute() { - return 0; + double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17); + return angle_rad; } + double OdriveMotor::getVelocityRadAbsolute() { return 0; } -double OdriveMotor::getAngleRadIncremental() +bool OdriveMotor::getIncrementalMorePrecise() const { - float iu_position; - std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); - if (this->read(command_name_, iu_position) == 1) - { - ROS_ERROR("Could not retrieve incremental position of the encoder"); - return ODRIVE_ERROR; - } + return true; +} - double angle_rad = iu_position * PI_2 / std::pow(2, 12); +int OdriveMotor::getAngleCountsIncremental() +{ + float iu_position; + std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); + if (this->read(command_name_, iu_position) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + return iu_position; +} + +double OdriveMotor::getAngleRadIncremental() +{ + double angle_rad = this->getAngleCountsIncremental() * PI_2 / std::pow(2, 12); return angle_rad; } + double OdriveMotor::getVelocityRadIncremental() { float iu_velocity; std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); if (this->read(command_name_, iu_velocity) == 1) { - ROS_ERROR("Could not retrieve incremental position of the encoder"); + ROS_ERROR("Could not retrieve incremental velocity of the encoder"); return ODRIVE_ERROR; } @@ -120,4 +170,28 @@ std::string OdriveMotor::create_command(std::string command_name) return command_name; } +int OdriveMotor::setState(uint8_t state) +{ + std::string command_name_ = this->create_command(O_PM_REQUEST_STATE); + if (this->write(command_name_, state) == 1) + { + ROS_ERROR("Could net set state; %i to the axis", state); + return ODRIVE_ERROR; + } + return ODRIVE_OK +} + +int OdriveMotor::getState() +{ + int axis_state; + std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); + if (this->read(command_name_, axis_state) == 1) + { + ROS_ERROR("Could not retrieve state of the motor"); + return ODRIVE_ERROR; + } + + return axis_state; +} + } // namespace march diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 312988aaf..1326a11ef 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -132,10 +132,10 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const controller = HardwareBuilder::createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); } - if (joint_config["odrive"]) - { - HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); - } + if (joint_config["odrive"]) + { + controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); + } if (!controller) { ROS_FATAL("Joint %s does not have a configuration for a motor controller", joint_name.c_str()); From 8896e69763b7a209aa02680afb49aac070841ea3 Mon Sep 17 00:00:00 2001 From: roel Date: Mon, 17 Aug 2020 12:27:19 +0200 Subject: [PATCH 08/28] Set testjoint controllers yaml to torque mode --- march_hardware_builder/robots/test_joint_rotational.yaml | 4 ++-- .../config/test_joint_rotational/controllers.yaml | 4 +++- 2 files changed, 5 insertions(+), 3 deletions(-) diff --git a/march_hardware_builder/robots/test_joint_rotational.yaml b/march_hardware_builder/robots/test_joint_rotational.yaml index 90fbdd145..0755c9196 100644 --- a/march_hardware_builder/robots/test_joint_rotational.yaml +++ b/march_hardware_builder/robots/test_joint_rotational.yaml @@ -10,8 +10,8 @@ testsetup: slaveIndex: 1 absoluteEncoder: resolution: 17 - minPositionIU: 97242 - maxPositionIU: 119617 + minPositionIU: 29266 + maxPositionIU: 52948 incrementalEncoder: resolution: 12 transmission: 101 diff --git a/march_hardware_interface/config/test_joint_rotational/controllers.yaml b/march_hardware_interface/config/test_joint_rotational/controllers.yaml index 90861bc5c..a364f96cb 100644 --- a/march_hardware_interface/config/test_joint_rotational/controllers.yaml +++ b/march_hardware_interface/config/test_joint_rotational/controllers.yaml @@ -10,7 +10,7 @@ march: type: march_pdb_state_controller/MarchPdbStateController publish_rate: 50 trajectory: - type: position_controllers/JointTrajectoryController + type: effort_controllers/JointTrajectoryController joints: - rotational_joint constraints: @@ -18,3 +18,5 @@ march: margin_soft_limit_error: 0.5 trajectory: 0.305 goal: 0.305 + gains: # Required because we're controlling an effort interface + rotational_joint: {p: 150, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true} From 3490875f0aed4c1791ba0baa79e221d7bed01c63 Mon Sep 17 00:00:00 2001 From: roel Date: Mon, 17 Aug 2020 15:36:49 +0200 Subject: [PATCH 09/28] Change IU torque to Ampere --- .../motor_controller/imotioncube/imotioncube.h | 2 +- .../motor_controller/odrive/odrive_enums.h | 1 + .../motor_controller/odrive/odrive_motor.h | 2 +- march_hardware/src/imotioncube/imotioncube.cpp | 10 +++++----- march_hardware/src/odrive/odrive_motor.cpp | 10 +++++++--- .../march_hardware_interface.h | 8 ++++---- 6 files changed, 19 insertions(+), 14 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h index bb228ba19..7b1ee7b43 100644 --- a/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h +++ b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h @@ -71,7 +71,7 @@ class IMotionCube : public MotorController, public Slave void setControlWord(uint16_t control_word); virtual void actuateRad(double target_rad) override; - virtual void actuateTorque(double target_torque) override; + virtual void actuateTorque(double target_torque_ampere) override; bool initialize(int cycle_time) override; void goToTargetState(IMotionCubeTargetState target_state); diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h index 720085fc5..ce897ef3d 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -7,6 +7,7 @@ #define O_PM_ENCODER_POSITION_UI ".encoder.pos_estimate" #define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate" #define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage" +#define O_PM_DESIRED_MOTOR_CURRENT ".motor.current_control.Iq_setpoint" #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" #define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d" #define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q" diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index eda5f5f27..89be0752e 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -25,7 +25,7 @@ class OdriveMotor : public Odrive void reset() override; void actuateRad(double target_rad) override; - void actuateTorque(int16_t target_torque) override; + void actuateTorque(double target_torque_ampere) override; MotorControllerStates& getStates() override; diff --git a/march_hardware/src/imotioncube/imotioncube.cpp b/march_hardware/src/imotioncube/imotioncube.cpp index ba8757308..756e48bbd 100644 --- a/march_hardware/src/imotioncube/imotioncube.cpp +++ b/march_hardware/src/imotioncube/imotioncube.cpp @@ -186,11 +186,11 @@ void IMotionCube::actuateIU(int32_t target_iu) void IMotionCube::actuateTorque(double target_torque_ampere) { - if (target_torque_ampere >= IPEAK) - { - throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE, - "Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK); - } + if (target_torque_ampere >= IPEAK) + { + throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE, + "Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK); + } int16_t target_torque = ampereToTorqueIU(target_torque_ampere); if (this->actuation_mode_ != ActuationMode::torque) diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index f3f8d0643..79e01f0c7 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -36,10 +36,14 @@ void OdriveMotor::actuateRad(double target_rad) return; } -void OdriveMotor::actuateTorque(int16_t target_torque) +void OdriveMotor::actuateTorque(double target_torque_ampere) { - ROS_INFO("Actuating torque %d", target_torque); - return; + float target_torque_ampere_float = (float) target_torque_ampere; + std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); + if (this->write(command_name_, target_torque_ampere_float) == 1) + { + ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere); + } } MotorControllerStates& OdriveMotor::getStates() diff --git a/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h b/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h index 27e0888b1..abf144307 100644 --- a/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h +++ b/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h @@ -64,14 +64,14 @@ class MarchHardwareInterface : public hardware_interface::RobotHW void write(const ros::Time& time, const ros::Duration& elapsed_time) override; /** - * Returns the ethercat cycle time in milliseconds. + * Returns the communication cycle time in milliseconds. */ - int getEthercatCycleTime() const; + int getCycleTime() const; /** - * Wait for received PDO. + * Wait for received communication update. */ - void waitForPdo(); + void waitForUpdate(); private: void uploadJointNames(ros::NodeHandle& nh) const; From af8049507b54a67abf359604056cc957998d51e4 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 15:42:01 +0200 Subject: [PATCH 10/28] check communication after fetching joint --- march_hardware/src/march_robot.cpp | 41 ++++++++++++++++++++---------- 1 file changed, 28 insertions(+), 13 deletions(-) diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index dff4d2298..19cfa3565 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -179,15 +179,18 @@ int MarchRobot::getCycleTime() const Joint& MarchRobot::getJoint(::std::string jointName) { - if (!this->isCommunicationOperational()) - { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); - } for (auto& joint : jointList) { if (joint.getName() == jointName) { + if (joint.getMotorControllerSlaveIndex() != -1) + { + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } + } return joint; } } @@ -197,12 +200,18 @@ Joint& MarchRobot::getJoint(::std::string jointName) Joint& MarchRobot::getJoint(size_t index) { - if (!this->isCommunicationOperational()) + Joint& joint = this->jointList.at(index); + + if (joint.getMotorControllerSlaveIndex() != -1) { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } } - return this->jointList.at(index); + + return joint; } size_t MarchRobot::size() const @@ -212,12 +221,18 @@ size_t MarchRobot::size() const MarchRobot::iterator MarchRobot::begin() { - if (!this->isCommunicationOperational()) + auto joint = this->jointList.begin(); + + if (joint[0].getMotorControllerSlaveIndex() != -1) { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } } - return this->jointList.begin(); + + return joint; } MarchRobot::iterator MarchRobot::end() From 758c1e15a0a08cedbcefb135efbbee5136e85cda Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 15:42:43 +0200 Subject: [PATCH 11/28] Set odrive config to creation in hardware_builder.cpp --- march_hardware_builder/src/hardware_builder.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 1326a11ef..9ef25a282 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -134,7 +134,7 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const } if (joint_config["odrive"]) { - controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); + controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); } if (!controller) { @@ -159,7 +159,7 @@ std::unique_ptr HardwareBuilder::createOdrive(const YAML::No return nullptr; } - HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS, "odrive"); + HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::ODRIVE_REQUIRED_KEYS, "odrive"); YAML::Node incremental_encoder_config = odrive_config["incrementalEncoder"]; YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; From b65d7b439b105aa5651a9f0c25e23a412bd94b03 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 15:43:10 +0200 Subject: [PATCH 12/28] Add error messages and update initialisation. --- .../motor_controller/odrive/odrive.h | 7 +- .../motor_controller/odrive/odrive_enums.h | 114 +++++++------ .../motor_controller/odrive/odrive_motor.h | 13 +- .../motor_controller/odrive/odrive_states.h | 28 +++- march_hardware/src/odrive/odrive_motor.cpp | 154 ++++++++++++++---- 5 files changed, 226 insertions(+), 90 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h index 0cf88b458..65eecf3bc 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -40,6 +40,11 @@ class Odrive : public MotorController */ Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json = true); + /** + * Import the odrive json structure from the odrive + */ + int importOdriveJson(); + /** * Check if given value type matched value type of odrive variable */ @@ -71,8 +76,6 @@ class Odrive : public MotorController std::string axis_number; private: - int importOdriveJson(); - int json_string_read(const Json::Value& json_parameter_object); int json_string_write(const Json::Value& json_parameter_object); diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h index ce897ef3d..e1133de89 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -11,6 +11,12 @@ #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" #define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d" #define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q" +#define O_PM_AXIS_ERROR ".error" +#define O_PM_AXIS_MOTOR_ERROR ".motor.error" +#define O_PM_AXIS_ENCODER_ERROR ".encoder.error" +#define O_PM_AXIS_CONTROLLER_ERROR ".controller.error" + +#define ERROR_NONE 0x00 // axis states enum States : int @@ -28,58 +34,74 @@ enum States : int AXIS_STATE_ENCODER_DIR_FIND = 10 }; -#define ERROR_NONE 0x00 - -// class axis: -#define ERROR_INVALID_STATE 0x01 //mode_; } - int getSlaveIndex() const + int getSlaveIndex() const override { return -1; } @@ -53,7 +60,7 @@ class OdriveMotor : public Odrive private: std::string create_command(std::string command_name); int setState(uint8_t state); - int getState(); + uint8_t getState(); int getAngleCountsAbsolute(); int getAngleCountsIncremental(); diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h index 1426118b8..d854d844a 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h @@ -14,18 +14,36 @@ struct OdriveStates : public MotorControllerStates OdriveStates() = default; States state; + uint16_t axisError; + uint16_t axisMotorError; + uint16_t axisEncoderError; + uint16_t axisControllerError; - virtual bool checkState() + std::string axisErrorDescription; + std::string axisMotorErrorDescription; + std::string axisEncoderErrorDescription; + std::string axisControllerErrorDescription; + + bool checkState() override { - return true; + if (this->axisError == ERROR_NONE) + { + return true; + } + return false; } - virtual std::string getErrorStatus() + std::string getErrorStatus() override { std::ostringstream error_stream; -// std::string state = IMCStateOfOperation(this->statusWord).getString().c_str(); - error_stream << "State: " << state; + error_stream << "State: " << this->state << "\nAxis Error: " << this->axisError << " (" + << this->axisErrorDescription << ")" + << "\nMotor Error: " << this->axisMotorError << " (" << this->axisMotorErrorDescription << ")" + << "\nEncoder Error: " << this->axisEncoderError << " (" << this->axisEncoderErrorDescription << ")" + << "\nController Error: " << this->axisControllerError << " (" << this->axisControllerErrorDescription + << ")"; + return error_stream.str(); } }; diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 79e01f0c7..26094bb58 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -18,17 +18,51 @@ bool OdriveMotor::initialize(int cycle_time) void OdriveMotor::prepareActuation() { + this->importOdriveJson(); + if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) { ROS_FATAL("Calibration sequence was not finished successfully"); + return; + } + + this->waitForIdleState(); + + if (this->setState(States::AXIS_STATE_CLOSED_LOOP_CONTROL) == 1) + { + ROS_FATAL("Calibration sequence was not finished successfully"); + return; } + } -// to be implemented - void OdriveMotor::reset() +bool OdriveMotor::waitForIdleState(float timeout) +{ + float current_time = 0; + while (this->getState() != States::AXIS_STATE_IDLE) + { + ros::Duration(0.5).sleep(); + current_time += 0.5; + + if (current_time == timeout) { - return; + ROS_FATAL("Odrive axis did not return to idle state, current state is %i", this->getState()); + return false; } + } + return true; +} + +// to be implemented +void OdriveMotor::reset() +{ + uint16_t axis_error = 0; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->write(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not reset axis"); + } +} void OdriveMotor::actuateRad(double target_rad) { @@ -36,34 +70,86 @@ void OdriveMotor::actuateRad(double target_rad) return; } -void OdriveMotor::actuateTorque(double target_torque_ampere) +void OdriveMotor::actuateTorque(int16_t target_torque) { - float target_torque_ampere_float = (float) target_torque_ampere; - std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); - if (this->write(command_name_, target_torque_ampere_float) == 1) - { - ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere); - } + ROS_INFO("Actuating torque %d", target_torque); + return; } MotorControllerStates& OdriveMotor::getStates() { - static OdriveStates states; + static OdriveStates states; + + // Common states + states.motorCurrent = this->getMotorCurrent(); + states.controllerVoltage = this->getMotorControllerVoltage(); + states.motorVoltage = this->getMotorVoltage(); + + states.absoluteEncoderValue = this->getAngleCountsAbsolute(); + states.incrementalEncoderValue = this->getAngleCountsIncremental(); + states.absoluteVelocity = this->getVelocityRadAbsolute(); + states.incrementalVelocity = this->getVelocityRadIncremental(); + + states.axisError = this->getAxisError(); + states.axisMotorError = this->getAxisMotorError(); + states.axisEncoderError = this->getAxisEncoderError(); + states.axisControllerError = this->getAxisControllerError(); + + states.state = States(this->getState()); + + return states; +} + +uint16_t OdriveMotor::getAxisError() +{ + uint16_t axis_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->read(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not retrieve axis error"); + return ODRIVE_ERROR; + } - // Common states - states.motorCurrent = this->getMotorCurrent(); - states.controllerVoltage = this->getMotorControllerVoltage(); - states.motorVoltage = this->getMotorVoltage(); + return axis_error; +} - states.absoluteEncoderValue = this->getAngleCountsAbsolute(); - states.incrementalEncoderValue = this->getAngleCountsIncremental(); - states.absoluteVelocity = this->getVelocityRadAbsolute(); - states.incrementalVelocity = this->getVelocityRadIncremental(); +uint16_t OdriveMotor::getAxisMotorError() +{ + uint16_t axis_motor_error; + std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->read(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not retrieve axis motor error"); + return ODRIVE_ERROR; + } - states.state = States(this->getState()); + return axis_motor_error; +} - return states; +uint8_t OdriveMotor::getAxisEncoderError() +{ + uint8_t axis_encoder_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->read(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not retrieve axis encoder error"); + return ODRIVE_ERROR; + } + return axis_encoder_error; +} + +uint8_t OdriveMotor::getAxisControllerError() +{ + uint8_t axis_controller_error; + std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->read(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not retrieve axis controller error"); + return ODRIVE_ERROR; + } + + return axis_controller_error; } float OdriveMotor::getMotorControllerVoltage() @@ -114,13 +200,13 @@ double OdriveMotor::getTorque() int OdriveMotor::getAngleCountsAbsolute() { - return 0; + return 0; } double OdriveMotor::getAngleRadAbsolute() { - double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17); - return angle_rad; + double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17); + return angle_rad; } double OdriveMotor::getVelocityRadAbsolute() @@ -135,14 +221,14 @@ bool OdriveMotor::getIncrementalMorePrecise() const int OdriveMotor::getAngleCountsIncremental() { - float iu_position; - std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); - if (this->read(command_name_, iu_position) == 1) - { - ROS_ERROR("Could not retrieve incremental position of the encoder"); - return ODRIVE_ERROR; - } - return iu_position; + float iu_position; + std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); + if (this->read(command_name_, iu_position) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR; + } + return iu_position; } double OdriveMotor::getAngleRadIncremental() @@ -185,9 +271,9 @@ int OdriveMotor::setState(uint8_t state) return ODRIVE_OK } -int OdriveMotor::getState() +uint8_t OdriveMotor::getState() { - int axis_state; + uint8_t axis_state; std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); if (this->read(command_name_, axis_state) == 1) { From 3bae4e7662828f1d581a11f4c3d65b4400cf2cb5 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 15:43:30 +0200 Subject: [PATCH 13/28] fixing clang format --- march_hardware/src/march_robot.cpp | 2 +- march_hardware/src/odrive/odrive_motor.cpp | 1 - 2 files changed, 1 insertion(+), 2 deletions(-) diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index 19cfa3565..15cc43804 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -231,7 +231,7 @@ MarchRobot::iterator MarchRobot::begin() "may lead to incorrect sensor data."); } } - + return joint; } diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 26094bb58..24784c0b4 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -33,7 +33,6 @@ void OdriveMotor::prepareActuation() ROS_FATAL("Calibration sequence was not finished successfully"); return; } - } bool OdriveMotor::waitForIdleState(float timeout) From 498e601473da9250057fd0cb437fdfd63a8dc67d Mon Sep 17 00:00:00 2001 From: roel Date: Mon, 17 Aug 2020 15:44:02 +0200 Subject: [PATCH 14/28] Clang --- .../motor_controller/odrive/odrive_states.h | 28 +---- .../src/imotioncube/imotioncube.cpp | 10 +- march_hardware/src/odrive/odrive_motor.cpp | 106 ++---------------- 3 files changed, 20 insertions(+), 124 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h index d854d844a..b5a6181c6 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h @@ -14,36 +14,18 @@ struct OdriveStates : public MotorControllerStates OdriveStates() = default; States state; - uint16_t axisError; - uint16_t axisMotorError; - uint16_t axisEncoderError; - uint16_t axisControllerError; - std::string axisErrorDescription; - std::string axisMotorErrorDescription; - std::string axisEncoderErrorDescription; - std::string axisControllerErrorDescription; - - bool checkState() override + virtual bool checkState() { - if (this->axisError == ERROR_NONE) - { - return true; - } - return false; + return true; } - std::string getErrorStatus() override + virtual std::string getErrorStatus() { std::ostringstream error_stream; + // std::string state = IMCStateOfOperation(this->statusWord).getString().c_str(); - error_stream << "State: " << this->state << "\nAxis Error: " << this->axisError << " (" - << this->axisErrorDescription << ")" - << "\nMotor Error: " << this->axisMotorError << " (" << this->axisMotorErrorDescription << ")" - << "\nEncoder Error: " << this->axisEncoderError << " (" << this->axisEncoderErrorDescription << ")" - << "\nController Error: " << this->axisControllerError << " (" << this->axisControllerErrorDescription - << ")"; - + error_stream << "State: " << state; return error_stream.str(); } }; diff --git a/march_hardware/src/imotioncube/imotioncube.cpp b/march_hardware/src/imotioncube/imotioncube.cpp index 756e48bbd..ba8757308 100644 --- a/march_hardware/src/imotioncube/imotioncube.cpp +++ b/march_hardware/src/imotioncube/imotioncube.cpp @@ -186,11 +186,11 @@ void IMotionCube::actuateIU(int32_t target_iu) void IMotionCube::actuateTorque(double target_torque_ampere) { - if (target_torque_ampere >= IPEAK) - { - throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE, - "Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK); - } + if (target_torque_ampere >= IPEAK) + { + throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE, + "Target torque of %dA exceeds max torque of %dA", target_torque_ampere, IPEAK); + } int16_t target_torque = ampereToTorqueIU(target_torque_ampere); if (this->actuation_mode_ != ActuationMode::torque) diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 24784c0b4..35ec06c0a 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -18,49 +18,16 @@ bool OdriveMotor::initialize(int cycle_time) void OdriveMotor::prepareActuation() { - this->importOdriveJson(); - if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) { ROS_FATAL("Calibration sequence was not finished successfully"); - return; - } - - this->waitForIdleState(); - - if (this->setState(States::AXIS_STATE_CLOSED_LOOP_CONTROL) == 1) - { - ROS_FATAL("Calibration sequence was not finished successfully"); - return; } } -bool OdriveMotor::waitForIdleState(float timeout) -{ - float current_time = 0; - while (this->getState() != States::AXIS_STATE_IDLE) - { - ros::Duration(0.5).sleep(); - current_time += 0.5; - - if (current_time == timeout) - { - ROS_FATAL("Odrive axis did not return to idle state, current state is %i", this->getState()); - return false; - } - } - return true; -} - // to be implemented void OdriveMotor::reset() { - uint16_t axis_error = 0; - std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); - if (this->write(command_name_, axis_error) == 1) - { - ROS_ERROR("Could not reset axis"); - } + return; } void OdriveMotor::actuateRad(double target_rad) @@ -69,10 +36,14 @@ void OdriveMotor::actuateRad(double target_rad) return; } -void OdriveMotor::actuateTorque(int16_t target_torque) +void OdriveMotor::actuateTorque(double target_torque_ampere) { - ROS_INFO("Actuating torque %d", target_torque); - return; + float target_torque_ampere_float = (float)target_torque_ampere; + std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); + if (this->write(command_name_, target_torque_ampere_float) == 1) + { + ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere); + } } MotorControllerStates& OdriveMotor::getStates() @@ -89,68 +60,11 @@ MotorControllerStates& OdriveMotor::getStates() states.absoluteVelocity = this->getVelocityRadAbsolute(); states.incrementalVelocity = this->getVelocityRadIncremental(); - states.axisError = this->getAxisError(); - states.axisMotorError = this->getAxisMotorError(); - states.axisEncoderError = this->getAxisEncoderError(); - states.axisControllerError = this->getAxisControllerError(); - states.state = States(this->getState()); return states; } -uint16_t OdriveMotor::getAxisError() -{ - uint16_t axis_error; - std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); - if (this->read(command_name_, axis_error) == 1) - { - ROS_ERROR("Could not retrieve axis error"); - return ODRIVE_ERROR; - } - - return axis_error; -} - -uint16_t OdriveMotor::getAxisMotorError() -{ - uint16_t axis_motor_error; - std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); - if (this->read(command_name_, axis_motor_error) == 1) - { - ROS_ERROR("Could not retrieve axis motor error"); - return ODRIVE_ERROR; - } - - return axis_motor_error; -} - -uint8_t OdriveMotor::getAxisEncoderError() -{ - uint8_t axis_encoder_error; - std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); - if (this->read(command_name_, axis_encoder_error) == 1) - { - ROS_ERROR("Could not retrieve axis encoder error"); - return ODRIVE_ERROR; - } - - return axis_encoder_error; -} - -uint8_t OdriveMotor::getAxisControllerError() -{ - uint8_t axis_controller_error; - std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); - if (this->read(command_name_, axis_controller_error) == 1) - { - ROS_ERROR("Could not retrieve axis controller error"); - return ODRIVE_ERROR; - } - - return axis_controller_error; -} - float OdriveMotor::getMotorControllerVoltage() { float motor_controller_voltage; @@ -270,9 +184,9 @@ int OdriveMotor::setState(uint8_t state) return ODRIVE_OK } -uint8_t OdriveMotor::getState() +int OdriveMotor::getState() { - uint8_t axis_state; + int axis_state; std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); if (this->read(command_name_, axis_state) == 1) { From 3cb78ede201868742795261a190e857b1544537b Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 19:33:01 +0200 Subject: [PATCH 15/28] Added the configuration of the Json file to the odrive object. also updated the xacro with the right limits. --- march_hardware/config/march_odrive.json | 5 + .../motor_controller/odrive/odrive_enums.h | 2 +- .../motor_controller/odrive/odrive_motor.h | 5 +- march_hardware/src/odrive/odrive.cpp | 14 +- march_hardware/src/odrive/odrive_motor.cpp | 148 +++++++++++++++++- .../src/hardware_builder.cpp | 4 +- 6 files changed, 159 insertions(+), 19 deletions(-) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json index dd5398520..e36347a9b 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/march_odrive.json @@ -9,6 +9,11 @@ "type":"uint32", "value":4096 }, + { + "name":"axis0.encoder.config.use_index", + "type":"bool", + "value":1 + }, { "name":"config.brake_resistance", "type":"float", diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h index e1133de89..3372503d9 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -7,7 +7,7 @@ #define O_PM_ENCODER_POSITION_UI ".encoder.pos_estimate" #define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate" #define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage" -#define O_PM_DESIRED_MOTOR_CURRENT ".motor.current_control.Iq_setpoint" +#define O_PM_DESIRED_MOTOR_CURRENT ".controller.current_setpoint" #define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" #define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d" #define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q" diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index 181f655ed..ac3559589 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -18,7 +18,9 @@ namespace march class OdriveMotor : public Odrive { public: - OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode); + OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode, + std::string json_config_file_path); + ~OdriveMotor(); bool initialize(int cycle_time) override; void prepareActuation() override; @@ -66,6 +68,7 @@ class OdriveMotor : public Odrive int getAngleCountsIncremental(); ActuationMode mode_; + std::string json_config_file_path_; }; } // namespace march diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp index d7c1a5bb8..a0fb370b6 100644 --- a/march_hardware/src/odrive/odrive.cpp +++ b/march_hardware/src/odrive/odrive.cpp @@ -296,7 +296,7 @@ int Odrive::json_string_read(const Json::Value& json_parameter_object) else { ROS_ERROR("Error converting string for reading, invalid type %s", parameter_name.c_str()); - return ODRIVE_ERROR; + return ODRIVE_ERROR } } @@ -349,7 +349,7 @@ int Odrive::json_string_write(const Json::Value& json_parameter_object) else { ROS_ERROR("Error converting string for writing, invalid type %s", parameter_name.c_str()); - return ODRIVE_ERROR; + return ODRIVE_ERROR } } @@ -374,23 +374,21 @@ int Odrive::setConfigurations(const std::string& configuration_json_path) if (!res) { ROS_INFO("Error parsing odrive configuration, error"); - return ODRIVE_ERROR; + return ODRIVE_ERROR } for (auto& parameter : this->odrive_configuration_json_) { - ROS_INFO("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + ROS_DEBUG("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); int result = this->json_string_write(parameter); if (result != LIBUSB_SUCCESS) { - ROS_INFO("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + ROS_WARN("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); continue; } - - ROS_INFO("Setting succeeded %s", parameter["name"].asString().c_str()); } - return ODRIVE_OK; + return ODRIVE_OK } template int Odrive::validateType(const odrive_json_object& json_object, int8_t&); diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 35ec06c0a..8e61c6650 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -6,11 +6,20 @@ namespace march { OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, - ActuationMode mode) - : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode) + ActuationMode mode, std::string json_config_file_path) + : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode), json_config_file_path_(json_config_file_path) { } +OdriveMotor::~OdriveMotor() +{ + if (this->setState(States::AXIS_STATE_IDLE) == 1) + { + ROS_FATAL("Not set to idle when closed"); + return; + } +} + bool OdriveMotor::initialize(int cycle_time) { return cycle_time; @@ -18,16 +27,77 @@ bool OdriveMotor::initialize(int cycle_time) void OdriveMotor::prepareActuation() { + this->importOdriveJson(); + + if (this->setConfigurations(this->json_config_file_path_) == 1) + { + ROS_FATAL("Setting configurations was not finished successfully"); + } + + this->reset(); + if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) { ROS_FATAL("Calibration sequence was not finished successfully"); + return; + } + + this->waitForIdleState(); + + if (this->setState(States::AXIS_STATE_CLOSED_LOOP_CONTROL) == 1) + { + ROS_FATAL("Setting closed loop control was not finished successfully"); + return; } } +bool OdriveMotor::waitForIdleState(float timeout) +{ + float current_time = 0; + while (this->getState() != States::AXIS_STATE_IDLE) + { + ros::Duration(0.5).sleep(); + current_time += 0.5; + + if (current_time == timeout) + { + ROS_FATAL("Odrive axis did not return to idle state, current state is %i", this->getState()); + return false; + } + } + return true; +} + // to be implemented void OdriveMotor::reset() { - return; + uint16_t axis_error = 0; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->write(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not reset axis"); + } + + uint16_t axis_motor_error = 0; + command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->write(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not reset motor axis"); + } + + uint8_t axis_encoder_error = 0; + command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->write(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not reset encoder axis"); + } + + uint8_t axis_controller_error = 0; + command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->write(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not reset controller axis"); + } } void OdriveMotor::actuateRad(double target_rad) @@ -39,6 +109,12 @@ void OdriveMotor::actuateRad(double target_rad) void OdriveMotor::actuateTorque(double target_torque_ampere) { float target_torque_ampere_float = (float)target_torque_ampere; + ROS_INFO("target torque: %f", target_torque_ampere_float); + + if (target_torque_ampere_float < 3.0) + { + target_torque_ampere_float = 3.0; + } std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); if (this->write(command_name_, target_torque_ampere_float) == 1) { @@ -50,7 +126,6 @@ MotorControllerStates& OdriveMotor::getStates() { static OdriveStates states; - // Common states states.motorCurrent = this->getMotorCurrent(); states.controllerVoltage = this->getMotorControllerVoltage(); states.motorVoltage = this->getMotorVoltage(); @@ -60,11 +135,68 @@ MotorControllerStates& OdriveMotor::getStates() states.absoluteVelocity = this->getVelocityRadAbsolute(); states.incrementalVelocity = this->getVelocityRadIncremental(); + states.axisError = this->getAxisError(); + states.axisMotorError = this->getAxisMotorError(); + states.axisEncoderError = this->getAxisEncoderError(); + states.axisControllerError = this->getAxisControllerError(); + states.state = States(this->getState()); return states; } +uint16_t OdriveMotor::getAxisError() +{ + uint16_t axis_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->read(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not retrieve axis error"); + return ODRIVE_ERROR; + } + + return axis_error; +} + +uint16_t OdriveMotor::getAxisMotorError() +{ + uint16_t axis_motor_error; + std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->read(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not retrieve axis motor error"); + return ODRIVE_ERROR; + } + + return axis_motor_error; +} + +uint8_t OdriveMotor::getAxisEncoderError() +{ + uint8_t axis_encoder_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->read(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not retrieve axis encoder error"); + return ODRIVE_ERROR; + } + + return axis_encoder_error; +} + +uint8_t OdriveMotor::getAxisControllerError() +{ + uint8_t axis_controller_error; + std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->read(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not retrieve axis controller error"); + return ODRIVE_ERROR; + } + + return axis_controller_error; +} + float OdriveMotor::getMotorControllerVoltage() { float motor_controller_voltage; @@ -118,8 +250,7 @@ int OdriveMotor::getAngleCountsAbsolute() double OdriveMotor::getAngleRadAbsolute() { - double angle_rad = this->getAngleCountsAbsolute() * PI_2 / std::pow(2, 17); - return angle_rad; + return 0; } double OdriveMotor::getVelocityRadAbsolute() @@ -141,6 +272,7 @@ int OdriveMotor::getAngleCountsIncremental() ROS_ERROR("Could not retrieve incremental position of the encoder"); return ODRIVE_ERROR; } + ROS_WARN("encoder position: %f", iu_position); return iu_position; } @@ -184,9 +316,9 @@ int OdriveMotor::setState(uint8_t state) return ODRIVE_OK } -int OdriveMotor::getState() +uint8_t OdriveMotor::getState() { - int axis_state; + uint8_t axis_state; std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); if (this->read(command_name_, axis_state) == 1) { diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 9ef25a282..fd1391737 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -165,9 +165,11 @@ std::unique_ptr HardwareBuilder::createOdrive(const YAML::No YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; std::string axis = odrive_config["axis"].as(); std::string serial_number = odrive_config["serial_number"].as(); + std::string json_configuration_path = ros::package::getPath("march_hardware").append("/config/march_odrive.json"); + auto odrive_endpoint = usb_master.getSerialConnection(serial_number); - return std::make_unique(axis, odrive_endpoint, mode); + return std::make_unique(axis, odrive_endpoint, mode, json_configuration_path); } std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, From aa8e1bf6b5b771e7ef7bd2aa1fc610e80a594f48 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Mon, 17 Aug 2020 20:08:27 +0200 Subject: [PATCH 16/28] Updated the configurations to every valid type --- march_hardware/config/march_odrive.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json index e36347a9b..cc2f558f8 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/march_odrive.json @@ -32,7 +32,7 @@ { "name":"axis0.controller.config.vel_limit", "type":"float", - "value":5000.0 + "value":50000.0 }, { "name":"axis0.motor.config.motor_type", @@ -56,7 +56,7 @@ }, { "name":"axis0.motor.config.direction", - "type":"uint8", + "type":"int32", "value":1 }, { From eff82431e4b69869baf431d9d48027acf2c5d44d Mon Sep 17 00:00:00 2001 From: roel Date: Mon, 17 Aug 2020 23:01:20 +0200 Subject: [PATCH 17/28] Fix some bugs --- march_hardware/config/march_odrive.json | 6 +++--- march_hardware/src/odrive/odrive_motor.cpp | 11 ++++------ .../march_hardware_builder/allowed_robot.h | 2 +- .../robots/odrive_test_joint_rotational.yaml | 1 - .../src/march_hardware_interface.cpp | 20 +++++++++---------- 5 files changed, 18 insertions(+), 22 deletions(-) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json index cc2f558f8..e69c34dd2 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/march_odrive.json @@ -6,7 +6,7 @@ }, { "name":"axis0.encoder.config.cpr", - "type":"uint32", + "type":"int32", "value":4096 }, { @@ -21,7 +21,7 @@ }, { "name":"axis0.motor.config.pole_pairs", - "type":"uint32", + "type":"int32", "value":21 }, { @@ -57,7 +57,7 @@ { "name":"axis0.motor.config.direction", "type":"int32", - "value":1 + "value":0 }, { "name":"axis0.sensorless_estimator.config.pm_flux_linkage", diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 8e61c6650..146886c0f 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -111,10 +111,6 @@ void OdriveMotor::actuateTorque(double target_torque_ampere) float target_torque_ampere_float = (float)target_torque_ampere; ROS_INFO("target torque: %f", target_torque_ampere_float); - if (target_torque_ampere_float < 3.0) - { - target_torque_ampere_float = 3.0; - } std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); if (this->write(command_name_, target_torque_ampere_float) == 1) { @@ -219,6 +215,8 @@ float OdriveMotor::getMotorCurrent() return ODRIVE_ERROR; } + ROS_INFO("actual torque: %f", motor_current); + return motor_current; } @@ -272,13 +270,12 @@ int OdriveMotor::getAngleCountsIncremental() ROS_ERROR("Could not retrieve incremental position of the encoder"); return ODRIVE_ERROR; } - ROS_WARN("encoder position: %f", iu_position); return iu_position; } double OdriveMotor::getAngleRadIncremental() { - double angle_rad = this->getAngleCountsIncremental() * PI_2 / std::pow(2, 12); + double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101); return angle_rad; } @@ -292,7 +289,7 @@ double OdriveMotor::getVelocityRadIncremental() return ODRIVE_ERROR; } - double angle_rad = iu_velocity * PI_2 / std::pow(2, 12); + double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101); return angle_rad; } diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index 09f52bb02..24628fe33 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -67,7 +67,7 @@ class AllowedRobot } else if (this->value == AllowedRobot::test_joint_rotational) { - return base_path.append("/robots/test_joint_rotational.yaml"); + return base_path.append("/robots/odrive_test_joint_rotational.yaml"); } else if (this->value == AllowedRobot::odrive_test_joint_rotational) { diff --git a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml index 525c4ec15..f7158555c 100644 --- a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml +++ b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml @@ -1,5 +1,4 @@ testsetup: - ifName: enp2s0 cycleTime: 4 slaveTimeout: 50 joints: diff --git a/march_hardware_interface/src/march_hardware_interface.cpp b/march_hardware_interface/src/march_hardware_interface.cpp index 73cea5731..a74bd7841 100644 --- a/march_hardware_interface/src/march_hardware_interface.cpp +++ b/march_hardware_interface/src/march_hardware_interface.cpp @@ -48,8 +48,8 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot this->reserveMemory(); - // Start ethercat cycle in the hardware - this->march_robot_->startEtherCAT(this->reset_motor_controllers_); + // Start communication cycle in the hardware + this->march_robot_->startCommunication(this->reset_motor_controllers_); for (size_t i = 0; i < num_joints_; ++i) { @@ -144,7 +144,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot &joint_temperature_variance_[i]); march_temperature_interface_.registerHandle(temperature_sensor_handle); - // Prepare Motor Controllers for actuations + // Enable high voltage on the Motor controllers if (joint.canActuate()) { joint.prepareActuation(); @@ -178,7 +178,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot void MarchHardwareInterface::validate() { - const auto last_exception = this->march_robot_->getLastEthercatException(); + const auto last_exception = this->march_robot_->getLastCommunicationException(); if (last_exception) { std::rethrow_exception(last_exception); @@ -195,14 +195,14 @@ void MarchHardwareInterface::validate() } if (fault_state) { - this->march_robot_->stopEtherCAT(); + this->march_robot_->stopCommunication(); throw std::runtime_error("One or more motor controllers are in fault state"); } } -void MarchHardwareInterface::waitForPdo() +void MarchHardwareInterface::waitForUpdate() { - this->march_robot_->waitForPdo(); + this->march_robot_->waitForUpdate(); } void MarchHardwareInterface::read(const ros::Time& /* time */, const ros::Duration& elapsed_time) @@ -277,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat } else if (joint.getActuationMode() == march::ActuationMode::torque) { - joint.actuateTorque(joint_effort_command_[i]); + joint.actuateTorque(joint_effort_command_[i])); } } } @@ -290,9 +290,9 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat } } -int MarchHardwareInterface::getEthercatCycleTime() const +int MarchHardwareInterface::getCycleTime() const { - return this->march_robot_->getEthercatCycleTime(); + return this->march_robot_->getCycleTime(); } void MarchHardwareInterface::uploadJointNames(ros::NodeHandle& nh) const From 7d0261affa9806d6faa343445d3c21ec12e5b796 Mon Sep 17 00:00:00 2001 From: roel Date: Tue, 18 Aug 2020 16:17:39 +0200 Subject: [PATCH 18/28] Add two_odrive_joints robot --- .../robots/two_odrive_joints.yaml | 32 +++++++++++++++++++ .../config/two_odrive_joints/controllers.yaml | 29 +++++++++++++++++ 2 files changed, 61 insertions(+) create mode 100644 march_hardware_builder/robots/two_odrive_joints.yaml create mode 100644 march_hardware_interface/config/two_odrive_joints/controllers.yaml diff --git a/march_hardware_builder/robots/two_odrive_joints.yaml b/march_hardware_builder/robots/two_odrive_joints.yaml new file mode 100644 index 000000000..aca24c143 --- /dev/null +++ b/march_hardware_builder/robots/two_odrive_joints.yaml @@ -0,0 +1,32 @@ +# For convenience it is easiest if the joint order is maintained, it is chosen to sort the joints alphabetically. +march4: + cycleTime: 4 + slaveTimeout: 50 + joints: + - right_hip_fe: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 2045 + maxPositionIU: 45632 + incrementalEncoder: + resolution: 12 + transmission: 101 + + - right_knee: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 41533 + maxPositionIU: 85298 + incrementalEncoder: + resolution: 12 + transmission: 101 diff --git a/march_hardware_interface/config/two_odrive_joints/controllers.yaml b/march_hardware_interface/config/two_odrive_joints/controllers.yaml new file mode 100644 index 000000000..e038f46c2 --- /dev/null +++ b/march_hardware_interface/config/two_odrive_joints/controllers.yaml @@ -0,0 +1,29 @@ +march: + controller: + pdb_state: + type: march_pdb_state_controller/MarchPdbStateController + publish_rate: 50 + joint_state: + type: joint_state_controller/JointStateController + publish_rate: 250 + temperature_sensor: + type: march_temperature_sensor_controller/MarchTemperatureSensorController + publish_rate: 1 + trajectory: + type: effort_controllers/JointTrajectoryController + allow_partial_joints_goal: true + joints: + - right_hip_fe + - right_knee + gains: # Required because we're controlling an effort interface + right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} + right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} + constraints: + right_hip_fe: + margin_soft_limit_error: 0.5 + trajectory: 0.305 + goal: 0.305 + right_knee: + margin_soft_limit_error: 0.5 + trajectory: 0.305 + goal: 0.305 From df625d08d3d39904670f5f53c7ebf7e793e9415d Mon Sep 17 00:00:00 2001 From: roel Date: Tue, 18 Aug 2020 16:38:48 +0200 Subject: [PATCH 19/28] Add two_odrive_joints robot #2 --- .../include/march_hardware_builder/allowed_robot.h | 10 ++++++++++ .../src/march_hardware_interface.cpp | 2 +- 2 files changed, 11 insertions(+), 1 deletion(-) diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index 24628fe33..c1f582f18 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -18,11 +18,13 @@ class AllowedRobot odrive_test_joint_rotational, test_joint_linear, pdb, + two_odrive_joints, }; AllowedRobot() = default; explicit AllowedRobot(const std::string& robot_name) { + ROS_WARN("Used robot %s", robot_name); if (robot_name == "march4") { this->value = march4; @@ -39,6 +41,10 @@ class AllowedRobot { this->value = odrive_test_joint_rotational; } + else if (robot_name == "two_odrive_joints") + { + this->value = two_odrive_joints; + } else if (robot_name == "test_joint_linear") { this->value = test_joint_linear; @@ -69,6 +75,10 @@ class AllowedRobot { return base_path.append("/robots/odrive_test_joint_rotational.yaml"); } + else if (this->value == AllowedRobot::two_odrive_joints) + { + return base_path.append("/robots/two_odrive_joints.yaml"); + } else if (this->value == AllowedRobot::odrive_test_joint_rotational) { return base_path.append("/robots/odrive_test_joint_rotational.yaml"); diff --git a/march_hardware_interface/src/march_hardware_interface.cpp b/march_hardware_interface/src/march_hardware_interface.cpp index a74bd7841..b334a7b88 100644 --- a/march_hardware_interface/src/march_hardware_interface.cpp +++ b/march_hardware_interface/src/march_hardware_interface.cpp @@ -277,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat } else if (joint.getActuationMode() == march::ActuationMode::torque) { - joint.actuateTorque(joint_effort_command_[i])); + joint.actuateTorque(joint_effort_command_[i]); } } } From 30dfc9afdc034b7354e4bc4e0174edaefe499327 Mon Sep 17 00:00:00 2001 From: roel Date: Tue, 18 Aug 2020 16:49:03 +0200 Subject: [PATCH 20/28] Remove one joint --- .../robots/two_odrive_joints.yaml | 26 +++++++++---------- .../config/two_odrive_joints/controllers.yaml | 12 ++++----- 2 files changed, 19 insertions(+), 19 deletions(-) diff --git a/march_hardware_builder/robots/two_odrive_joints.yaml b/march_hardware_builder/robots/two_odrive_joints.yaml index aca24c143..f84f695e6 100644 --- a/march_hardware_builder/robots/two_odrive_joints.yaml +++ b/march_hardware_builder/robots/two_odrive_joints.yaml @@ -3,19 +3,19 @@ march4: cycleTime: 4 slaveTimeout: 50 joints: - - right_hip_fe: - actuationMode: torque - allowActuation: true - odrive: - serial_number: "0x2084387E304E" - axis: "axis0" - absoluteEncoder: - resolution: 17 - minPositionIU: 2045 - maxPositionIU: 45632 - incrementalEncoder: - resolution: 12 - transmission: 101 +# - right_hip_fe: +# actuationMode: torque +# allowActuation: true +# odrive: +# serial_number: "0x2084387E304E" +# axis: "axis0" +# absoluteEncoder: +# resolution: 17 +# minPositionIU: 2045 +# maxPositionIU: 45632 +# incrementalEncoder: +# resolution: 12 +# transmission: 101 - right_knee: actuationMode: torque diff --git a/march_hardware_interface/config/two_odrive_joints/controllers.yaml b/march_hardware_interface/config/two_odrive_joints/controllers.yaml index e038f46c2..3841042ad 100644 --- a/march_hardware_interface/config/two_odrive_joints/controllers.yaml +++ b/march_hardware_interface/config/two_odrive_joints/controllers.yaml @@ -13,16 +13,16 @@ march: type: effort_controllers/JointTrajectoryController allow_partial_joints_goal: true joints: - - right_hip_fe +# - right_hip_fe - right_knee gains: # Required because we're controlling an effort interface - right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} +# right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} constraints: - right_hip_fe: - margin_soft_limit_error: 0.5 - trajectory: 0.305 - goal: 0.305 +# right_hip_fe: +# margin_soft_limit_error: 0.5 +# trajectory: 0.305 +# goal: 0.305 right_knee: margin_soft_limit_error: 0.5 trajectory: 0.305 From b07636f2770fea457c82d9349f2f08c5a08a6c70 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 18 Aug 2020 17:11:42 +0200 Subject: [PATCH 21/28] Added current control bandwith to the json and updated the format. --- march_hardware/config/march_odrive.json | 11 ++++++----- march_hardware/src/odrive/odrive.cpp | 6 +++--- march_hardware/src/odrive/odrive_motor.cpp | 14 ++++---------- .../include/march_hardware_builder/allowed_robot.h | 1 - 4 files changed, 13 insertions(+), 19 deletions(-) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json index e69c34dd2..b54df8e70 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/march_odrive.json @@ -34,6 +34,12 @@ "type":"float", "value":50000.0 }, + { + "name":"axis0.motor.config.current_control_bandwidth", + "type":"float", + "value":100.0 + }, + { "name":"axis0.motor.config.motor_type", "type":"uint8", @@ -54,11 +60,6 @@ "type":"uint8", "value":1 }, - { - "name":"axis0.motor.config.direction", - "type":"int32", - "value":0 - }, { "name":"axis0.sensorless_estimator.config.pm_flux_linkage", "type":"float", diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp index a0fb370b6..b119731b4 100644 --- a/march_hardware/src/odrive/odrive.cpp +++ b/march_hardware/src/odrive/odrive.cpp @@ -323,17 +323,17 @@ int Odrive::json_string_write(const Json::Value& json_parameter_object) } else if (type_name == "int8") { - int8_t casted_value = value.asUInt(); + int8_t casted_value = value.asInt(); return this->write(parameter_name, casted_value); } else if (type_name == "int16") { - int16_t casted_value = value.asUInt(); + int16_t casted_value = value.asInt(); return this->write(parameter_name, casted_value); } else if (type_name == "int32") { - int32_t casted_value = value.asUInt(); + int32_t casted_value = value.asInt(); return this->write(parameter_name, casted_value); } else if (type_name == "float") diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 146886c0f..679183f77 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -109,7 +109,6 @@ void OdriveMotor::actuateRad(double target_rad) void OdriveMotor::actuateTorque(double target_torque_ampere) { float target_torque_ampere_float = (float)target_torque_ampere; - ROS_INFO("target torque: %f", target_torque_ampere_float); std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); if (this->write(command_name_, target_torque_ampere_float) == 1) @@ -211,12 +210,10 @@ float OdriveMotor::getMotorCurrent() std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); if (this->read(command_name_, motor_current) == 1) { - ROS_ERROR("Could not retrieve motor courrent"); + ROS_ERROR("Could not retrieve motor current"); return ODRIVE_ERROR; } - ROS_INFO("actual torque: %f", motor_current); - return motor_current; } @@ -235,10 +232,7 @@ float OdriveMotor::getMotorVoltage() double OdriveMotor::getTorque() { - double motor_current = this->getMotorCurrent(); - double motor_torque = CURRENT_TO_TORQUE_CONVERSION * motor_current / MOTOR_KV; - - return motor_torque; + return this->getMotorCurrent(); } int OdriveMotor::getAngleCountsAbsolute() @@ -268,7 +262,7 @@ int OdriveMotor::getAngleCountsIncremental() if (this->read(command_name_, iu_position) == 1) { ROS_ERROR("Could not retrieve incremental position of the encoder"); - return ODRIVE_ERROR; + return ODRIVE_ERROR } return iu_position; } @@ -286,7 +280,7 @@ double OdriveMotor::getVelocityRadIncremental() if (this->read(command_name_, iu_velocity) == 1) { ROS_ERROR("Could not retrieve incremental velocity of the encoder"); - return ODRIVE_ERROR; + return ODRIVE_ERROR } double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101); diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index c1f582f18..5dbd4111a 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -24,7 +24,6 @@ class AllowedRobot AllowedRobot() = default; explicit AllowedRobot(const std::string& robot_name) { - ROS_WARN("Used robot %s", robot_name); if (robot_name == "march4") { this->value = march4; From a37af6dad8427b0b9e0d77b7ab3777520b8efbf9 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 18 Aug 2020 18:36:12 +0200 Subject: [PATCH 22/28] updated json configurations --- march_hardware/config/march_odrive.json | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/march_odrive.json index b54df8e70..ae3c4d913 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/march_odrive.json @@ -2,7 +2,7 @@ { "name":"axis0.motor.config.current_lim", "type":"float", - "value":10.0 + "value":30.0 }, { "name":"axis0.encoder.config.cpr", @@ -32,7 +32,7 @@ { "name":"axis0.controller.config.vel_limit", "type":"float", - "value":50000.0 + "value":200000.0 }, { "name":"axis0.motor.config.current_control_bandwidth", From 4bf6e9361cdb79de25f3c9ffeddcd691f6f5b370 Mon Sep 17 00:00:00 2001 From: roel Date: Tue, 18 Aug 2020 20:44:02 +0200 Subject: [PATCH 23/28] Read parameters only once each cycle --- .../motor_controller/odrive/odrive.h | 19 +++ .../motor_controller/odrive/odrive_motor.h | 18 ++- march_hardware/src/odrive/odrive.cpp | 135 ++++++++++++++++++ march_hardware/src/odrive/odrive_motor.cpp | 124 +++++----------- 4 files changed, 202 insertions(+), 94 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h index 65eecf3bc..7824c4958 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -22,6 +22,8 @@ #define ODRIVE_OK 0; #define ODRIVE_ERROR 1; +static constexpr double PI_2 = 2 * M_PI; + namespace march { typedef struct odrive_json_object @@ -75,6 +77,23 @@ class Odrive : public MotorController std::string axis_number; +protected: + float readMotorControllerVoltage(); + float readMotorCurrent(); + float readMotorVoltage(); + + uint16_t readAxisError(); + uint16_t readAxisMotorError(); + uint8_t readAxisEncoderError(); + uint8_t readAxisControllerError(); + + int readAngleCountsAbsolute(); + double readVelocityRadAbsolute(); + + int readAngleCountsIncremental(); + double readVelocityRadIncremental(); + std::string create_command(std::string command_name); + private: int json_string_read(const Json::Value& json_parameter_object); diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h index ac3559589..26374a7bd 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -9,7 +9,6 @@ #include "odrive_enums.h" #include -static constexpr double PI_2 = 2 * M_PI; static constexpr double MOTOR_KV = 100; static constexpr double CURRENT_TO_TORQUE_CONVERSION = 8.27; @@ -60,7 +59,6 @@ class OdriveMotor : public Odrive } private: - std::string create_command(std::string command_name); int setState(uint8_t state); uint8_t getState(); @@ -69,6 +67,22 @@ class OdriveMotor : public Odrive ActuationMode mode_; std::string json_config_file_path_; + + void readValues(); + + uint16_t axis_error; + uint16_t axis_motor_error; + uint8_t axis_encoder_error; + uint8_t axis_controller_error; + + float motor_controller_voltage; + float motor_current; + float motor_voltage; + + double angle_counts_absolute; + double velocity_rad_absolute; + double angle_counts_incremental; + double velocity_rad_incremental; }; } // namespace march diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp index b119731b4..e5b69e25c 100644 --- a/march_hardware/src/odrive/odrive.cpp +++ b/march_hardware/src/odrive/odrive.cpp @@ -391,6 +391,141 @@ int Odrive::setConfigurations(const std::string& configuration_json_path) return ODRIVE_OK } +uint16_t Odrive::readAxisError() +{ + uint16_t axis_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->read(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not retrieve axis error"); + return ODRIVE_ERROR; + } + + return axis_error; +} + +uint16_t Odrive::readAxisMotorError() +{ + uint16_t axis_motor_error; + std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->read(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not retrieve axis motor error"); + return ODRIVE_ERROR; + } + + return axis_motor_error; +} + +uint8_t Odrive::readAxisEncoderError() +{ + uint8_t axis_encoder_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->read(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not retrieve axis encoder error"); + return ODRIVE_ERROR; + } + + return axis_encoder_error; +} + +uint8_t Odrive::readAxisControllerError() +{ + uint8_t axis_controller_error; + std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->read(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not retrieve axis controller error"); + return ODRIVE_ERROR; + } + + return axis_controller_error; +} + +float Odrive::readMotorControllerVoltage() +{ + float motor_controller_voltage; + std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE); + if (this->read(command_name_, motor_controller_voltage) == 1) + { + ROS_ERROR("Could not retrieve motor controller voltage"); + return ODRIVE_ERROR; + } + return motor_controller_voltage; +} + +float Odrive::readMotorCurrent() +{ + float motor_current; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); + if (this->read(command_name_, motor_current) == 1) + { + ROS_ERROR("Could not retrieve motor current"); + return ODRIVE_ERROR; + } + + return motor_current; +} + +float Odrive::readMotorVoltage() +{ + float motor_voltage; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_VOLTAGE_D); + if (this->read(command_name_, motor_voltage) == 1) + { + ROS_ERROR("Could not retrieve motor voltage"); + return ODRIVE_ERROR; + } + + return motor_voltage; +} + +int Odrive::readAngleCountsAbsolute() +{ + return 0; +} + +double Odrive::readVelocityRadAbsolute() +{ + return 0; +} + +int Odrive::readAngleCountsIncremental() +{ + float iu_position; + std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); + if (this->read(command_name_, iu_position) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR + } + return iu_position; +} + +double Odrive::readVelocityRadIncremental() +{ + float iu_velocity; + std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); + if (this->read(command_name_, iu_velocity) == 1) + { + ROS_ERROR("Could not retrieve incremental velocity of the encoder"); + return ODRIVE_ERROR + } + + double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101); + return angle_rad; +} + +std::string Odrive::create_command(std::string command_name) +{ + if (command_name.at(0) == '.') + { + return this->axis_number + command_name; + } + return command_name; +} + template int Odrive::validateType(const odrive_json_object& json_object, int8_t&); template int Odrive::validateType(const odrive_json_object& json_object, int16_t&); template int Odrive::validateType(const odrive_json_object& json_object, int32_t&); diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index 679183f77..b777a0da9 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -115,6 +115,8 @@ void OdriveMotor::actuateTorque(double target_torque_ampere) { ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere); } + + this->readValues(); } MotorControllerStates& OdriveMotor::getStates() @@ -140,94 +142,56 @@ MotorControllerStates& OdriveMotor::getStates() return states; } -uint16_t OdriveMotor::getAxisError() +void OdriveMotor::readValues() { - uint16_t axis_error; - std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); - if (this->read(command_name_, axis_error) == 1) - { - ROS_ERROR("Could not retrieve axis error"); - return ODRIVE_ERROR; - } + this->axis_error = this->readAxisError(); + this->axis_motor_error = this->readAxisMotorError(); + this->axis_encoder_error = this->readAxisEncoderError(); + this->axis_controller_error = this->readAxisControllerError(); + + this->motor_controller_voltage = this->readMotorControllerVoltage(); + this->motor_current = this->readMotorCurrent(); + this->motor_voltage = this->readMotorVoltage(); + + this->angle_counts_absolute = this->readAngleCountsAbsolute(); + this->velocity_rad_absolute = this->readVelocityRadAbsolute(); + this->angle_counts_incremental = this->readAngleCountsIncremental(); + this->velocity_rad_incremental = this->readVelocityRadIncremental(); +} - return axis_error; +uint16_t OdriveMotor::getAxisError() +{ + return this->axis_error; } uint16_t OdriveMotor::getAxisMotorError() { - uint16_t axis_motor_error; - std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); - if (this->read(command_name_, axis_motor_error) == 1) - { - ROS_ERROR("Could not retrieve axis motor error"); - return ODRIVE_ERROR; - } - - return axis_motor_error; + return this->axis_motor_error; } uint8_t OdriveMotor::getAxisEncoderError() { - uint8_t axis_encoder_error; - std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); - if (this->read(command_name_, axis_encoder_error) == 1) - { - ROS_ERROR("Could not retrieve axis encoder error"); - return ODRIVE_ERROR; - } - - return axis_encoder_error; + return this->axis_encoder_error; } uint8_t OdriveMotor::getAxisControllerError() { - uint8_t axis_controller_error; - std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); - if (this->read(command_name_, axis_controller_error) == 1) - { - ROS_ERROR("Could not retrieve axis controller error"); - return ODRIVE_ERROR; - } - - return axis_controller_error; + return this->axis_controller_error; } float OdriveMotor::getMotorControllerVoltage() { - float motor_controller_voltage; - std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE); - if (this->read(command_name_, motor_controller_voltage) == 1) - { - ROS_ERROR("Could not retrieve motor controller voltage"); - return ODRIVE_ERROR; - } - return motor_controller_voltage; + return this->motor_controller_voltage; } float OdriveMotor::getMotorCurrent() { - float motor_current; - std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); - if (this->read(command_name_, motor_current) == 1) - { - ROS_ERROR("Could not retrieve motor current"); - return ODRIVE_ERROR; - } - - return motor_current; + return this->motor_current; } float OdriveMotor::getMotorVoltage() { - float motor_voltage; - std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_VOLTAGE_D); - if (this->read(command_name_, motor_voltage) == 1) - { - ROS_ERROR("Could not retrieve motor voltage"); - return ODRIVE_ERROR; - } - - return motor_voltage; + return this->motor_voltage; } double OdriveMotor::getTorque() @@ -237,17 +201,18 @@ double OdriveMotor::getTorque() int OdriveMotor::getAngleCountsAbsolute() { - return 0; + return this->angle_counts_absolute; } double OdriveMotor::getAngleRadAbsolute() { - return 0; + double angle_rad = this->getAngleCountsAbsolute() * PI_2 / (std::pow(2, 12) * 101); + return angle_rad; } double OdriveMotor::getVelocityRadAbsolute() { - return 0; + return this->velocity_rad_absolute; } bool OdriveMotor::getIncrementalMorePrecise() const @@ -257,14 +222,7 @@ bool OdriveMotor::getIncrementalMorePrecise() const int OdriveMotor::getAngleCountsIncremental() { - float iu_position; - std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); - if (this->read(command_name_, iu_position) == 1) - { - ROS_ERROR("Could not retrieve incremental position of the encoder"); - return ODRIVE_ERROR - } - return iu_position; + return this->angle_counts_incremental; } double OdriveMotor::getAngleRadIncremental() @@ -275,25 +233,7 @@ double OdriveMotor::getAngleRadIncremental() double OdriveMotor::getVelocityRadIncremental() { - float iu_velocity; - std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); - if (this->read(command_name_, iu_velocity) == 1) - { - ROS_ERROR("Could not retrieve incremental velocity of the encoder"); - return ODRIVE_ERROR - } - - double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101); - return angle_rad; -} - -std::string OdriveMotor::create_command(std::string command_name) -{ - if (command_name.at(0) == '.') - { - return this->axis_number + command_name; - } - return command_name; + return this->velocity_rad_incremental; } int OdriveMotor::setState(uint8_t state) From e011f29349fc21b5d7c52827436634ffd7dbe90a Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Tue, 18 Aug 2020 21:19:53 +0200 Subject: [PATCH 24/28] added read after the initiation. --- march_hardware/src/odrive/odrive_motor.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index b777a0da9..c61c6c236 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -49,6 +49,7 @@ void OdriveMotor::prepareActuation() ROS_FATAL("Setting closed loop control was not finished successfully"); return; } + this->readValues(); } bool OdriveMotor::waitForIdleState(float timeout) @@ -227,13 +228,14 @@ int OdriveMotor::getAngleCountsIncremental() double OdriveMotor::getAngleRadIncremental() { - double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101); + double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101 * -1); return angle_rad; } double OdriveMotor::getVelocityRadIncremental() { - return this->velocity_rad_incremental; + double velocity_rad_incremental_double = this->velocity_rad_incremental * -1; + return velocity_rad_incremental_double; } int OdriveMotor::setState(uint8_t state) From ae85cddb2088cafd3e4dc58869f55a6fc5a62651 Mon Sep 17 00:00:00 2001 From: jorisweeda Date: Wed, 19 Aug 2020 17:24:18 +0200 Subject: [PATCH 25/28] Added all the configurations to launch two motors --- .../{march_odrive.json => right_hip_fe.json} | 5 -- march_hardware/config/right_knee.json | 74 +++++++++++++++++++ .../motor_controller/odrive/usb_master.h | 2 +- march_hardware/src/odrive/usb_master.cpp | 12 +-- .../march_hardware_builder/allowed_robot.h | 7 +- .../march_hardware_builder/hardware_builder.h | 6 +- .../robots/two_odrive_joints.yaml | 28 +++---- .../src/hardware_builder.cpp | 26 +++++-- .../config/two_odrive_joints/controllers.yaml | 12 +-- 9 files changed, 129 insertions(+), 43 deletions(-) rename march_hardware/config/{march_odrive.json => right_hip_fe.json} (93%) create mode 100644 march_hardware/config/right_knee.json diff --git a/march_hardware/config/march_odrive.json b/march_hardware/config/right_hip_fe.json similarity index 93% rename from march_hardware/config/march_odrive.json rename to march_hardware/config/right_hip_fe.json index ae3c4d913..f00a0a926 100644 --- a/march_hardware/config/march_odrive.json +++ b/march_hardware/config/right_hip_fe.json @@ -70,9 +70,4 @@ "type":"float", "value":0 }, - { - "name":"axis1.config.watchdog_timeout", - "type":"float", - "value":0 - } ] diff --git a/march_hardware/config/right_knee.json b/march_hardware/config/right_knee.json new file mode 100644 index 000000000..47bd65487 --- /dev/null +++ b/march_hardware/config/right_knee.json @@ -0,0 +1,74 @@ +[ + { + "name":"axis1.motor.config.current_lim", + "type":"float", + "value":30.0 + }, + { + "name":"axis1.encoder.config.cpr", + "type":"int32", + "value":4096 + }, + { + "name":"axis1.encoder.config.use_index", + "type":"bool", + "value":1 + }, + { + "name":"config.brake_resistance", + "type":"float", + "value":0.0 + }, + { + "name":"axis1.motor.config.pole_pairs", + "type":"int32", + "value":21 + }, + { + "name":"axis1.motor.config.calibration_current", + "type":"float", + "value":10.0 + }, + { + "name":"axis1.controller.config.vel_limit", + "type":"float", + "value":200000.0 + }, + { + "name":"axis1.motor.config.current_control_bandwidth", + "type":"float", + "value":100.0 + }, + + { + "name":"axis1.motor.config.motor_type", + "type":"uint8", + "value":0 + }, + { + "name":"axis1.controller.config.vel_gain", + "type":"float", + "value":0.01 + }, + { + "name":"axis1.controller.config.vel_integrator_gain", + "type":"float", + "value":0.05 + }, + { + "name":"axis1.controller.config.control_mode", + "type":"uint8", + "value":1 + }, + { + "name":"axis1.sensorless_estimator.config.pm_flux_linkage", + "type":"float", + "value":0.001944723 + }, + + { + "name":"axis1.config.watchdog_timeout", + "type":"float", + "value":0 + } +] diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h index be414e184..4c4847f3e 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h @@ -19,7 +19,7 @@ class UsbMaster std::shared_ptr getSerialConnection(const std::string& serial_number); private: - std::vector> odrive_endpoints; + std::vector> odrive_endpoints_; }; } // namespace march diff --git a/march_hardware/src/odrive/usb_master.cpp b/march_hardware/src/odrive/usb_master.cpp index aa90522b8..dbc8ab04f 100644 --- a/march_hardware/src/odrive/usb_master.cpp +++ b/march_hardware/src/odrive/usb_master.cpp @@ -6,20 +6,20 @@ namespace march { std::shared_ptr UsbMaster::getSerialConnection(const std::string& serial_number) { - for (uint i = 0; i < this->odrive_endpoints.size(); ++i) + for (uint i = 0; i < this->odrive_endpoints_.size(); ++i) { - if (odrive_endpoints[i]->getSerialNumber().compare(serial_number) == 0) + if (this->odrive_endpoints_[i]->getSerialNumber().compare(serial_number) == 0) { - return odrive_endpoints[i]; + return this->odrive_endpoints_[i]; } } std::shared_ptr odrive_endpoint = std::make_shared(); - ROS_INFO("Open serial connection %s", serial_number); + ROS_INFO("Open serial connection %s", serial_number.c_str()); odrive_endpoint->open_connection(serial_number); - odrive_endpoints.push_back(odrive_endpoint); - odrive_endpoint->getSerialNumber(); + this->odrive_endpoints_.push_back(odrive_endpoint); + return odrive_endpoint; } } // namespace march diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index 5dbd4111a..3f093a61c 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -42,7 +42,7 @@ class AllowedRobot } else if (robot_name == "two_odrive_joints") { - this->value = two_odrive_joints; + this->value = two_odrive_joints; } else if (robot_name == "test_joint_linear") { @@ -76,7 +76,7 @@ class AllowedRobot } else if (this->value == AllowedRobot::two_odrive_joints) { - return base_path.append("/robots/two_odrive_joints.yaml"); + return base_path.append("/robots/two_odrive_joints.yaml"); } else if (this->value == AllowedRobot::odrive_test_joint_rotational) { @@ -126,6 +126,9 @@ class AllowedRobot case odrive_test_joint_rotational: out << "odrive_test_joint_rotational"; break; + case two_odrive_joints: + out << "two_odrive_joints"; + break; case pdb: out << "pdb"; break; diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index 4fdb84cba..63a3a6886 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -71,7 +71,7 @@ class HardwareBuilder static march::Joint createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master); + march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master); static std::unique_ptr createAbsoluteEncoder(const YAML::Node& absolute_encoder_config, const urdf::JointConstSharedPtr& urdf_joint); static std::unique_ptr @@ -89,7 +89,7 @@ class HardwareBuilder static std::unique_ptr createOdrive(const YAML::Node& imc_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, - march::UsbMaster usb_master); + march::UsbMaster& usb_master); static const std::vector INCREMENTAL_ENCODER_REQUIRED_KEYS; static const std::vector ABSOLUTE_ENCODER_REQUIRED_KEYS; @@ -113,7 +113,7 @@ class HardwareBuilder * @return list of created joints */ std::vector createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface, march::UsbMaster usb_master) const; + march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master) const; YAML::Node robot_config_; urdf::Model urdf_; diff --git a/march_hardware_builder/robots/two_odrive_joints.yaml b/march_hardware_builder/robots/two_odrive_joints.yaml index f84f695e6..7579ecf8b 100644 --- a/march_hardware_builder/robots/two_odrive_joints.yaml +++ b/march_hardware_builder/robots/two_odrive_joints.yaml @@ -3,26 +3,26 @@ march4: cycleTime: 4 slaveTimeout: 50 joints: -# - right_hip_fe: -# actuationMode: torque -# allowActuation: true -# odrive: -# serial_number: "0x2084387E304E" -# axis: "axis0" -# absoluteEncoder: -# resolution: 17 -# minPositionIU: 2045 -# maxPositionIU: 45632 -# incrementalEncoder: -# resolution: 12 -# transmission: 101 + - right_hip_fe: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 2045 + maxPositionIU: 45632 + incrementalEncoder: + resolution: 12 + transmission: 101 - right_knee: actuationMode: torque allowActuation: true odrive: serial_number: "0x2084387E304E" - axis: "axis0" + axis: "axis1" absoluteEncoder: resolution: 17 minPositionIU: 41533 diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index fd1391737..90b4d6632 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -72,7 +72,7 @@ std::unique_ptr HardwareBuilder::createMarchRobot() auto pdo_interface = march::PdoInterfaceImpl::create(); auto sdo_interface = march::SdoInterfaceImpl::create(); - auto usb_master = march::UsbMaster(); + march::UsbMaster usb_master = march::UsbMaster(); std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface, usb_master); @@ -98,7 +98,7 @@ std::unique_ptr HardwareBuilder::createMarchRobot() march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, - march::UsbMaster usb_master) + march::UsbMaster& usb_master) { ROS_DEBUG("Starting creation of joint %s", joint_name.c_str()); if (!urdf_joint) @@ -152,7 +152,7 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::unique_ptr HardwareBuilder::createOdrive(const YAML::Node& odrive_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, - march::UsbMaster usb_master) + march::UsbMaster& usb_master) { if (!odrive_config || !urdf_joint) { @@ -165,11 +165,25 @@ std::unique_ptr HardwareBuilder::createOdrive(const YAML::No YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; std::string axis = odrive_config["axis"].as(); std::string serial_number = odrive_config["serial_number"].as(); - std::string json_configuration_path = ros::package::getPath("march_hardware").append("/config/march_odrive.json"); + + std::string configuration_path = + ros::package::getPath("march_hardware").append("/config/" + urdf_joint->name + ".json"); + + std::ifstream file(configuration_path); + + if (file.fail()) + { + throw std::runtime_error("Could not open configuration file for the odrive"); + } + + if (file.is_open()) + { + file.close(); + } auto odrive_endpoint = usb_master.getSerialConnection(serial_number); - return std::make_unique(axis, odrive_endpoint, mode, json_configuration_path); + return std::make_unique(axis, odrive_endpoint, mode, configuration_path); } std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, @@ -329,7 +343,7 @@ void HardwareBuilder::initUrdf() std::vector HardwareBuilder::createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, - march::UsbMaster usb_master) const + march::UsbMaster& usb_master) const { std::vector joints; for (const YAML::Node& joint_config : joints_config) diff --git a/march_hardware_interface/config/two_odrive_joints/controllers.yaml b/march_hardware_interface/config/two_odrive_joints/controllers.yaml index 3841042ad..e038f46c2 100644 --- a/march_hardware_interface/config/two_odrive_joints/controllers.yaml +++ b/march_hardware_interface/config/two_odrive_joints/controllers.yaml @@ -13,16 +13,16 @@ march: type: effort_controllers/JointTrajectoryController allow_partial_joints_goal: true joints: -# - right_hip_fe + - right_hip_fe - right_knee gains: # Required because we're controlling an effort interface -# right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} + right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} constraints: -# right_hip_fe: -# margin_soft_limit_error: 0.5 -# trajectory: 0.305 -# goal: 0.305 + right_hip_fe: + margin_soft_limit_error: 0.5 + trajectory: 0.305 + goal: 0.305 right_knee: margin_soft_limit_error: 0.5 trajectory: 0.305 From 7120112219005f1311be510a1755200e7c74bad4 Mon Sep 17 00:00:00 2001 From: roel Date: Fri, 21 Aug 2020 14:33:47 +0200 Subject: [PATCH 26/28] Reverse encoder counting --- march_hardware/src/odrive/odrive_motor.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp index c61c6c236..40ae398a2 100644 --- a/march_hardware/src/odrive/odrive_motor.cpp +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -228,7 +228,7 @@ int OdriveMotor::getAngleCountsIncremental() double OdriveMotor::getAngleRadIncremental() { - double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101 * -1); + double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101); return angle_rad; } From 55c9efb681fcf1b4bc5b97ee1dfaf3d769cfb1e1 Mon Sep 17 00:00:00 2001 From: roel Date: Fri, 21 Aug 2020 17:15:31 +0200 Subject: [PATCH 27/28] Fix merge conflicts --- .../motor_controller/odrive/odrive_states.h | 28 +++++++++++++++---- march_hardware_builder/robots/march3.yaml | 2 +- march_hardware_builder/robots/march4.yaml | 4 +-- march_hardware_builder/robots/pdb.yaml | 2 +- .../robots/test_joint_linear.yaml | 4 +-- .../robots/test_joint_rotational.yaml | 4 +-- .../src/march_hardware_interface_node.cpp | 2 +- 7 files changed, 32 insertions(+), 14 deletions(-) diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h index b5a6181c6..d854d844a 100644 --- a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h @@ -14,18 +14,36 @@ struct OdriveStates : public MotorControllerStates OdriveStates() = default; States state; + uint16_t axisError; + uint16_t axisMotorError; + uint16_t axisEncoderError; + uint16_t axisControllerError; - virtual bool checkState() + std::string axisErrorDescription; + std::string axisMotorErrorDescription; + std::string axisEncoderErrorDescription; + std::string axisControllerErrorDescription; + + bool checkState() override { - return true; + if (this->axisError == ERROR_NONE) + { + return true; + } + return false; } - virtual std::string getErrorStatus() + std::string getErrorStatus() override { std::ostringstream error_stream; - // std::string state = IMCStateOfOperation(this->statusWord).getString().c_str(); - error_stream << "State: " << state; + error_stream << "State: " << this->state << "\nAxis Error: " << this->axisError << " (" + << this->axisErrorDescription << ")" + << "\nMotor Error: " << this->axisMotorError << " (" << this->axisMotorErrorDescription << ")" + << "\nEncoder Error: " << this->axisEncoderError << " (" << this->axisEncoderErrorDescription << ")" + << "\nController Error: " << this->axisControllerError << " (" << this->axisControllerErrorDescription + << ")"; + return error_stream.str(); } }; diff --git a/march_hardware_builder/robots/march3.yaml b/march_hardware_builder/robots/march3.yaml index 15b819537..44d760365 100644 --- a/march_hardware_builder/robots/march3.yaml +++ b/march_hardware_builder/robots/march3.yaml @@ -1,6 +1,6 @@ march3: ifName: enp3s0 - ecatCycleTime: 4 + cycleTime: 4 joints: - right_hip: actuationMode: position diff --git a/march_hardware_builder/robots/march4.yaml b/march_hardware_builder/robots/march4.yaml index c1ee0b158..c895c200b 100644 --- a/march_hardware_builder/robots/march4.yaml +++ b/march_hardware_builder/robots/march4.yaml @@ -1,8 +1,8 @@ # For convenience it is easiest if the joint order is maintained, it is chosen to sort the joints alphabetically. march4: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - left_ankle: actuationMode: torque diff --git a/march_hardware_builder/robots/pdb.yaml b/march_hardware_builder/robots/pdb.yaml index fa355356f..115a9270c 100644 --- a/march_hardware_builder/robots/pdb.yaml +++ b/march_hardware_builder/robots/pdb.yaml @@ -1,6 +1,6 @@ pdb: ifName: enp2s0 - ecatCycleTime: 4 + cycleTime: 4 powerDistributionBoard: slaveIndex: 1 bootShutdownOffsets: diff --git a/march_hardware_builder/robots/test_joint_linear.yaml b/march_hardware_builder/robots/test_joint_linear.yaml index 72f44c9b8..7da655726 100644 --- a/march_hardware_builder/robots/test_joint_linear.yaml +++ b/march_hardware_builder/robots/test_joint_linear.yaml @@ -1,7 +1,7 @@ testjoint_linear: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - linear_joint: actuationMode: torque diff --git a/march_hardware_builder/robots/test_joint_rotational.yaml b/march_hardware_builder/robots/test_joint_rotational.yaml index 0755c9196..2cea384c8 100644 --- a/march_hardware_builder/robots/test_joint_rotational.yaml +++ b/march_hardware_builder/robots/test_joint_rotational.yaml @@ -1,7 +1,7 @@ testsetup: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - rotational_joint: actuationMode: torque diff --git a/march_hardware_interface/src/march_hardware_interface_node.cpp b/march_hardware_interface/src/march_hardware_interface_node.cpp index df99b6a7d..4f9d7279f 100644 --- a/march_hardware_interface/src/march_hardware_interface_node.cpp +++ b/march_hardware_interface/src/march_hardware_interface_node.cpp @@ -54,7 +54,7 @@ int main(int argc, char** argv) { try { - march.waitForPdo(); + march.waitForUpdate(); const ros::Time now = ros::Time::now(); ros::Duration elapsed_time = now - last_update_time; From 42299a0904766f9d2f30a719c487063df8c43f5d Mon Sep 17 00:00:00 2001 From: roel Date: Fri, 21 Aug 2020 17:20:16 +0200 Subject: [PATCH 28/28] Fix comment --- march_hardware_interface/src/march_hardware_interface.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/march_hardware_interface/src/march_hardware_interface.cpp b/march_hardware_interface/src/march_hardware_interface.cpp index b334a7b88..667c38a84 100644 --- a/march_hardware_interface/src/march_hardware_interface.cpp +++ b/march_hardware_interface/src/march_hardware_interface.cpp @@ -144,7 +144,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot &joint_temperature_variance_[i]); march_temperature_interface_.registerHandle(temperature_sensor_handle); - // Enable high voltage on the Motor controllers + // Prepare Motor Controllers for actuation if (joint.canActuate()) { joint.prepareActuation();