diff --git a/march_hardware/CMakeLists.txt b/march_hardware/CMakeLists.txt index 32aa9f66f..23190f008 100644 --- a/march_hardware/CMakeLists.txt +++ b/march_hardware/CMakeLists.txt @@ -53,10 +53,13 @@ add_library(${PROJECT_NAME} include/${PROJECT_NAME}/ethercat/pdo_types.h include/${PROJECT_NAME}/ethercat/sdo_interface.h include/${PROJECT_NAME}/ethercat/slave.h - include/${PROJECT_NAME}/imotioncube/actuation_mode.h - include/${PROJECT_NAME}/imotioncube/imotioncube.h - include/${PROJECT_NAME}/imotioncube/imotioncube_state.h - include/${PROJECT_NAME}/imotioncube/imotioncube_target_state.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}/power/boot_shutdown_offsets.h @@ -123,7 +126,7 @@ if(CATKIN_ENABLE_TESTING) test/joint_test.cpp test/mocks/mock_absolute_encoder.h test/mocks/mock_encoder.h - test/mocks/mock_imotioncube.h + test/mocks/mock_motor_controller.h test/mocks/mock_incremental_encoder.h test/mocks/mock_joint.h test/mocks/mock_pdo_interface.h diff --git a/march_hardware/include/march_hardware/ethercat/ethercat_master.h b/march_hardware/include/march_hardware/ethercat/ethercat_master.h index 955a86fb5..7499fb631 100644 --- a/march_hardware/include/march_hardware/ethercat/ethercat_master.h +++ b/march_hardware/include/march_hardware/ethercat/ethercat_master.h @@ -24,7 +24,7 @@ namespace march class EthercatMaster { public: - EthercatMaster(std::string ifname, int max_slave_index, int cycle_time, int slave_timeout); + EthercatMaster(std::string ifname, std::vector>, int cycle_time, int slave_timeout); ~EthercatMaster(); /* Delete copy constructor/assignment since the member thread can not be copied */ @@ -45,12 +45,20 @@ class EthercatMaster */ int getCycleTime() const; + /** + * Returns the largest slave index. + */ + int getMaxSlaveIndex(); + + bool hasValidSlaves(); + /** * Initializes the ethercat train and starts a thread for the loop. * @throws HardwareException If not the configured amount of slaves was found * or they did not all reach operational state + * @returns true if one of the slaves requires resetting ethercat, false otherwise */ - bool start(std::vector& joints); + bool start(); /** * Stops the ethercat loop and joins the thread. @@ -67,8 +75,10 @@ class EthercatMaster /** * Configures the found slaves to operational state. + * + * @returns true if one of the slaves requires resetting ethercat, false otherwise */ - bool ethercatSlaveInitiation(std::vector& joints); + bool ethercatSlaveInitiation(); /** * The ethercat train PDO loop. If the working counter is lower than @@ -112,6 +122,7 @@ class EthercatMaster std::atomic is_operational_; const std::string ifname_; + std::vector> slave_list_; const int max_slave_index_; const int cycle_time_ms_; diff --git a/march_hardware/include/march_hardware/ethercat/slave.h b/march_hardware/include/march_hardware/ethercat/slave.h index fe68186fa..0154a0c98 100644 --- a/march_hardware/include/march_hardware/ethercat/slave.h +++ b/march_hardware/include/march_hardware/ethercat/slave.h @@ -48,6 +48,11 @@ class Slave : public PdoSlaveInterface this->reset(sdo_slave_interface); } + virtual bool hasWatchdog() + { + return false; + } + protected: virtual bool initSdo(SdoSlaveInterface& /* sdo */, int /* cycle_time */) { diff --git a/march_hardware/include/march_hardware/joint.h b/march_hardware/include/march_hardware/joint.h index 059dcaf97..08ba016c0 100644 --- a/march_hardware/include/march_hardware/joint.h +++ b/march_hardware/include/march_hardware/joint.h @@ -7,10 +7,10 @@ #include #include -#include +#include #include #include -#include +#include namespace march { @@ -26,31 +26,30 @@ class Joint /** * Initializes a Joint with motor controller and without temperature slave. */ - Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr imc); + Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr controller); /** * Initializes a Joint with motor controller and temperature slave. */ - Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr imc, - std::unique_ptr temperature_ges); + Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr controller, + std::shared_ptr temperature_ges); virtual ~Joint() noexcept = default; - /* Delete copy constructor/assignment since the unique_ptr cannot be copied */ + /* Delete copy constructor/assignment since the shared_ptr cannot be copied */ Joint(const Joint&) = delete; Joint& operator=(const Joint&) = delete; - void resetIMotionCube(); + void resetMotorController(); /* Delete move assignment since string cannot be move assigned */ Joint(Joint&&) = default; Joint& operator=(Joint&&) = delete; - bool initialize(int cycle_time); void prepareActuation(); void actuateRad(double target_position); - void actuateTorque(int16_t target_torque); + void actuateTorque(double target_torque); void readEncoders(const ros::Duration& elapsed_time); double getPosition() const; @@ -58,75 +57,26 @@ class Joint double getVoltageVelocity() const; double getIncrementalPosition() const; double getAbsolutePosition() const; - int16_t getTorque(); - int32_t getAngleIUAbsolute(); - int32_t getAngleIUIncremental(); - double getVelocityIUAbsolute(); - double getVelocityIUIncremental(); + double getTorque(); float getTemperature(); - IMotionCubeState getIMotionCubeState(); + std::unique_ptr getMotorControllerStates(); std::string getName() const; - int getTemperatureGESSlaveIndex() const; - int getIMotionCubeSlaveIndex() const; int getNetNumber() const; ActuationMode getActuationMode() const; - bool hasIMotionCube() const; + bool hasMotorController() const; bool hasTemperatureGES() const; bool canActuate() const; bool receivedDataUpdate(); void setAllowActuation(bool allow_actuation); - /** @brief Override comparison operator */ - friend bool operator==(const Joint& lhs, const Joint& rhs) - { - return lhs.name_ == rhs.name_ && ((lhs.imc_ && rhs.imc_ && *lhs.imc_ == *rhs.imc_) || (!lhs.imc_ && !rhs.imc_)) && - ((lhs.temperature_ges_ && rhs.temperature_ges_ && *lhs.temperature_ges_ == *rhs.temperature_ges_) || - (!lhs.temperature_ges_ && !rhs.temperature_ges_)) && - lhs.allow_actuation_ == rhs.allow_actuation_ && - lhs.getActuationMode().getValue() == rhs.getActuationMode().getValue(); - } - - friend bool operator!=(const Joint& lhs, const Joint& rhs) - { - return !(lhs == rhs); - } - /** @brief Override stream operator for clean printing */ - friend ::std::ostream& operator<<(std::ostream& os, const Joint& joint) - { - os << "name: " << joint.name_ << ", " - << "ActuationMode: " << joint.getActuationMode().toString() << ", " - << "allowActuation: " << joint.allow_actuation_ << ", " - << "imotioncube: "; - if (joint.imc_) - { - os << *joint.imc_; - } - else - { - os << "none"; - } - - os << ", temperatureges: "; - if (joint.temperature_ges_) - { - os << *joint.temperature_ges_; - } - else - { - os << "none"; - } - - return os; - } - private: const std::string name_; const int net_number_; bool allow_actuation_ = false; - float previous_imc_volt_ = 0.0; + float previous_controller_volt_ = 0.0; float previous_motor_current_ = 0.0; float previous_motor_volt_ = 0.0; double previous_absolute_position_ = 0.0; @@ -139,8 +89,8 @@ class Joint double absolute_position_ = 0.0; double velocity_ = 0.0; - std::unique_ptr imc_ = nullptr; - std::unique_ptr temperature_ges_ = nullptr; + std::shared_ptr controller_ = nullptr; + std::shared_ptr temperature_ges_ = nullptr; }; } // namespace march diff --git a/march_hardware/include/march_hardware/march_robot.h b/march_hardware/include/march_hardware/march_robot.h index 3145d3f39..ff244e988 100644 --- a/march_hardware/include/march_hardware/march_robot.h +++ b/march_hardware/include/march_hardware/march_robot.h @@ -21,17 +21,17 @@ class MarchRobot ::std::vector jointList; urdf::Model urdf_; EthercatMaster ethercatMaster; - std::unique_ptr pdb_; + std::shared_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::string ifName, + std::vector> slave_list, int ecatCycleTime, int ecatSlaveTimeout); MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout); + std::shared_ptr powerDistributionBoard, ::std::string ifName, + std::vector> slave_list, int ecatCycleTime, int ecatSlaveTimeout); ~MarchRobot(); @@ -43,16 +43,14 @@ class MarchRobot MarchRobot(MarchRobot&&) = delete; MarchRobot& operator=(MarchRobot&&) = delete; - void resetIMotionCubes(); + void resetMotorControllers(); - void startEtherCAT(bool reset_imc); + void startEtherCAT(bool reset_motor_controllers); void stopEtherCAT(); int getMaxSlaveIndex(); - bool hasValidSlaves(); - bool isEthercatOperational(); std::exception_ptr getLastEthercatException() const noexcept; @@ -74,35 +72,6 @@ class MarchRobot PowerDistributionBoard* getPowerDistributionBoard() const; const urdf::Model& getUrdf() const; - - /** @brief Override comparison operator */ - friend bool operator==(const MarchRobot& lhs, const MarchRobot& rhs) - { - if (lhs.jointList.size() != rhs.jointList.size()) - { - return false; - } - for (unsigned int i = 0; i < lhs.jointList.size(); i++) - { - const march::Joint& lhsJoint = lhs.jointList.at(i); - const march::Joint& rhsJoint = rhs.jointList.at(i); - if (lhsJoint != rhsJoint) - { - return false; - } - } - return true; - } - - /** @brief Override stream operator for clean printing */ - friend ::std::ostream& operator<<(std::ostream& os, const MarchRobot& marchRobot) - { - for (unsigned int i = 0; i < marchRobot.jointList.size(); i++) - { - os << marchRobot.jointList.at(i) << "\n"; - } - return os; - } }; } // namespace march #endif // MARCH_HARDWARE_MARCH_ROBOT_H diff --git a/march_hardware/include/march_hardware/imotioncube/actuation_mode.h b/march_hardware/include/march_hardware/motor_controller/actuation_mode.h similarity index 100% rename from march_hardware/include/march_hardware/imotioncube/actuation_mode.h rename to march_hardware/include/march_hardware/motor_controller/actuation_mode.h diff --git a/march_hardware/include/march_hardware/imotioncube/imotioncube.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h similarity index 81% rename from march_hardware/include/march_hardware/imotioncube/imotioncube.h rename to march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h index 04baf0c80..76fa38472 100644 --- a/march_hardware/include/march_hardware/imotioncube/imotioncube.h +++ b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h @@ -2,12 +2,13 @@ #ifndef MARCH_HARDWARE_IMOTIONCUBE_H #define MARCH_HARDWARE_IMOTIONCUBE_H -#include "actuation_mode.h" +#include "march_hardware/motor_controller/actuation_mode.h" #include "march_hardware/ethercat/pdo_map.h" #include "march_hardware/ethercat/pdo_types.h" #include "march_hardware/ethercat/sdo_interface.h" #include "march_hardware/ethercat/slave.h" -#include "imotioncube_state.h" +#include "march_hardware/motor_controller/motor_controller.h" +#include "march_hardware/motor_controller/motor_controller_states.h" #include "imotioncube_target_state.h" #include "march_hardware/encoder/absolute_encoder.h" #include "march_hardware/encoder/incremental_encoder.h" @@ -18,7 +19,7 @@ namespace march { -class IMotionCube : public Slave +class IMotionCube : public MotorController, public Slave { public: /** @@ -36,42 +37,47 @@ class IMotionCube : public Slave std::unique_ptr incremental_encoder, std::string& sw_stream, ActuationMode actuation_mode); - ~IMotionCube() noexcept override = default; + virtual ~IMotionCube() noexcept override = default; /* Delete copy constructor/assignment since the unique_ptrs cannot be copied */ IMotionCube(const IMotionCube&) = delete; IMotionCube& operator=(const IMotionCube&) = delete; - virtual double getAngleRadAbsolute(); - virtual double getAngleRadIncremental(); + virtual double getAngleRadAbsolute() override; + virtual double getAngleRadIncremental() override; double getAbsoluteRadPerBit() const; double getIncrementalRadPerBit() const; - int16_t getTorque(); + bool getIncrementalMorePrecise() const override; + double getTorque() override; int32_t getAngleIUAbsolute(); int32_t getAngleIUIncremental(); double getVelocityIUAbsolute(); double getVelocityIUIncremental(); - virtual double getVelocityRadAbsolute(); - virtual double getVelocityRadIncremental(); + virtual double getVelocityRadAbsolute() override; + virtual double getVelocityRadIncremental() override; uint16_t getStatusWord(); uint16_t getMotionError(); uint16_t getDetailedError(); uint16_t getSecondDetailedError(); - ActuationMode getActuationMode() const; + ActuationMode getActuationMode() const override; - virtual float getMotorCurrent(); - virtual float getIMCVoltage(); - virtual float getMotorVoltage(); + virtual float getMotorCurrent() override; + virtual float getMotorControllerVoltage() override; + virtual float getMotorVoltage() override; - void setControlWord(uint16_t control_word); + std::unique_ptr getStates() override; - virtual void actuateRad(double target_rad); - virtual void actuateTorque(int16_t target_torque); + void setControlWord(uint16_t control_word); + virtual void actuateRad(double target_rad) override; + virtual void actuateTorque(double target_torque_ampere) override; void goToTargetState(IMotionCubeTargetState target_state); - virtual void goToOperationEnabled(); + virtual void prepareActuation() override; + + virtual void reset() override; + bool hasWatchdog() override; /** @brief Override comparison operator */ friend bool operator==(const IMotionCube& lhs, const IMotionCube& rhs) @@ -90,7 +96,9 @@ class IMotionCube : public Slave constexpr static double MAX_TARGET_DIFFERENCE = 0.393; // This value is slightly larger than the current limit of the // linear joints defined in the URDF. - const static int16_t MAX_TARGET_TORQUE = 23500; + constexpr static double IPEAK = 40; + // See CoE manual page 222 + constexpr static double AMPERE_TO_IU_FACTOR = 65520; // Watchdog base time = 1 / 25 MHz * (2498 + 2) = 0.0001 seconds=100 µs static const uint16_t WATCHDOG_DIVIDER = 2498; @@ -104,6 +112,7 @@ class IMotionCube : public Slave private: void actuateIU(int32_t target_iu); + int16_t ampereToTorqueIU(double ampere); void mapMisoPDOs(SdoSlaveInterface& sdo); void mapMosiPDOs(SdoSlaveInterface& sdo); diff --git a/march_hardware/include/march_hardware/imotioncube/imotioncube_state.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_state_of_operation.h similarity index 66% rename from march_hardware/include/march_hardware/imotioncube/imotioncube_state.h rename to march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_state_of_operation.h index 4c3e7e503..65cb80449 100644 --- a/march_hardware/include/march_hardware/imotioncube/imotioncube_state.h +++ b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_state_of_operation.h @@ -1,12 +1,12 @@ // Copyright 2019 Project March. -#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATE_H -#define MARCH_HARDWARE_IMOTIONCUBE_STATE_H +#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H +#define MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H #include namespace march { -class IMCState +class IMCStateOfOperation { public: enum Value @@ -22,11 +22,11 @@ class IMCState UNKNOWN, }; - IMCState() : value_(UNKNOWN) + IMCStateOfOperation() : value_(UNKNOWN) { } - explicit IMCState(uint16_t status) : value_(UNKNOWN) + explicit IMCStateOfOperation(uint16_t status) : value_(UNKNOWN) { const uint16_t five_bit_mask = 0b0000000001001111; const uint16_t six_bit_mask = 0b0000000001101111; @@ -45,35 +45,35 @@ class IMCState if (five_bit_masked == not_ready_switch_on) { - this->value_ = IMCState::NOT_READY_TO_SWITCH_ON; + this->value_ = IMCStateOfOperation::NOT_READY_TO_SWITCH_ON; } else if (five_bit_masked == switch_on_disabled) { - this->value_ = IMCState::SWITCH_ON_DISABLED; + this->value_ = IMCStateOfOperation::SWITCH_ON_DISABLED; } else if (six_bit_masked == ready_to_switch_on) { - this->value_ = IMCState::READY_TO_SWITCH_ON; + this->value_ = IMCStateOfOperation::READY_TO_SWITCH_ON; } else if (six_bit_masked == switched_on) { - this->value_ = IMCState::SWITCHED_ON; + this->value_ = IMCStateOfOperation::SWITCHED_ON; } else if (six_bit_masked == operation_enabled) { - this->value_ = IMCState::OPERATION_ENABLED; + this->value_ = IMCStateOfOperation::OPERATION_ENABLED; } else if (six_bit_masked == quick_stop_active) { - this->value_ = IMCState::QUICK_STOP_ACTIVE; + this->value_ = IMCStateOfOperation::QUICK_STOP_ACTIVE; } else if (five_bit_masked == fault_reaction_active) { - this->value_ = IMCState::FAULT_REACTION_ACTIVE; + this->value_ = IMCStateOfOperation::FAULT_REACTION_ACTIVE; } else if (five_bit_masked == fault) { - this->value_ = IMCState::FAULT; + this->value_ = IMCStateOfOperation::FAULT; } } @@ -99,6 +99,8 @@ class IMCState return "Fault"; case UNKNOWN: return "Not in a recognized IMC state"; + default: + return "Not in a recognized IMC state"; } } @@ -106,11 +108,11 @@ class IMCState { return this->value_ == v; } - bool operator==(IMCState a) const + bool operator==(IMCStateOfOperation a) const { return this->value_ == a.value_; } - bool operator!=(IMCState a) const + bool operator!=(IMCStateOfOperation a) const { return this->value_ != a.value_; } @@ -118,30 +120,6 @@ class IMCState private: Value value_; }; - -struct IMotionCubeState -{ -public: - IMotionCubeState() = default; - - std::string statusWord; - std::string motionError; - std::string detailedError; - std::string secondDetailedError; - IMCState state; - std::string detailedErrorDescription; - std::string motionErrorDescription; - std::string secondDetailedErrorDescription; - - float motorCurrent; - float IMCVoltage; - float motorVoltage; - int absoluteEncoderValue; - int incrementalEncoderValue; - double absoluteVelocity; - double incrementalVelocity; -}; - } // namespace march -#endif // MARCH_HARDWARE_IMOTIONCUBE_STATE_H +#endif // MARCH_HARDWARE_IMOTIONCUBE_STATE_OF_OPERATION_H diff --git a/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_states.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_states.h new file mode 100644 index 000000000..8234f5cea --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_states.h @@ -0,0 +1,45 @@ +// 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 "imotioncube_state_of_operation.h" + +namespace march +{ +struct IMotionCubeStates : public MotorControllerStates +{ +public: + IMotionCubeStates() = default; + + uint16_t status_word; + std::string motion_error; + std::string detailed_error; + std::string second_detailed_error; + IMCStateOfOperation state; + std::string detailed_error_description; + std::string motion_error_description; + std::string second_detailed_error_description; + + bool checkState() override + { + return !(this->state == march::IMCStateOfOperation::FAULT); + } + + std::string getErrorStatus() override + { + std::ostringstream error_stream; + std::string state = IMCStateOfOperation(this->status_word).getString(); + + error_stream << "State: " << state << "\nMotion Error: " << this->motion_error_description << " (" + << this->motion_error << ")\nDetailed Error: " << this->detailed_error_description << " (" + << this->detailed_error << ")\nSecond Detailed Error: " << this->second_detailed_error_description + << " (" << this->second_detailed_error << ")"; + return error_stream.str(); + } +}; + +} // namespace march + +#endif // MARCH_HARDWARE_IMOTIONCUBE_STATES_H diff --git a/march_hardware/include/march_hardware/imotioncube/imotioncube_target_state.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_target_state.h similarity index 100% rename from march_hardware/include/march_hardware/imotioncube/imotioncube_target_state.h rename to march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube_target_state.h diff --git a/march_hardware/include/march_hardware/motor_controller/motor_controller.h b/march_hardware/include/march_hardware/motor_controller/motor_controller.h new file mode 100644 index 000000000..cbb4797fc --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/motor_controller.h @@ -0,0 +1,50 @@ +// Copyright 2019 Project March. + +#ifndef MARCH_HARDWARE_MOTOR_CONTROLLER_H +#define MARCH_HARDWARE_MOTOR_CONTROLLER_H +#include "actuation_mode.h" +#include "motor_controller_states.h" +#include + +namespace march +{ +class MotorController +{ +public: + virtual double getAngleRadAbsolute() = 0; + virtual double getAngleRadIncremental() = 0; + virtual double getVelocityRadAbsolute() = 0; + virtual double getVelocityRadIncremental() = 0; + virtual double getTorque() = 0; + + virtual ActuationMode getActuationMode() const = 0; + + virtual float getMotorCurrent() = 0; + virtual float getMotorControllerVoltage() = 0; + virtual float getMotorVoltage() = 0; + + /** + * Get whether the incremental encoder is more precise than the absolute encoder + * @return true if the incremental encoder has a higher resolution than the absolute encoder, false otherwise + */ + virtual bool getIncrementalMorePrecise() const = 0; + + virtual void actuateRad(double target_rad) = 0; + virtual void actuateTorque(double target_torque_ampere) = 0; + + virtual void prepareActuation() = 0; + virtual void reset() = 0; + + /** + * Get the most recent states of the motor controller, i.e. all data that is read from the controller at every + * communication cycle. + * @return A MotorControllerState object containing all data read from the motor controller at every communication + * cycle. + */ + virtual std::unique_ptr getStates() = 0; + + virtual ~MotorController() noexcept = default; +}; + +} // namespace march +#endif // MARCH_HARDWARE_MOTOR_CONTROLLER_H diff --git a/march_hardware/include/march_hardware/motor_controller/motor_controller_states.h b/march_hardware/include/march_hardware/motor_controller/motor_controller_states.h new file mode 100644 index 000000000..7ad997b9a --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/motor_controller_states.h @@ -0,0 +1,36 @@ +// Copyright 2019 Project March. +#ifndef MARCH_HARDWARE_MOTOR_CONTROLLER_STATES_H +#define MARCH_HARDWARE_MOTOR_CONTROLLER_STATES_H + +#include + +namespace march +{ +struct MotorControllerStates +{ +public: + MotorControllerStates() = default; + + float motor_current; + float controller_voltage; + float motor_voltage; + int absolute_encoder_value; + int incremental_encoder_value; + double absolute_velocity; + double incremental_velocity; + + /** + * Check whether the motor controller is in an error state + * @return false if the motor controller is in error state, otherwise true + */ + virtual bool checkState() = 0; + /** + * Get a string description of the state and error states of the motor controller + * @return string describing the current state as well as the error state(s) of the motor controller + */ + virtual std::string getErrorStatus() = 0; +}; + +} // namespace march + +#endif // MARCH_HARDWARE_MOTOR_CONTROLLER_STATES_H diff --git a/march_hardware/src/ethercat/ethercat_master.cpp b/march_hardware/src/ethercat/ethercat_master.cpp index 7aab67885..555efce18 100644 --- a/march_hardware/src/ethercat/ethercat_master.cpp +++ b/march_hardware/src/ethercat/ethercat_master.cpp @@ -14,10 +14,12 @@ namespace march { -EthercatMaster::EthercatMaster(std::string ifname, int max_slave_index, int cycle_time, int slave_timeout) +EthercatMaster::EthercatMaster(std::string ifname, std::vector> slave_list, int cycle_time, + int slave_timeout) : is_operational_(false) , ifname_(std::move(ifname)) - , max_slave_index_(max_slave_index) + , slave_list_(slave_list) + , max_slave_index_(this->getMaxSlaveIndex()) , cycle_time_ms_(cycle_time) , slave_watchdog_timeout_(slave_timeout) { @@ -38,6 +40,17 @@ int EthercatMaster::getCycleTime() const return this->cycle_time_ms_; } +int EthercatMaster::getMaxSlaveIndex() +{ + uint16_t max_slave_index = 0; + + for (std::shared_ptr slave : this->slave_list_) + { + max_slave_index = std::max(max_slave_index, slave->getSlaveIndex()); + } + return max_slave_index; +} + void EthercatMaster::waitForPdo() { std::unique_lock lock(this->wait_on_pdo_condition_mutex_); @@ -50,11 +63,41 @@ std::exception_ptr EthercatMaster::getLastException() const noexcept return this->last_exception_; } -bool EthercatMaster::start(std::vector& joints) +bool EthercatMaster::start() { + if (!this->hasValidSlaves()) + { + throw error::HardwareException(error::ErrorType::INVALID_SLAVE_CONFIGURATION); + } + ROS_INFO("Slave configuration is non-conflicting"); + this->last_exception_ = nullptr; this->ethercatMasterInitiation(); - return this->ethercatSlaveInitiation(joints); + return this->ethercatSlaveInitiation(); +} + +bool EthercatMaster::hasValidSlaves() +{ + ::std::vector slaveIndices; + for (auto slave : this->slave_list_) + { + slaveIndices.push_back(slave->getSlaveIndex()); + } + + if (slaveIndices.size() == 1) + { + ROS_INFO("Found configuration for 1 slave."); + return true; + } + + ROS_INFO("Found configuration for %lu slaves.", slaveIndices.size()); + + // Sort the indices and check for duplicates. + // If there are no duplicates, the configuration is valid. + ::std::sort(slaveIndices.begin(), slaveIndices.end()); + auto it = ::std::unique(slaveIndices.begin(), slaveIndices.end()); + bool isUnique = (it == slaveIndices.end()); + return isUnique; } void EthercatMaster::ethercatMasterInitiation() @@ -87,19 +130,19 @@ int setSlaveWatchdogTimer(uint16 slave) return 1; } -bool EthercatMaster::ethercatSlaveInitiation(std::vector& joints) +bool EthercatMaster::ethercatSlaveInitiation() { ROS_INFO("Request pre-operational state for all slaves"); bool reset = false; ec_statecheck(0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4); - for (Joint& joint : joints) + for (std::shared_ptr slave : this->slave_list_) { - if (joint.hasIMotionCube()) + reset |= slave->initSdo(this->cycle_time_ms_); + if (slave->hasWatchdog()) { - ec_slave[joint.getIMotionCubeSlaveIndex()].PO2SOconfig = setSlaveWatchdogTimer; + ec_slave[slave->getSlaveIndex()].PO2SOconfig = setSlaveWatchdogTimer; } - reset |= joint.initialize(this->cycle_time_ms_); } ec_config_map(&this->io_map_); diff --git a/march_hardware/src/imotioncube/imotioncube.cpp b/march_hardware/src/imotioncube/imotioncube.cpp index 75fd3b077..7573c07ab 100644 --- a/march_hardware/src/imotioncube/imotioncube.cpp +++ b/march_hardware/src/imotioncube/imotioncube.cpp @@ -1,5 +1,6 @@ // Copyright 2018 Project March. -#include "march_hardware/imotioncube/imotioncube.h" +#include "march_hardware/motor_controller/imotioncube/imotioncube.h" +#include "march_hardware/motor_controller/imotioncube/imotioncube_states.h" #include "march_hardware/error/hardware_exception.h" #include "march_hardware/error/motion_error.h" #include "march_hardware/ethercat/pdo_types.h" @@ -178,8 +179,15 @@ void IMotionCube::actuateIU(int32_t target_iu) this->write32(target_position_location, target_position); } -void IMotionCube::actuateTorque(int16_t target_torque) +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); + } + int16_t target_torque = ampereToTorqueIU(target_torque_ampere); + if (this->actuation_mode_ != ActuationMode::torque) { throw error::HardwareException(error::ErrorType::INVALID_ACTUATION_MODE, @@ -187,12 +195,6 @@ void IMotionCube::actuateTorque(int16_t target_torque) this->actuation_mode_.toString().c_str()); } - if (target_torque >= MAX_TARGET_TORQUE) - { - throw error::HardwareException(error::ErrorType::TARGET_TORQUE_EXCEEDS_MAX_TORQUE, - "Target torque of %d exceeds max torque of %d", target_torque, MAX_TARGET_TORQUE); - } - bit16 target_torque_struct = { .i = target_torque }; uint8_t target_torque_location = this->mosi_byte_offsets_.at(IMCObjectName::TargetTorque); @@ -220,6 +222,11 @@ double IMotionCube::getAngleRadIncremental() return this->incremental_encoder_->getAngleRad(*this, this->miso_byte_offsets_.at(IMCObjectName::MotorPosition)); } +bool IMotionCube::getIncrementalMorePrecise() const +{ + return this->incremental_encoder_->getRadPerBit() < this->absolute_encoder_->getRadPerBit(); +} + double IMotionCube::getAbsoluteRadPerBit() const { return this->absolute_encoder_->getRadPerBit(); @@ -230,7 +237,7 @@ double IMotionCube::getIncrementalRadPerBit() const return this->incremental_encoder_->getRadPerBit(); } -int16_t IMotionCube::getTorque() +double IMotionCube::getTorque() { bit16 return_byte = this->read16(this->miso_byte_offsets_.at(IMCObjectName::ActualTorque)); return return_byte.i; @@ -296,7 +303,7 @@ float IMotionCube::getMotorCurrent() static_cast(motor_current_iu); // Conversion to Amp, see Technosoft CoE programming manual } -float IMotionCube::getIMCVoltage() +float IMotionCube::getMotorControllerVoltage() { // maximum measurable DC voltage found in EMS Setup/Drive info button const float V_DC_MAX_MEASURABLE = 102.3; @@ -319,6 +326,39 @@ void IMotionCube::setControlWord(uint16_t control_word) this->write16(this->mosi_byte_offsets_.at(IMCObjectName::ControlWord), control_word_ui); } +std::unique_ptr IMotionCube::getStates() +{ + std::unique_ptr states = std::make_unique(); + + // Common states + states->motor_current = this->getMotorCurrent(); + states->controller_voltage = this->getMotorControllerVoltage(); + states->motor_voltage = this->getMotorVoltage(); + + states->absolute_encoder_value = this->getAngleIUAbsolute(); + states->incremental_encoder_value = this->getAngleIUIncremental(); + states->absolute_velocity = this->getVelocityIUAbsolute(); + states->incremental_velocity = this->getVelocityIUIncremental(); + + states->status_word = this->getStatusWord(); + std::bitset<16> motion_error_bits = this->getMotionError(); + states->motion_error = motion_error_bits.to_string(); + std::bitset<16> detailed_error_bits = this->getDetailedError(); + states->detailed_error = detailed_error_bits.to_string(); + std::bitset<16> second_detailed_error_bits = this->getSecondDetailedError(); + states->second_detailed_error = second_detailed_error_bits.to_string(); + + states->state = IMCStateOfOperation(this->getStatusWord()); + + states->motion_error_description = error::parseError(this->getMotionError(), error::ErrorRegisters::MOTION_ERROR); + states->detailed_error_description = + error::parseError(this->getDetailedError(), error::ErrorRegisters::DETAILED_ERROR); + states->second_detailed_error_description = + error::parseError(this->getSecondDetailedError(), error::ErrorRegisters::SECOND_DETAILED_ERROR); + + return states; +} + void IMotionCube::goToTargetState(IMotionCubeTargetState target_state) { ROS_DEBUG("\tTry to go to '%s'", target_state.getDescription().c_str()); @@ -328,7 +368,7 @@ void IMotionCube::goToTargetState(IMotionCubeTargetState target_state) ROS_INFO_DELAYED_THROTTLE(5, "\tWaiting for '%s': %s", target_state.getDescription().c_str(), std::bitset<16>(this->getStatusWord()).to_string().c_str()); if (target_state.getState() == IMotionCubeTargetState::OPERATION_ENABLED.getState() && - IMCState(this->getStatusWord()) == IMCState::FAULT) + IMCStateOfOperation(this->getStatusWord()) == IMCStateOfOperation::FAULT) { ROS_FATAL("IMotionCube went to fault state while attempting to go to '%s'. Shutting down.", target_state.getDescription().c_str()); @@ -346,7 +386,7 @@ void IMotionCube::goToTargetState(IMotionCubeTargetState target_state) ROS_DEBUG("\tReached '%s'!", target_state.getDescription().c_str()); } -void IMotionCube::goToOperationEnabled() +void IMotionCube::prepareActuation() { if (this->actuation_mode_ == ActuationMode::unknown) { @@ -396,6 +436,11 @@ void IMotionCube::reset(SdoSlaveInterface& sdo) sdo.write(0x2080, 0, 1); } +void IMotionCube::reset() +{ + return this->Slave::reset(); +} + uint16_t IMotionCube::computeSWCheckSum(uint16_t& start_address, uint16_t& end_address) { size_t pos = 0; @@ -497,8 +542,19 @@ void IMotionCube::downloadSetupToDrive(SdoSlaveInterface& sdo) } } +bool IMotionCube::hasWatchdog() +{ + return true; +} + ActuationMode IMotionCube::getActuationMode() const { return this->actuation_mode_; } + +int16_t IMotionCube::ampereToTorqueIU(double ampere) +{ + // See CoE manual page 222 + return AMPERE_TO_IU_FACTOR * ampere / (2 * IPEAK); +} } // namespace march diff --git a/march_hardware/src/imotioncube/imotioncube_target_state.cpp b/march_hardware/src/imotioncube/imotioncube_target_state.cpp index 980b464c9..89413e88e 100644 --- a/march_hardware/src/imotioncube/imotioncube_target_state.cpp +++ b/march_hardware/src/imotioncube/imotioncube_target_state.cpp @@ -1,6 +1,6 @@ // Copyright 2019 Project March. -#include +#include namespace march { diff --git a/march_hardware/src/joint.cpp b/march_hardware/src/joint.cpp index 3a4afce1f..d84e80f78 100644 --- a/march_hardware/src/joint.cpp +++ b/march_hardware/src/joint.cpp @@ -1,8 +1,6 @@ // Copyright 2019 Project March. -#include "march_hardware/ethercat/slave.h" #include "march_hardware/joint.h" #include "march_hardware/error/hardware_exception.h" -#include "march_hardware/error/motion_error.h" #include @@ -18,35 +16,24 @@ Joint::Joint(std::string name, int net_number) : name_(std::move(name)), net_num { } -Joint::Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr imc) - : name_(std::move(name)), net_number_(net_number), allow_actuation_(allow_actuation), imc_(std::move(imc)) +Joint::Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr controller) + : name_(std::move(name)) + , net_number_(net_number) + , allow_actuation_(allow_actuation) + , controller_(std::move(controller)) { } -Joint::Joint(std::string name, int net_number, bool allow_actuation, std::unique_ptr imc, - std::unique_ptr temperature_ges) +Joint::Joint(std::string name, int net_number, bool allow_actuation, std::shared_ptr controller, + std::shared_ptr temperature_ges) : name_(std::move(name)) , net_number_(net_number) , allow_actuation_(allow_actuation) - , imc_(std::move(imc)) + , controller_(std::move(controller)) , temperature_ges_(std::move(temperature_ges)) { } -bool Joint::initialize(int cycle_time) -{ - bool reset = false; - if (this->hasIMotionCube()) - { - reset |= this->imc_->Slave::initSdo(cycle_time); - } - if (this->hasTemperatureGES()) - { - reset |= this->temperature_ges_->initSdo(cycle_time); - } - return reset; -} - void Joint::prepareActuation() { if (!this->canActuate()) @@ -55,25 +42,18 @@ void Joint::prepareActuation() this->name_.c_str()); } ROS_INFO("[%s] Preparing for actuation", this->name_.c_str()); - this->imc_->goToOperationEnabled(); + this->controller_->prepareActuation(); ROS_INFO("[%s] Successfully prepared for actuation", this->name_.c_str()); - this->incremental_position_ = this->imc_->getAngleRadIncremental(); - this->absolute_position_ = this->imc_->getAngleRadAbsolute(); + this->incremental_position_ = this->controller_->getAngleRadIncremental(); + this->absolute_position_ = this->controller_->getAngleRadAbsolute(); this->position_ = this->absolute_position_; this->velocity_ = 0; } -void Joint::resetIMotionCube() +void Joint::resetMotorController() { - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - } - else - { - this->imc_->Slave::reset(); - } + this->controller_->reset(); } void Joint::actuateRad(double target_position) @@ -83,34 +63,29 @@ void Joint::actuateRad(double target_position) throw error::HardwareException(error::ErrorType::NOT_ALLOWED_TO_ACTUATE, "Joint %s is not allowed to actuate", this->name_.c_str()); } - this->imc_->actuateRad(target_position); + this->controller_->actuateRad(target_position); } void Joint::readEncoders(const ros::Duration& elapsed_time) { - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return; - } - if (this->receivedDataUpdate()) { - const double incremental_position_change = this->imc_->getAngleRadIncremental() - this->incremental_position_; + const double incremental_position_change = + this->controller_->getAngleRadIncremental() - this->incremental_position_; // Take the velocity and position from the encoder with the highest resolution. - if (this->imc_->getIncrementalRadPerBit() < this->imc_->getAbsoluteRadPerBit()) + if (this->controller_->getIncrementalMorePrecise()) { - this->velocity_ = this->imc_->getVelocityRadIncremental(); + this->velocity_ = this->controller_->getVelocityRadIncremental(); this->position_ += incremental_position_change; } else { - this->velocity_ = this->imc_->getVelocityRadAbsolute(); - this->position_ = this->imc_->getAngleRadAbsolute(); + this->velocity_ = this->controller_->getVelocityRadAbsolute(); + this->position_ = this->controller_->getAngleRadAbsolute(); } this->incremental_position_ += incremental_position_change; - this->absolute_position_ = this->imc_->getAngleRadAbsolute(); + this->absolute_position_ = this->controller_->getAngleRadAbsolute(); } else { @@ -140,7 +115,7 @@ double Joint::getVoltageVelocity() const const double velocity_constant = 355; const double rpm_to_rad = M_PI / 30; const double electric_constant = velocity_constant * rpm_to_rad; - return (this->imc_->getMotorVoltage() + this->imc_->getMotorCurrent() * resistance) / electric_constant; + return (this->controller_->getMotorVoltage() + this->controller_->getMotorCurrent() * resistance) / electric_constant; } double Joint::getIncrementalPosition() const @@ -153,64 +128,19 @@ double Joint::getAbsolutePosition() const return this->absolute_position_; } -void Joint::actuateTorque(int16_t target_torque) +void Joint::actuateTorque(double target_torque) { if (!this->canActuate()) { throw error::HardwareException(error::ErrorType::NOT_ALLOWED_TO_ACTUATE, "Joint %s is not allowed to actuate", this->name_.c_str()); } - this->imc_->actuateTorque(target_torque); -} - -int16_t Joint::getTorque() -{ - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return -1; - } - return this->imc_->getTorque(); + this->controller_->actuateTorque(target_torque); } -int32_t Joint::getAngleIUAbsolute() +double Joint::getTorque() { - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return -1; - } - return this->imc_->getAngleIUAbsolute(); -} - -int32_t Joint::getAngleIUIncremental() -{ - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return -1; - } - return this->imc_->getAngleIUIncremental(); -} - -double Joint::getVelocityIUAbsolute() -{ - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return -1; - } - return this->imc_->getVelocityIUAbsolute(); -} - -double Joint::getVelocityIUIncremental() -{ - if (!this->hasIMotionCube()) - { - ROS_WARN("[%s] Has no iMotionCube", this->name_.c_str()); - return -1; - } - return this->imc_->getVelocityIUIncremental(); + return this->controller_->getTorque(); } float Joint::getTemperature() @@ -223,37 +153,9 @@ float Joint::getTemperature() return this->temperature_ges_->getTemperature(); } -IMotionCubeState Joint::getIMotionCubeState() +std::unique_ptr Joint::getMotorControllerStates() { - IMotionCubeState states; - - std::bitset<16> statusWordBits = this->imc_->getStatusWord(); - states.statusWord = statusWordBits.to_string(); - std::bitset<16> motionErrorBits = this->imc_->getMotionError(); - states.motionError = motionErrorBits.to_string(); - std::bitset<16> detailedErrorBits = this->imc_->getDetailedError(); - states.detailedError = detailedErrorBits.to_string(); - std::bitset<16> secondDetailedErrorBits = this->imc_->getSecondDetailedError(); - states.secondDetailedError = secondDetailedErrorBits.to_string(); - - states.state = IMCState(this->imc_->getStatusWord()); - - states.motionErrorDescription = error::parseError(this->imc_->getMotionError(), error::ErrorRegisters::MOTION_ERROR); - states.detailedErrorDescription = - error::parseError(this->imc_->getDetailedError(), error::ErrorRegisters::DETAILED_ERROR); - states.secondDetailedErrorDescription = - error::parseError(this->imc_->getSecondDetailedError(), error::ErrorRegisters::SECOND_DETAILED_ERROR); - - states.motorCurrent = this->imc_->getMotorCurrent(); - states.IMCVoltage = this->imc_->getIMCVoltage(); - states.motorVoltage = this->imc_->getMotorVoltage(); - - states.absoluteEncoderValue = this->imc_->getAngleIUAbsolute(); - states.incrementalEncoderValue = this->imc_->getAngleIUIncremental(); - states.absoluteVelocity = this->imc_->getVelocityIUAbsolute(); - states.incrementalVelocity = this->imc_->getVelocityIUIncremental(); - - return states; + return this->controller_->getStates(); } void Joint::setAllowActuation(bool allow_actuation) @@ -261,24 +163,6 @@ void Joint::setAllowActuation(bool allow_actuation) this->allow_actuation_ = allow_actuation; } -int Joint::getTemperatureGESSlaveIndex() const -{ - if (this->hasTemperatureGES()) - { - return this->temperature_ges_->getSlaveIndex(); - } - return -1; -} - -int Joint::getIMotionCubeSlaveIndex() const -{ - if (this->hasIMotionCube()) - { - return this->imc_->getSlaveIndex(); - } - return -1; -} - int Joint::getNetNumber() const { return this->net_number_; @@ -289,9 +173,9 @@ std::string Joint::getName() const return this->name_; } -bool Joint::hasIMotionCube() const +bool Joint::hasMotorController() const { - return this->imc_ != nullptr; + return this->controller_ != nullptr; } bool Joint::hasTemperatureGES() const @@ -301,32 +185,29 @@ bool Joint::hasTemperatureGES() const bool Joint::canActuate() const { - return this->allow_actuation_ && this->hasIMotionCube(); + return this->allow_actuation_ && this->hasMotorController(); } bool Joint::receivedDataUpdate() { - if (!this->hasIMotionCube()) - { - return false; - } // If imc voltage, motor current, and both encoders positions and velocities did not change, // we probably did not receive an update for this joint. - float new_imc_volt = this->imc_->getIMCVoltage(); - float new_motor_volt = this->imc_->getMotorVoltage(); - float new_motor_current = this->imc_->getMotorCurrent(); - double new_absolute_position = this->imc_->getAngleRadAbsolute(); - double new_incremental_position = this->imc_->getAngleRadIncremental(); - double new_absolute_velocity = this->imc_->getVelocityRadAbsolute(); - double new_incremental_velocity = this->imc_->getVelocityRadIncremental(); - bool data_updated = (new_imc_volt != this->previous_imc_volt_ || new_motor_volt != this->previous_motor_volt_ || - new_motor_current != this->previous_motor_current_ || - new_absolute_position != this->previous_absolute_position_ || - new_incremental_position != this->previous_incremental_position_ || - new_absolute_velocity != this->previous_absolute_velocity_ || - new_incremental_velocity != this->previous_incremental_velocity_); - - this->previous_imc_volt_ = new_imc_volt; + float new_controller_volt = this->controller_->getMotorControllerVoltage(); + float new_motor_volt = this->controller_->getMotorVoltage(); + float new_motor_current = this->controller_->getMotorCurrent(); + double new_absolute_position = this->controller_->getAngleRadAbsolute(); + double new_incremental_position = this->controller_->getAngleRadIncremental(); + double new_absolute_velocity = this->controller_->getVelocityRadAbsolute(); + double new_incremental_velocity = this->controller_->getVelocityRadIncremental(); + bool data_updated = + (new_controller_volt != this->previous_controller_volt_ || new_motor_volt != this->previous_motor_volt_ || + new_motor_current != this->previous_motor_current_ || + new_absolute_position != this->previous_absolute_position_ || + new_incremental_position != this->previous_incremental_position_ || + new_absolute_velocity != this->previous_absolute_velocity_ || + new_incremental_velocity != this->previous_incremental_velocity_); + + this->previous_controller_volt_ = new_controller_volt; this->previous_motor_volt_ = new_motor_volt; this->previous_motor_current_ = new_motor_current; this->previous_absolute_position_ = new_absolute_position; @@ -338,7 +219,7 @@ bool Joint::receivedDataUpdate() ActuationMode Joint::getActuationMode() const { - return this->imc_->getActuationMode(); + return this->controller_->getActuationMode(); } } // namespace march diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index 66d0ea35f..928cc6c4e 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -14,51 +14,44 @@ 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::string ifName, + std::vector> slave_list, int ecatCycleTime, int ecatSlaveTimeout) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) + , ethercatMaster(ifName, slave_list, ecatCycleTime, ecatSlaveTimeout) , pdb_(nullptr) { } MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, - int ecatCycleTime, int ecatSlaveTimeout) + std::shared_ptr powerDistributionBoard, ::std::string ifName, + std::vector> slave_list, int ecatCycleTime, int ecatSlaveTimeout) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) + , ethercatMaster(ifName, slave_list, ecatCycleTime, ecatSlaveTimeout) , pdb_(std::move(powerDistributionBoard)) { } -void MarchRobot::startEtherCAT(bool reset_imc) +void MarchRobot::startEtherCAT(bool reset_motor_controllers) { - if (!hasValidSlaves()) - { - throw error::HardwareException(error::ErrorType::INVALID_SLAVE_CONFIGURATION); - } - - ROS_INFO("Slave configuration is non-conflicting"); - if (ethercatMaster.isOperational()) { ROS_WARN("Trying to start EtherCAT while it is already active."); return; } - bool sw_reset = ethercatMaster.start(this->jointList); + bool config_overwritten = ethercatMaster.start(); - if (reset_imc || sw_reset) + if (reset_motor_controllers || config_overwritten) { - ROS_DEBUG("Resetting all IMotionCubes due to either: reset arg: %d or downloading of .sw fie: %d", reset_imc, - sw_reset); - resetIMotionCubes(); + ROS_DEBUG("Resetting all motor controllers due to either: reset arg: %d or downloading of configuration file: %d", + reset_motor_controllers, config_overwritten); + resetMotorControllers(); ROS_INFO("Restarting the EtherCAT Master"); ethercatMaster.stop(); - sw_reset = ethercatMaster.start(this->jointList); + config_overwritten = ethercatMaster.start(); } } @@ -73,83 +66,12 @@ void MarchRobot::stopEtherCAT() ethercatMaster.stop(); } -void MarchRobot::resetIMotionCubes() +void MarchRobot::resetMotorControllers() { for (auto& joint : jointList) { - joint.resetIMotionCube(); - } -} - -int MarchRobot::getMaxSlaveIndex() -{ - int maxSlaveIndex = -1; - - for (Joint& joint : jointList) - { - int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); - if (temperatureSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = temperatureSlaveIndex; - } - - int iMotionCubeSlaveIndex = joint.getIMotionCubeSlaveIndex(); - - if (iMotionCubeSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = iMotionCubeSlaveIndex; - } + joint.resetMotorController(); } - return maxSlaveIndex; -} - -bool MarchRobot::hasValidSlaves() -{ - ::std::vector iMotionCubeIndices; - ::std::vector temperatureSlaveIndices; - - for (auto& joint : jointList) - { - if (joint.hasTemperatureGES()) - { - int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); - temperatureSlaveIndices.push_back(temperatureSlaveIndex); - } - - if (joint.hasIMotionCube()) - { - int iMotionCubeSlaveIndex = joint.getIMotionCubeSlaveIndex(); - iMotionCubeIndices.push_back(iMotionCubeSlaveIndex); - } - } - // Multiple temperature sensors may be connected to the same slave. - // Remove duplicate temperatureSlaveIndices so they don't trigger as - // duplicates later. - sort(temperatureSlaveIndices.begin(), temperatureSlaveIndices.end()); - temperatureSlaveIndices.erase(unique(temperatureSlaveIndices.begin(), temperatureSlaveIndices.end()), - temperatureSlaveIndices.end()); - - // Merge the slave indices - ::std::vector slaveIndices; - - slaveIndices.reserve(iMotionCubeIndices.size() + temperatureSlaveIndices.size()); - slaveIndices.insert(slaveIndices.end(), iMotionCubeIndices.begin(), iMotionCubeIndices.end()); - slaveIndices.insert(slaveIndices.end(), temperatureSlaveIndices.begin(), temperatureSlaveIndices.end()); - - if (slaveIndices.size() == 1) - { - ROS_INFO("Found configuration for 1 slave."); - return true; - } - - ROS_INFO("Found configuration for %lu slaves.", slaveIndices.size()); - - // Sort the indices and check for duplicates. - // If there are no duplicates, the configuration is valid. - ::std::sort(slaveIndices.begin(), slaveIndices.end()); - auto it = ::std::unique(slaveIndices.begin(), slaveIndices.end()); - bool isUnique = (it == slaveIndices.end()); - return isUnique; } bool MarchRobot::isEthercatOperational() diff --git a/march_hardware/test/imotioncube/imotioncube_test.cpp b/march_hardware/test/imotioncube/imotioncube_test.cpp index 1756c3573..b6c7a9aeb 100644 --- a/march_hardware/test/imotioncube/imotioncube_test.cpp +++ b/march_hardware/test/imotioncube/imotioncube_test.cpp @@ -3,7 +3,7 @@ #include "../mocks/mock_incremental_encoder.h" #include "../mocks/mock_slave.h" -#include +#include #include #include @@ -77,5 +77,5 @@ TEST_F(IMotionCubeTest, OperationEnabledWithoutActuationMode) { march::IMotionCube imc(mock_slave, std::move(this->mock_absolute_encoder), std::move(this->mock_incremental_encoder), march::ActuationMode::unknown); - ASSERT_THROW(imc.goToOperationEnabled(), march::error::HardwareException); + ASSERT_THROW(imc.prepareActuation(), march::error::HardwareException); } diff --git a/march_hardware/test/joint_test.cpp b/march_hardware/test/joint_test.cpp index 975e61fd0..93d2153ff 100644 --- a/march_hardware/test/joint_test.cpp +++ b/march_hardware/test/joint_test.cpp @@ -1,6 +1,6 @@ // Copyright 2018 Project March. #include "mocks/mock_temperature_ges.h" -#include "mocks/mock_imotioncube.h" +#include "mocks/mock_motor_controller.h" #include "march_hardware/error/hardware_exception.h" #include "march_hardware/joint.h" @@ -19,59 +19,29 @@ class JointTest : public testing::Test protected: void SetUp() override { - this->imc = std::make_unique(); + this->motor_controller = std::make_unique(); this->temperature_ges = std::make_unique(); } - std::unique_ptr imc; + std::unique_ptr motor_controller; std::unique_ptr temperature_ges; }; -TEST_F(JointTest, InitializeWithoutMotorControllerAndGes) -{ - march::Joint joint("test", 0); - ASSERT_NO_THROW(joint.initialize(1)); -} - -TEST_F(JointTest, InitializeWithoutTemperatureGes) -{ - const int expected_cycle = 3; - EXPECT_CALL(*this->imc, initSdo(_, Eq(expected_cycle))).Times(1); - - march::Joint joint("test", 0, false, std::move(this->imc)); - ASSERT_NO_THROW(joint.initialize(expected_cycle)); -} - -TEST_F(JointTest, InitializeWithoutMotorController) -{ - const int expected_cycle = 3; - EXPECT_CALL(*this->temperature_ges, initSdo(_, Eq(expected_cycle))).Times(1); - - march::Joint joint("test", 0, false, nullptr, std::move(this->temperature_ges)); - ASSERT_NO_THROW(joint.initialize(expected_cycle)); -} - TEST_F(JointTest, AllowActuation) { - march::Joint joint("test", 0, true, std::move(this->imc)); + march::Joint joint("test", 0, true, std::move(this->motor_controller)); ASSERT_TRUE(joint.canActuate()); } -TEST_F(JointTest, DisallowActuationWithoutMotorController) -{ - march::Joint joint("test", 0, true, nullptr); - ASSERT_FALSE(joint.canActuate()); -} - TEST_F(JointTest, DisableActuation) { - march::Joint joint("test", 0, false, std::move(this->imc)); + march::Joint joint("test", 0, false, std::move(this->motor_controller)); ASSERT_FALSE(joint.canActuate()); } TEST_F(JointTest, SetAllowActuation) { - march::Joint joint("test", 0, false, std::move(this->imc)); + march::Joint joint("test", 0, false, std::move(this->motor_controller)); ASSERT_FALSE(joint.canActuate()); joint.setAllowActuation(true); ASSERT_TRUE(joint.canActuate()); @@ -80,20 +50,20 @@ TEST_F(JointTest, SetAllowActuation) TEST_F(JointTest, GetName) { const std::string expected_name = "test"; - march::Joint joint(expected_name, 0, false, std::move(this->imc)); + march::Joint joint(expected_name, 0, false, std::move(this->motor_controller)); ASSERT_EQ(expected_name, joint.getName()); } TEST_F(JointTest, GetNetNumber) { const int expected_net_number = 2; - march::Joint joint("test", expected_net_number, false, std::move(this->imc)); + march::Joint joint("test", expected_net_number, false, std::move(this->motor_controller)); ASSERT_EQ(expected_net_number, joint.getNetNumber()); } TEST_F(JointTest, ActuatePositionDisableActuation) { - march::Joint joint("actuate_false", 0, false, std::move(this->imc)); + march::Joint joint("actuate_false", 0, false, std::move(this->motor_controller)); EXPECT_FALSE(joint.canActuate()); ASSERT_THROW(joint.actuateRad(0.3), march::error::HardwareException); } @@ -101,15 +71,15 @@ TEST_F(JointTest, ActuatePositionDisableActuation) TEST_F(JointTest, ActuatePosition) { const double expected_rad = 5; - EXPECT_CALL(*this->imc, actuateRad(Eq(expected_rad))).Times(1); + EXPECT_CALL(*this->motor_controller, actuateRad(Eq(expected_rad))).Times(1); - march::Joint joint("actuate_false", 0, true, std::move(this->imc)); + march::Joint joint("actuate_false", 0, true, std::move(this->motor_controller)); ASSERT_NO_THROW(joint.actuateRad(expected_rad)); } TEST_F(JointTest, ActuateTorqueDisableActuation) { - march::Joint joint("actuate_false", 0, false, std::move(this->imc)); + march::Joint joint("actuate_false", 0, false, std::move(this->motor_controller)); EXPECT_FALSE(joint.canActuate()); ASSERT_THROW(joint.actuateTorque(3), march::error::HardwareException); } @@ -117,22 +87,22 @@ TEST_F(JointTest, ActuateTorqueDisableActuation) TEST_F(JointTest, ActuateTorque) { const int16_t expected_torque = 5; - EXPECT_CALL(*this->imc, actuateTorque(Eq(expected_torque))).Times(1); + EXPECT_CALL(*this->motor_controller, actuateTorque(Eq(expected_torque))).Times(1); - march::Joint joint("actuate_false", 0, true, std::move(this->imc)); + march::Joint joint("actuate_false", 0, true, std::move(this->motor_controller)); ASSERT_NO_THROW(joint.actuateTorque(expected_torque)); } TEST_F(JointTest, PrepareForActuationNotAllowed) { - march::Joint joint("actuate_false", 0, false, std::move(this->imc)); + march::Joint joint("actuate_false", 0, false, std::move(this->motor_controller)); ASSERT_THROW(joint.prepareActuation(), march::error::HardwareException); } TEST_F(JointTest, PrepareForActuationAllowed) { - EXPECT_CALL(*this->imc, goToOperationEnabled()).Times(1); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + EXPECT_CALL(*this->motor_controller, prepareActuation()).Times(1); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); ASSERT_NO_THROW(joint.prepareActuation()); } @@ -153,24 +123,17 @@ TEST_F(JointTest, GetTemperatureWithoutTemperatureGes) TEST_F(JointTest, ResetController) { - EXPECT_CALL(*this->imc, reset(_)).Times(1); - march::Joint joint("reset_controller", 0, true, std::move(this->imc)); - ASSERT_NO_THROW(joint.resetIMotionCube()); -} - -TEST_F(JointTest, ResetControllerWithoutController) -{ - EXPECT_CALL(*this->imc, reset(_)).Times(0); - march::Joint joint("reset_controller", 0, true, nullptr, std::move(this->temperature_ges)); - ASSERT_NO_THROW(joint.resetIMotionCube()); + EXPECT_CALL(*this->motor_controller, reset()).Times(1); + march::Joint joint("reset_controller", 0, true, std::move(this->motor_controller)); + ASSERT_NO_THROW(joint.resetMotorController()); } TEST_F(JointTest, TestPrepareActuation) { - EXPECT_CALL(*this->imc, getAngleRadIncremental()).WillOnce(Return(5)); - EXPECT_CALL(*this->imc, getAngleRadAbsolute()).WillOnce(Return(3)); - EXPECT_CALL(*this->imc, goToOperationEnabled()).Times(1); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + EXPECT_CALL(*this->motor_controller, getAngleRadIncremental()).WillOnce(Return(5)); + EXPECT_CALL(*this->motor_controller, getAngleRadAbsolute()).WillOnce(Return(3)); + EXPECT_CALL(*this->motor_controller, prepareActuation()).Times(1); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.prepareActuation(); ASSERT_EQ(joint.getIncrementalPosition(), 5); ASSERT_EQ(joint.getAbsolutePosition(), 3); @@ -178,23 +141,23 @@ TEST_F(JointTest, TestPrepareActuation) TEST_F(JointTest, TestReceivedDataUpdateFirstTimeTrue) { - EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillOnce(Return(48)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); ASSERT_TRUE(joint.receivedDataUpdate()); } TEST_F(JointTest, TestReceivedDataUpdateTrue) { - EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48)).WillOnce(Return(48.001)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillOnce(Return(48)).WillOnce(Return(48.001)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.receivedDataUpdate(); ASSERT_TRUE(joint.receivedDataUpdate()); } TEST_F(JointTest, TestReceivedDataUpdateFalse) { - EXPECT_CALL(*this->imc, getIMCVoltage()).WillRepeatedly(Return(48)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillRepeatedly(Return(48)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.receivedDataUpdate(); ASSERT_FALSE(joint.receivedDataUpdate()); } @@ -203,7 +166,7 @@ TEST_F(JointTest, TestReadEncodersOnce) { ros::Duration elapsed_time(0.2); double velocity = 0.5; - double velocity_with_noise = velocity - 2 * this->imc->getAbsoluteRadPerBit() / elapsed_time.toSec(); + double velocity_with_noise = velocity - 2 / elapsed_time.toSec(); double initial_incremental_position = 5; double initial_absolute_position = 3; @@ -211,21 +174,24 @@ TEST_F(JointTest, TestReadEncodersOnce) double new_incremental_position = initial_incremental_position + velocity * elapsed_time.toSec(); double new_absolute_position = initial_absolute_position + velocity_with_noise * elapsed_time.toSec(); - EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48)); - EXPECT_CALL(*this->imc, getMotorCurrent()).WillOnce(Return(5)); - EXPECT_CALL(*this->imc, getAngleRadIncremental()) + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillOnce(Return(48)); + EXPECT_CALL(*this->motor_controller, getIncrementalMorePrecise()).WillOnce(Return(true)); + EXPECT_CALL(*this->motor_controller, getMotorCurrent()).WillOnce(Return(5)); + EXPECT_CALL(*this->motor_controller, getAngleRadIncremental()) .WillOnce(Return(initial_incremental_position)) .WillOnce(Return(new_incremental_position)) .WillOnce(Return(new_incremental_position)); - EXPECT_CALL(*this->imc, getAngleRadAbsolute()) + EXPECT_CALL(*this->motor_controller, getAngleRadAbsolute()) .WillOnce(Return(initial_absolute_position)) .WillOnce(Return(new_absolute_position)) .WillOnce(Return(new_absolute_position)); - EXPECT_CALL(*this->imc, getVelocityRadIncremental()).WillOnce(Return(velocity)).WillOnce(Return(velocity)); - EXPECT_CALL(*this->imc, getVelocityRadAbsolute()).WillOnce(Return(velocity_with_noise)); + EXPECT_CALL(*this->motor_controller, getVelocityRadIncremental()) + .WillOnce(Return(velocity)) + .WillOnce(Return(velocity)); + EXPECT_CALL(*this->motor_controller, getVelocityRadAbsolute()).WillOnce(Return(velocity_with_noise)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.prepareActuation(); joint.readEncoders(elapsed_time); @@ -241,7 +207,7 @@ TEST_F(JointTest, TestReadEncodersTwice) double first_velocity = 0.5; double second_velocity = 0.8; - double absolute_noise = -this->imc->getAbsoluteRadPerBit(); + double absolute_noise = -1; double first_velocity_with_noise = first_velocity + absolute_noise / elapsed_time.toSec(); double second_velocity_with_noise = second_velocity + absolute_noise / elapsed_time.toSec(); @@ -252,29 +218,30 @@ TEST_F(JointTest, TestReadEncodersTwice) double third_incremental_position = second_incremental_position + second_velocity * elapsed_time.toSec(); double third_absolute_position = second_absolute_position + second_velocity_with_noise * elapsed_time.toSec(); - EXPECT_CALL(*this->imc, getIMCVoltage()).WillOnce(Return(48)).WillOnce(Return(48.01)); - EXPECT_CALL(*this->imc, getAngleRadIncremental()) + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillOnce(Return(48)).WillOnce(Return(48.01)); + EXPECT_CALL(*this->motor_controller, getIncrementalMorePrecise()).WillRepeatedly(Return(true)); + EXPECT_CALL(*this->motor_controller, getAngleRadIncremental()) .WillOnce(Return(initial_incremental_position)) .WillOnce(Return(second_incremental_position)) .WillOnce(Return(second_incremental_position)) .WillOnce(Return(third_incremental_position)) .WillOnce(Return(third_incremental_position)); - EXPECT_CALL(*this->imc, getAngleRadAbsolute()) + EXPECT_CALL(*this->motor_controller, getAngleRadAbsolute()) .WillOnce(Return(initial_absolute_position)) .WillOnce(Return(second_absolute_position)) .WillOnce(Return(second_absolute_position)) .WillOnce(Return(third_absolute_position)) .WillOnce(Return(third_absolute_position)); - EXPECT_CALL(*this->imc, getVelocityRadIncremental()) + EXPECT_CALL(*this->motor_controller, getVelocityRadIncremental()) .WillOnce(Return(first_velocity)) .WillOnce(Return(first_velocity)) .WillOnce(Return(second_velocity)) .WillOnce(Return(second_velocity)); - EXPECT_CALL(*this->imc, getVelocityRadAbsolute()) + EXPECT_CALL(*this->motor_controller, getVelocityRadAbsolute()) .WillOnce(Return(first_velocity_with_noise)) .WillOnce(Return(second_velocity_with_noise)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.prepareActuation(); joint.readEncoders(elapsed_time); @@ -292,24 +259,25 @@ TEST_F(JointTest, TestReadEncodersNoUpdate) double velocity = 0.5; - double absolute_noise = -this->imc->getAbsoluteRadPerBit(); + double absolute_noise = -1; double initial_incremental_position = 5; double initial_absolute_position = 3; double second_incremental_position = initial_incremental_position + velocity * elapsed_time.toSec(); double second_absolute_position = initial_absolute_position + velocity * elapsed_time.toSec() + absolute_noise; - EXPECT_CALL(*this->imc, getIMCVoltage()).WillRepeatedly(Return(48)); - EXPECT_CALL(*this->imc, getAngleRadIncremental()) + EXPECT_CALL(*this->motor_controller, getMotorControllerVoltage()).WillRepeatedly(Return(48)); + EXPECT_CALL(*this->motor_controller, getIncrementalMorePrecise()).WillRepeatedly(Return(true)); + EXPECT_CALL(*this->motor_controller, getAngleRadIncremental()) .WillOnce(Return(initial_incremental_position)) .WillRepeatedly(Return(second_incremental_position)); - EXPECT_CALL(*this->imc, getAngleRadAbsolute()) + EXPECT_CALL(*this->motor_controller, getAngleRadAbsolute()) .WillOnce(Return(initial_absolute_position)) .WillRepeatedly(Return(second_absolute_position)); - EXPECT_CALL(*this->imc, getVelocityRadIncremental()).WillRepeatedly(Return(velocity)); - EXPECT_CALL(*this->imc, getVelocityRadAbsolute()).WillRepeatedly(Return(velocity)); + EXPECT_CALL(*this->motor_controller, getVelocityRadIncremental()).WillRepeatedly(Return(velocity)); + EXPECT_CALL(*this->motor_controller, getVelocityRadAbsolute()).WillRepeatedly(Return(velocity)); - march::Joint joint("actuate_true", 0, true, std::move(this->imc)); + march::Joint joint("actuate_true", 0, true, std::move(this->motor_controller)); joint.prepareActuation(); joint.readEncoders(elapsed_time); diff --git a/march_hardware/test/mocks/mock_imotioncube.h b/march_hardware/test/mocks/mock_imotioncube.h deleted file mode 100644 index 43bc70741..000000000 --- a/march_hardware/test/mocks/mock_imotioncube.h +++ /dev/null @@ -1,39 +0,0 @@ -#pragma once -#include "mock_absolute_encoder.h" -#include "mock_incremental_encoder.h" -#include "mock_slave.h" - -#include "march_hardware/imotioncube/imotioncube.h" -#include "march_hardware/ethercat/sdo_interface.h" - -#include - -#include - -class MockIMotionCube : public march::IMotionCube -{ -public: - MockIMotionCube() - : IMotionCube(MockSlave(), std::make_unique(), std::make_unique(), - march::ActuationMode::unknown) - { - } - - MOCK_METHOD0(goToOperationEnabled, void()); - - MOCK_METHOD0(getAngleRadIncremental, double()); - MOCK_METHOD0(getAngleRadAbsolute, double()); - - MOCK_METHOD0(getVelocityRadIncremental, double()); - MOCK_METHOD0(getVelocityRadAbsolute, double()); - - MOCK_METHOD0(getIMCVoltage, float()); - MOCK_METHOD0(getMotorVoltage, float()); - MOCK_METHOD0(getMotorCurrent, float()); - - MOCK_METHOD1(actuateRad, void(double)); - MOCK_METHOD1(actuateTorque, void(int16_t)); - - MOCK_METHOD2(initSdo, bool(march::SdoSlaveInterface& sdo, int cycle_time)); - MOCK_METHOD1(reset, void(march::SdoSlaveInterface&)); -}; diff --git a/march_hardware/test/mocks/mock_joint.h b/march_hardware/test/mocks/mock_joint.h index c560769b1..2cd3aea30 100644 --- a/march_hardware/test/mocks/mock_joint.h +++ b/march_hardware/test/mocks/mock_joint.h @@ -7,9 +7,7 @@ class MockJoint : public march::Joint public: MOCK_METHOD0(getAngle, float()); MOCK_METHOD0(getTemperature, float()); - MOCK_METHOD0(getTemperatureGESSlaveIndex, int()); - MOCK_METHOD0(getIMotionCubeSlaveIndex, int()); - MOCK_METHOD0(hasIMotionCube, bool()); + MOCK_METHOD0(hasMotorController, bool()); MOCK_METHOD0(hasTemperatureGES, bool()); MOCK_METHOD1(hasTemperatureGES, void(double effort)); diff --git a/march_hardware/test/mocks/mock_motor_controller.h b/march_hardware/test/mocks/mock_motor_controller.h new file mode 100644 index 000000000..526a84809 --- /dev/null +++ b/march_hardware/test/mocks/mock_motor_controller.h @@ -0,0 +1,38 @@ +#pragma once +#include "mock_absolute_encoder.h" +#include "mock_incremental_encoder.h" +#include "mock_slave.h" + +#include "march_hardware/motor_controller/motor_controller.h" +#include "march_hardware/ethercat/sdo_interface.h" +#include "march_hardware/motor_controller/actuation_mode.h" +#include "march_hardware/motor_controller/motor_controller_states.h" + +#include + +#include + +class MockMotorController : public march::MotorController +{ +public: + MOCK_METHOD0(prepareActuation, void()); + MOCK_CONST_METHOD0(getActuationMode, march::ActuationMode()); + + MOCK_CONST_METHOD0(getIncrementalMorePrecise, bool()); + MOCK_METHOD0(getStates, std::unique_ptr()); + + MOCK_METHOD0(getAngleRadIncremental, double()); + MOCK_METHOD0(getAngleRadAbsolute, double()); + MOCK_METHOD0(getVelocityRadIncremental, double()); + MOCK_METHOD0(getVelocityRadAbsolute, double()); + + MOCK_METHOD0(getMotorControllerVoltage, float()); + MOCK_METHOD0(getMotorVoltage, float()); + MOCK_METHOD0(getMotorCurrent, float()); + MOCK_METHOD0(getTorque, double()); + + MOCK_METHOD1(actuateRad, void(double)); + MOCK_METHOD1(actuateTorque, void(double)); + + MOCK_METHOD0(reset, void()); +}; 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 dbc37df9d..5148541de 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -10,12 +10,13 @@ #include #include -#include +#include #include #include #include #include -#include +#include +#include #include #include #include @@ -66,21 +67,21 @@ class HardwareBuilder static void validateRequiredKeysExist(const YAML::Node& config, const std::vector& key_list, const std::string& object_name); - 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::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); static std::unique_ptr createAbsoluteEncoder(const YAML::Node& absolute_encoder_config, const urdf::JointConstSharedPtr& urdf_joint); static std::unique_ptr createIncrementalEncoder(const YAML::Node& incremental_encoder_config); - static std::unique_ptr createIMotionCube(const YAML::Node& imc_config, march::ActuationMode mode, - const urdf::JointConstSharedPtr& urdf_joint, - march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface); - static std::unique_ptr createTemperatureGES(const YAML::Node& temperature_ges_config, - march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface); - static std::unique_ptr + std::shared_ptr createIMotionCube(const YAML::Node& imc_config, march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::PdoInterfacePtr pdo_interface, + march::SdoInterfacePtr sdo_interface); + std::shared_ptr createTemperatureGES(const YAML::Node& temperature_ges_config, + march::PdoInterfacePtr pdo_interface, + march::SdoInterfacePtr sdo_interface); + std::shared_ptr createPowerDistributionBoard(const YAML::Node& power_distribution_board_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface); @@ -105,11 +106,13 @@ 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); YAML::Node robot_config_; urdf::Model urdf_; bool init_urdf_ = true; + std::vector> ges_list_; + std::vector> slave_list_; }; /** diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 0ff4a2390..50068496c 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -71,9 +71,9 @@ std::unique_ptr HardwareBuilder::createMarchRobot() 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); + auto pdb = this->createPowerDistributionBoard(pdb_config, pdo_interface, sdo_interface); + return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), if_name, this->slave_list_, + cycle_time, slave_timeout); } march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, @@ -106,22 +106,25 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const mode = march::ActuationMode(joint_config["actuationMode"].as()); } - auto imc = - HardwareBuilder::createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); - if (!imc) + std::shared_ptr controller; + if (joint_config["imotioncube"]) { - ROS_WARN("Joint %s does not have a configuration for an IMotionCube", joint_name.c_str()); + controller = this->createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); + } + if (!controller) + { + ROS_FATAL("Joint %s does not have a configuration for a motor controller", joint_name.c_str()); } - auto ges = HardwareBuilder::createTemperatureGES(joint_config["temperatureges"], pdo_interface, sdo_interface); + auto ges = this->createTemperatureGES(joint_config["temperatureges"], pdo_interface, sdo_interface); if (!ges) { ROS_WARN("Joint %s does not have a configuration for a TemperatureGes", joint_name.c_str()); } - return { joint_name, net_number, allow_actuation, std::move(imc), std::move(ges) }; + return { joint_name, net_number, allow_actuation, std::move(controller), std::move(ges) }; } -std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, +std::shared_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, @@ -141,10 +144,14 @@ std::unique_ptr HardwareBuilder::createIMotionCube(const YAM std::ifstream imc_setup_data; imc_setup_data.open(ros::package::getPath("march_ems_projects").append("/sw_files/" + urdf_joint->name + ".sw")); std::string setup = convertSWFileToString(imc_setup_data); - return std::make_unique( - march::Slave(slave_index, pdo_interface, sdo_interface), - HardwareBuilder::createAbsoluteEncoder(absolute_encoder_config, urdf_joint), - HardwareBuilder::createIncrementalEncoder(incremental_encoder_config), setup, mode); + + std::shared_ptr imc = + std::make_shared(march::Slave(slave_index, pdo_interface, sdo_interface), + this->createAbsoluteEncoder(absolute_encoder_config, urdf_joint), + this->createIncrementalEncoder(incremental_encoder_config), setup, mode); + + this->slave_list_.push_back(imc); + return imc; } std::unique_ptr HardwareBuilder::createAbsoluteEncoder( @@ -196,7 +203,7 @@ HardwareBuilder::createIncrementalEncoder(const YAML::Node& incremental_encoder_ return std::make_unique(resolution, transmission); } -std::unique_ptr HardwareBuilder::createTemperatureGES(const YAML::Node& temperature_ges_config, +std::shared_ptr HardwareBuilder::createTemperatureGES(const YAML::Node& temperature_ges_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface) { @@ -210,24 +217,36 @@ std::unique_ptr HardwareBuilder::createTemperatureGES(con const auto slave_index = temperature_ges_config["slaveIndex"].as(); const auto byte_offset = temperature_ges_config["byteOffset"].as(); - return std::make_unique(march::Slave(slave_index, pdo_interface, sdo_interface), byte_offset); + for (auto ges : this->ges_list_) + { + if (ges->getSlaveIndex() == slave_index) + { + return ges; + } + } + + std::shared_ptr ges = + std::make_shared(march::Slave(slave_index, pdo_interface, sdo_interface), byte_offset); + this->slave_list_.push_back(ges); + this->ges_list_.push_back(ges); + return ges; } -std::unique_ptr HardwareBuilder::createPowerDistributionBoard( - const YAML::Node& pdb, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface) +std::shared_ptr HardwareBuilder::createPowerDistributionBoard( + const YAML::Node& pdb_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface) { - if (!pdb) + if (!pdb_config) { return nullptr; } - HardwareBuilder::validateRequiredKeysExist(pdb, HardwareBuilder::POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS, + HardwareBuilder::validateRequiredKeysExist(pdb_config, HardwareBuilder::POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS, "powerdistributionboard"); - const auto slave_index = pdb["slaveIndex"].as(); - YAML::Node net_monitor_byte_offsets = pdb["netMonitorByteOffsets"]; - YAML::Node net_driver_byte_offsets = pdb["netDriverByteOffsets"]; - YAML::Node boot_shutdown_byte_offsets = pdb["bootShutdownOffsets"]; + const auto slave_index = pdb_config["slaveIndex"].as(); + YAML::Node net_monitor_byte_offsets = pdb_config["netMonitorByteOffsets"]; + YAML::Node net_driver_byte_offsets = pdb_config["netDriverByteOffsets"]; + YAML::Node boot_shutdown_byte_offsets = pdb_config["bootShutdownOffsets"]; NetMonitorOffsets net_monitor_offsets = NetMonitorOffsets( net_monitor_byte_offsets["powerDistributionBoardCurrent"].as(), @@ -246,9 +265,12 @@ std::unique_ptr HardwareBuilder::createPowerDistr boot_shutdown_byte_offsets["masterOk"].as(), boot_shutdown_byte_offsets["shutdown"].as(), boot_shutdown_byte_offsets["shutdownAllowed"].as()); - return std::make_unique(march::Slave(slave_index, pdo_interface, sdo_interface), - net_monitor_offsets, net_driver_offsets, - boot_shutdown_offsets); + std::shared_ptr pdb = + std::make_shared(march::Slave(slave_index, pdo_interface, sdo_interface), + net_monitor_offsets, net_driver_offsets, boot_shutdown_offsets); + this->slave_list_.push_back(pdb); + + return pdb; } void HardwareBuilder::validateRequiredKeysExist(const YAML::Node& config, const std::vector& key_list, @@ -277,7 +299,7 @@ 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) { std::vector joints; for (const YAML::Node& joint_config : joints_config) @@ -289,7 +311,7 @@ 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)); + std::move(createJoint(joint_config[joint_name], joint_name, urdf_joint, pdo_interface, sdo_interface))); } for (const auto& urdf_joint : this->urdf_.joints_) diff --git a/march_hardware_builder/test/absolute_encoder_builder_test.cpp b/march_hardware_builder/test/absolute_encoder_builder_test.cpp index bee181f32..010a636da 100644 --- a/march_hardware_builder/test/absolute_encoder_builder_test.cpp +++ b/march_hardware_builder/test/absolute_encoder_builder_test.cpp @@ -15,6 +15,8 @@ class AbsoluteEncoderBuilderTest : public ::testing::Test protected: std::string base_path; urdf::JointSharedPtr joint; + AllowedRobot robot; + HardwareBuilder builder = HardwareBuilder(robot); void SetUp() override { @@ -41,33 +43,33 @@ TEST_F(AbsoluteEncoderBuilderTest, ValidEncoderHip) march::AbsoluteEncoder expected = march::AbsoluteEncoder(16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit); - auto created = HardwareBuilder::createAbsoluteEncoder(config, this->joint); + auto created = this->builder.createAbsoluteEncoder(config, this->joint); ASSERT_EQ(expected, *created); } TEST_F(AbsoluteEncoderBuilderTest, NoConfig) { YAML::Node config; - ASSERT_EQ(nullptr, HardwareBuilder::createAbsoluteEncoder(config[""], this->joint)); + ASSERT_EQ(nullptr, this->builder.createAbsoluteEncoder(config[""], this->joint)); } TEST_F(AbsoluteEncoderBuilderTest, NoResolution) { YAML::Node config = this->loadTestYaml("/absolute_encoder_no_resolution.yaml"); - ASSERT_THROW(HardwareBuilder::createAbsoluteEncoder(config, this->joint), MissingKeyException); + ASSERT_THROW(this->builder.createAbsoluteEncoder(config, this->joint), MissingKeyException); } TEST_F(AbsoluteEncoderBuilderTest, NoMinPosition) { YAML::Node config = this->loadTestYaml("/absolute_encoder_no_min_position.yaml"); - ASSERT_THROW(HardwareBuilder::createAbsoluteEncoder(config, this->joint), MissingKeyException); + ASSERT_THROW(this->builder.createAbsoluteEncoder(config, this->joint), MissingKeyException); } TEST_F(AbsoluteEncoderBuilderTest, NoMaxPosition) { YAML::Node config = this->loadTestYaml("/absolute_encoder_no_max_position.yaml"); - ASSERT_THROW(HardwareBuilder::createAbsoluteEncoder(config, this->joint), MissingKeyException); + ASSERT_THROW(this->builder.createAbsoluteEncoder(config, this->joint), MissingKeyException); } diff --git a/march_hardware_builder/test/imc_builder_test.cpp b/march_hardware_builder/test/imc_builder_test.cpp index 732dad1b5..5663d4c29 100644 --- a/march_hardware_builder/test/imc_builder_test.cpp +++ b/march_hardware_builder/test/imc_builder_test.cpp @@ -10,7 +10,7 @@ #include #include -#include +#include class IMotionCubeBuilderTest : public ::testing::Test { @@ -19,6 +19,8 @@ class IMotionCubeBuilderTest : public ::testing::Test urdf::JointSharedPtr joint; march::PdoInterfacePtr pdo_interface; march::SdoInterfacePtr sdo_interface; + AllowedRobot robot; + HardwareBuilder builder = HardwareBuilder(robot); void SetUp() override { @@ -44,8 +46,8 @@ TEST_F(IMotionCubeBuilderTest, ValidIMotionCubeHip) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; - auto created = HardwareBuilder::createIMotionCube(config, march::ActuationMode::unknown, this->joint, - this->pdo_interface, this->sdo_interface); + auto created = this->builder.createIMotionCube(config, march::ActuationMode::unknown, this->joint, + this->pdo_interface, this->sdo_interface); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -60,23 +62,23 @@ TEST_F(IMotionCubeBuilderTest, ValidIMotionCubeHip) TEST_F(IMotionCubeBuilderTest, NoConfig) { YAML::Node config; - ASSERT_EQ(nullptr, HardwareBuilder::createIMotionCube(config["imotioncube"], march::ActuationMode::unknown, - this->joint, this->pdo_interface, this->sdo_interface)); + ASSERT_EQ(nullptr, this->builder.createIMotionCube(config["imotioncube"], march::ActuationMode::unknown, this->joint, + this->pdo_interface, this->sdo_interface)); } TEST_F(IMotionCubeBuilderTest, NoUrdfJoint) { YAML::Node config = this->loadTestYaml("/imotioncube_correct.yaml"); - ASSERT_EQ(nullptr, HardwareBuilder::createIMotionCube(config, march::ActuationMode::unknown, nullptr, - this->pdo_interface, this->sdo_interface)); + ASSERT_EQ(nullptr, this->builder.createIMotionCube(config, march::ActuationMode::unknown, nullptr, + this->pdo_interface, this->sdo_interface)); } TEST_F(IMotionCubeBuilderTest, NoAbsoluteEncoder) { YAML::Node config = this->loadTestYaml("/imotioncube_no_absolute_encoder.yaml"); - ASSERT_THROW(HardwareBuilder::createIMotionCube(config, march::ActuationMode::unknown, this->joint, - this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createIMotionCube(config, march::ActuationMode::unknown, this->joint, this->pdo_interface, + this->sdo_interface), MissingKeyException); } @@ -84,8 +86,8 @@ TEST_F(IMotionCubeBuilderTest, NoIncrementalEncoder) { YAML::Node config = this->loadTestYaml("/imotioncube_no_incremental_encoder.yaml"); - ASSERT_THROW(HardwareBuilder::createIMotionCube(config, march::ActuationMode::unknown, this->joint, - this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createIMotionCube(config, march::ActuationMode::unknown, this->joint, this->pdo_interface, + this->sdo_interface), MissingKeyException); } @@ -93,7 +95,7 @@ TEST_F(IMotionCubeBuilderTest, NoSlaveIndex) { YAML::Node config = this->loadTestYaml("/imotioncube_no_slave_index.yaml"); - ASSERT_THROW(HardwareBuilder::createIMotionCube(config, march::ActuationMode::unknown, this->joint, - this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createIMotionCube(config, march::ActuationMode::unknown, this->joint, this->pdo_interface, + this->sdo_interface), MissingKeyException); } diff --git a/march_hardware_builder/test/incremental_encoder_builder_test.cpp b/march_hardware_builder/test/incremental_encoder_builder_test.cpp index 9636db769..f518f8552 100644 --- a/march_hardware_builder/test/incremental_encoder_builder_test.cpp +++ b/march_hardware_builder/test/incremental_encoder_builder_test.cpp @@ -13,6 +13,8 @@ class IncrementalEncoderBuilderTest : public ::testing::Test { protected: std::string base_path; + AllowedRobot robot; + HardwareBuilder builder = HardwareBuilder(robot); void SetUp() override { @@ -30,26 +32,26 @@ TEST_F(IncrementalEncoderBuilderTest, ValidIncrementalEncoder) YAML::Node config = this->loadTestYaml("/incremental_encoder_correct.yaml"); march::IncrementalEncoder expected = march::IncrementalEncoder(12, 45.5); - auto created = HardwareBuilder::createIncrementalEncoder(config); + auto created = this->builder.createIncrementalEncoder(config); ASSERT_EQ(expected, *created); } TEST_F(IncrementalEncoderBuilderTest, NoConfig) { YAML::Node config; - ASSERT_EQ(nullptr, HardwareBuilder::createIncrementalEncoder(config[""])); + ASSERT_EQ(nullptr, this->builder.createIncrementalEncoder(config[""])); } TEST_F(IncrementalEncoderBuilderTest, NoResolution) { YAML::Node config = this->loadTestYaml("/incremental_encoder_no_resolution.yaml"); - ASSERT_THROW(HardwareBuilder::createIncrementalEncoder(config), MissingKeyException); + ASSERT_THROW(this->builder.createIncrementalEncoder(config), MissingKeyException); } TEST_F(IncrementalEncoderBuilderTest, NoTransmission) { YAML::Node config = this->loadTestYaml("/incremental_encoder_no_transmission.yaml"); - ASSERT_THROW(HardwareBuilder::createIncrementalEncoder(config), MissingKeyException); + ASSERT_THROW(this->builder.createIncrementalEncoder(config), MissingKeyException); } diff --git a/march_hardware_builder/test/joint_builder_test.cpp b/march_hardware_builder/test/joint_builder_test.cpp index 122c744c7..2fc9957f3 100644 --- a/march_hardware_builder/test/joint_builder_test.cpp +++ b/march_hardware_builder/test/joint_builder_test.cpp @@ -11,7 +11,7 @@ #include #include #include -#include +#include class JointBuilderTest : public ::testing::Test { @@ -20,6 +20,8 @@ class JointBuilderTest : public ::testing::Test urdf::JointSharedPtr joint; march::PdoInterfacePtr pdo_interface; march::SdoInterfacePtr sdo_interface; + AllowedRobot robot; + HardwareBuilder builder = HardwareBuilder(robot); void SetUp() override { @@ -46,20 +48,9 @@ TEST_F(JointBuilderTest, ValidJointHip) this->joint->safety->soft_upper_limit = 1.9; const std::string name = "test_joint_hip"; - march::Joint created = - HardwareBuilder::createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface); - - auto absolute_encoder = std::make_unique( - 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, - this->joint->safety->soft_upper_limit); - auto incremental_encoder = std::make_unique(12, 50.0); - auto imc = std::make_unique(march::Slave(2, this->pdo_interface, this->sdo_interface), - std::move(absolute_encoder), std::move(incremental_encoder), - march::ActuationMode::unknown); - auto ges = std::make_unique(march::Slave(1, this->pdo_interface, this->sdo_interface), 2); - march::Joint expected(name, -1, true, std::move(imc), std::move(ges)); - - ASSERT_EQ(expected, created); + march::Joint joint = this->builder.createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface); + + ASSERT_TRUE(joint.hasMotorController()); } TEST_F(JointBuilderTest, ValidNotActuated) @@ -70,38 +61,28 @@ TEST_F(JointBuilderTest, ValidNotActuated) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; - march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); - - auto absolute_encoder = std::make_unique( - 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, - this->joint->safety->soft_upper_limit); - auto incremental_encoder = std::make_unique(12, 50.0); - auto imc = std::make_unique(march::Slave(2, this->pdo_interface, this->sdo_interface), - std::move(absolute_encoder), std::move(incremental_encoder), - march::ActuationMode::unknown); - auto ges = std::make_unique(march::Slave(1, this->pdo_interface, this->sdo_interface), 2); - march::Joint expected("test_joint_hip", -1, false, std::move(imc), std::move(ges)); + march::Joint joint = + this->builder.createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); - ASSERT_EQ(expected, created); + ASSERT_TRUE(joint.hasMotorController()); } TEST_F(JointBuilderTest, NoActuate) { YAML::Node config = this->loadTestYaml("/joint_no_actuate.yaml"); - ASSERT_THROW(HardwareBuilder::createJoint(config, "test_joint_no_actuate", this->joint, this->pdo_interface, - this->sdo_interface), - MissingKeyException); + ASSERT_THROW( + this->builder.createJoint(config, "test_joint_no_actuate", this->joint, this->pdo_interface, this->sdo_interface), + MissingKeyException); } TEST_F(JointBuilderTest, NoIMotionCube) { 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); + march::Joint joint = this->builder.createJoint(config, "test_joint_no_imotioncube", this->joint, this->pdo_interface, + this->sdo_interface); - ASSERT_FALSE(joint.hasIMotionCube()); + ASSERT_FALSE(joint.hasMotorController()); } TEST_F(JointBuilderTest, NoTemperatureGES) @@ -112,8 +93,8 @@ TEST_F(JointBuilderTest, NoTemperatureGES) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 0.15; - ASSERT_NO_THROW(HardwareBuilder::createJoint(config, "test_joint_no_temperature_ges", this->joint, - this->pdo_interface, this->sdo_interface)); + ASSERT_NO_THROW(this->builder.createJoint(config, "test_joint_no_temperature_ges", this->joint, this->pdo_interface, + this->sdo_interface)); } TEST_F(JointBuilderTest, ValidActuationMode) @@ -124,31 +105,23 @@ TEST_F(JointBuilderTest, ValidActuationMode) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; - march::Joint created = - HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); - - march::Joint expected("test_joint_hip", -1, false, - std::make_unique( - march::Slave(1, this->pdo_interface, this->sdo_interface), - std::make_unique( - 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, - this->joint->safety->soft_lower_limit, this->joint->safety->soft_upper_limit), - std::make_unique(12, 50.0), march::ActuationMode::position)); + march::Joint joint = + this->builder.createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, this->sdo_interface); - ASSERT_EQ(expected, created); + ASSERT_TRUE(joint.hasMotorController()); } TEST_F(JointBuilderTest, EmptyJoint) { YAML::Node config; ASSERT_THROW( - HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface), + this->builder.createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface), MissingKeyException); } TEST_F(JointBuilderTest, NoUrdfJoint) { YAML::Node config = this->loadTestYaml("/joint_correct.yaml"); - ASSERT_THROW(HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface), march::error::HardwareException); } diff --git a/march_hardware_builder/test/pdb_builder_test.cpp b/march_hardware_builder/test/pdb_builder_test.cpp index 7fbc905ab..9a309c792 100644 --- a/march_hardware_builder/test/pdb_builder_test.cpp +++ b/march_hardware_builder/test/pdb_builder_test.cpp @@ -12,6 +12,8 @@ class PowerDistributionBoardBuilderTest : public ::testing::Test std::string base_path; march::PdoInterfacePtr pdo_interface; march::SdoInterfacePtr sdo_interface; + AllowedRobot robot; + HardwareBuilder builder = HardwareBuilder(robot); void SetUp() override { @@ -32,7 +34,7 @@ TEST_F(PowerDistributionBoardBuilderTest, ValidPowerDistributionBoard) YAML::Node config = YAML::LoadFile(fullPath); auto createdPowerDistributionBoard = - HardwareBuilder::createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface); + this->builder.createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface); NetMonitorOffsets netMonitoringOffsets(5, 9, 13, 17, 3, 2, 1, 4); NetDriverOffsets netDriverOffsets(4, 3, 2); BootShutdownOffsets bootShutdownOffsets(0, 0, 1); @@ -47,14 +49,14 @@ TEST_F(PowerDistributionBoardBuilderTest, NoConfig) { YAML::Node config; ASSERT_EQ(nullptr, - HardwareBuilder::createPowerDistributionBoard(config["pdb"], this->pdo_interface, this->sdo_interface)); + this->builder.createPowerDistributionBoard(config["pdb"], this->pdo_interface, this->sdo_interface)); } TEST_F(PowerDistributionBoardBuilderTest, MissingSlaveIndex) { std::string fullPath = this->fullPath("/power_distribution_board_missing_slave_index.yaml"); YAML::Node config = YAML::LoadFile(fullPath); - ASSERT_THROW(HardwareBuilder::createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), MissingKeyException); } @@ -62,7 +64,7 @@ TEST_F(PowerDistributionBoardBuilderTest, MissingHighVoltageStateIndex) { std::string fullPath = this->fullPath("/power_distribution_board_missing_high_voltage_state_index.yaml"); YAML::Node config = YAML::LoadFile(fullPath); - ASSERT_THROW(HardwareBuilder::createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), MissingKeyException); } @@ -70,6 +72,6 @@ TEST_F(PowerDistributionBoardBuilderTest, NegativeOffset) { std::string fullPath = this->fullPath("/power_distribution_board_negative_offset.yaml"); YAML::Node config = YAML::LoadFile(fullPath); - ASSERT_THROW(HardwareBuilder::createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), + ASSERT_THROW(this->builder.createPowerDistributionBoard(config, this->pdo_interface, this->sdo_interface), std::runtime_error); } 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 6f25fe7cd..27e0888b1 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 @@ -21,7 +21,7 @@ #include #include #include -#include +#include template using RtPublisherPtr = std::unique_ptr>; @@ -34,7 +34,7 @@ using RtPublisherPtr = std::unique_ptr>; class MarchHardwareInterface : public hardware_interface::RobotHW { public: - MarchHardwareInterface(std::unique_ptr robot, bool reset_imc); + MarchHardwareInterface(std::unique_ptr robot, bool reset_motor_controllers); /** * @brief Initialize the HardwareInterface by registering position interfaces @@ -84,14 +84,14 @@ class MarchHardwareInterface : public hardware_interface::RobotHW void updateHighVoltageEnable(); void updatePowerDistributionBoard(); void updateAfterLimitJointCommand(); - void updateIMotionCubeState(); + void updateMotorControllerStates(); void outsideLimitsCheck(size_t joint_index); - bool iMotionCubeStateCheck(size_t joint_index); + bool motorControllerStateCheck(size_t joint_index); static void getSoftJointLimitsError(const std::string& name, const urdf::JointConstSharedPtr& urdf_joint, joint_limits_interface::SoftJointLimits& error_soft_limits); - /* Limit of the change in effort command over one cycle, can be overridden by safety controller */ - static constexpr double MAX_EFFORT_CHANGE = 5000; + /* Limit of the change in effort (ampere) command over one cycle, can be overridden by safety controller */ + static constexpr double MAX_EFFORT_CHANGE = 6; /* March hardware */ std::unique_ptr march_robot_; @@ -130,13 +130,13 @@ class MarchHardwareInterface : public hardware_interface::RobotHW PowerNetOnOffCommand power_net_on_off_command_; bool master_shutdown_allowed_command_ = false; bool enable_high_voltage_command_ = true; - bool reset_imc_ = false; + bool reset_motor_controllers_ = false; bool has_actuated_ = false; /* Real time safe publishers */ RtPublisherPtr after_limit_joint_command_pub_; - RtPublisherPtr imc_state_pub_; + RtPublisherPtr motor_controller_state_pub_; }; #endif // MARCH_HARDWARE_INTERFACE_MARCH_HARDWARE_INTERFACE_H diff --git a/march_hardware_interface/src/march_hardware_interface.cpp b/march_hardware_interface/src/march_hardware_interface.cpp index 213c28146..86efad0f1 100644 --- a/march_hardware_interface/src/march_hardware_interface.cpp +++ b/march_hardware_interface/src/march_hardware_interface.cpp @@ -14,7 +14,8 @@ #include #include -#include +#include +#include #include using hardware_interface::JointHandle; @@ -25,16 +26,19 @@ using joint_limits_interface::JointLimits; using joint_limits_interface::PositionJointSoftLimitsHandle; using joint_limits_interface::SoftJointLimits; -MarchHardwareInterface::MarchHardwareInterface(std::unique_ptr robot, bool reset_imc) - : march_robot_(std::move(robot)), num_joints_(this->march_robot_->size()), reset_imc_(reset_imc) +MarchHardwareInterface::MarchHardwareInterface(std::unique_ptr robot, bool reset_motor_controllers) + : march_robot_(std::move(robot)) + , num_joints_(this->march_robot_->size()) + , reset_motor_controllers_(reset_motor_controllers) { } bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot_hw_nh */) { - // Initialize realtime publisher for the IMotionCube states - this->imc_state_pub_ = std::make_unique>( - nh, "/march/imc_states/", 4); + // Initialize realtime publisher for the motor controller states + this->motor_controller_state_pub_ = + std::make_unique>( + nh, "/march/motor_controller_states/", 4); this->after_limit_joint_command_pub_ = std::make_unique>( @@ -45,7 +49,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot this->reserveMemory(); // Start ethercat cycle in the hardware - this->march_robot_->startEtherCAT(this->reset_imc_); + this->march_robot_->startEtherCAT(this->reset_motor_controllers_); for (size_t i = 0; i < num_joints_; ++i) { @@ -140,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 IMC + // Prepare Motor Controllers for actuations if (joint.canActuate()) { joint.prepareActuation(); @@ -184,7 +188,7 @@ void MarchHardwareInterface::validate() for (size_t i = 0; i < num_joints_; i++) { this->outsideLimitsCheck(i); - if (!this->iMotionCubeStateCheck(i)) + if (!this->motorControllerStateCheck(i)) { fault_state = true; } @@ -192,7 +196,7 @@ void MarchHardwareInterface::validate() if (fault_state) { this->march_robot_->stopEtherCAT(); - throw std::runtime_error("One or more IMC's are in fault state"); + throw std::runtime_error("One or more motor controllers are in fault state"); } } @@ -207,7 +211,7 @@ void MarchHardwareInterface::read(const ros::Time& /* time */, const ros::Durati { march::Joint& joint = march_robot_->getJoint(i); - // Update position with he most accurate velocity + // Update position with the most accurate velocity joint.readEncoders(elapsed_time); joint_position_[i] = joint.getPosition(); joint_velocity_[i] = joint.getVelocity(); @@ -219,7 +223,7 @@ void MarchHardwareInterface::read(const ros::Time& /* time */, const ros::Durati joint_effort_[i] = joint.getTorque(); } - this->updateIMotionCubeState(); + this->updateMotorControllerStates(); } void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Duration& elapsed_time) @@ -227,7 +231,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat for (size_t i = 0; i < num_joints_; i++) { // Enlarge joint_effort_command because ROS control limits the pid values to a certain maximum - joint_effort_command_[i] = joint_effort_command_[i] * 1000.0; + joint_effort_command_[i] = joint_effort_command_[i]; if (std::abs(joint_last_effort_command_[i] - joint_effort_command_[i]) > MAX_EFFORT_CHANGE) { joint_effort_command_[i] = @@ -273,7 +277,7 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat } else if (joint.getActuationMode() == march::ActuationMode::torque) { - joint.actuateTorque(std::round(joint_effort_command_[i])); + joint.actuateTorque(joint_effort_command_[i]); } } } @@ -320,20 +324,15 @@ void MarchHardwareInterface::reserveMemory() after_limit_joint_command_pub_->msg_.position_command.resize(num_joints_); after_limit_joint_command_pub_->msg_.effort_command.resize(num_joints_); - imc_state_pub_->msg_.joint_names.resize(num_joints_); - imc_state_pub_->msg_.status_word.resize(num_joints_); - imc_state_pub_->msg_.detailed_error.resize(num_joints_); - imc_state_pub_->msg_.motion_error.resize(num_joints_); - imc_state_pub_->msg_.state.resize(num_joints_); - imc_state_pub_->msg_.detailed_error_description.resize(num_joints_); - imc_state_pub_->msg_.motion_error_description.resize(num_joints_); - imc_state_pub_->msg_.motor_current.resize(num_joints_); - imc_state_pub_->msg_.imc_voltage.resize(num_joints_); - imc_state_pub_->msg_.motor_voltage.resize(num_joints_); - imc_state_pub_->msg_.absolute_encoder_value.resize(num_joints_); - imc_state_pub_->msg_.incremental_encoder_value.resize(num_joints_); - imc_state_pub_->msg_.absolute_velocity.resize(num_joints_); - imc_state_pub_->msg_.incremental_velocity.resize(num_joints_); + motor_controller_state_pub_->msg_.joint_names.resize(num_joints_); + motor_controller_state_pub_->msg_.motor_current.resize(num_joints_); + motor_controller_state_pub_->msg_.controller_voltage.resize(num_joints_); + motor_controller_state_pub_->msg_.motor_voltage.resize(num_joints_); + motor_controller_state_pub_->msg_.absolute_encoder_value.resize(num_joints_); + motor_controller_state_pub_->msg_.incremental_encoder_value.resize(num_joints_); + motor_controller_state_pub_->msg_.absolute_velocity.resize(num_joints_); + motor_controller_state_pub_->msg_.incremental_velocity.resize(num_joints_); + motor_controller_state_pub_->msg_.error_status.resize(num_joints_); } void MarchHardwareInterface::updatePowerDistributionBoard() @@ -428,52 +427,42 @@ void MarchHardwareInterface::updateAfterLimitJointCommand() after_limit_joint_command_pub_->unlockAndPublish(); } -void MarchHardwareInterface::updateIMotionCubeState() +void MarchHardwareInterface::updateMotorControllerStates() { - if (!imc_state_pub_->trylock()) + if (!motor_controller_state_pub_->trylock()) { return; } - imc_state_pub_->msg_.header.stamp = ros::Time::now(); + motor_controller_state_pub_->msg_.header.stamp = ros::Time::now(); for (size_t i = 0; i < num_joints_; i++) { march::Joint& joint = march_robot_->getJoint(i); - march::IMotionCubeState imc_state = joint.getIMotionCubeState(); - imc_state_pub_->msg_.header.stamp = ros::Time::now(); - imc_state_pub_->msg_.joint_names[i] = joint.getName(); - imc_state_pub_->msg_.status_word[i] = imc_state.statusWord; - imc_state_pub_->msg_.detailed_error[i] = imc_state.detailedError; - imc_state_pub_->msg_.motion_error[i] = imc_state.motionError; - imc_state_pub_->msg_.state[i] = imc_state.state.getString(); - imc_state_pub_->msg_.detailed_error_description[i] = imc_state.detailedErrorDescription; - imc_state_pub_->msg_.motion_error_description[i] = imc_state.motionErrorDescription; - imc_state_pub_->msg_.motor_current[i] = imc_state.motorCurrent; - imc_state_pub_->msg_.imc_voltage[i] = imc_state.IMCVoltage; - imc_state_pub_->msg_.motor_voltage[i] = imc_state.motorVoltage; - imc_state_pub_->msg_.absolute_encoder_value[i] = imc_state.absoluteEncoderValue; - imc_state_pub_->msg_.incremental_encoder_value[i] = imc_state.incrementalEncoderValue; - imc_state_pub_->msg_.absolute_velocity[i] = imc_state.absoluteVelocity; - imc_state_pub_->msg_.incremental_velocity[i] = imc_state.incrementalVelocity; + std::unique_ptr motor_controller_state = joint.getMotorControllerStates(); + motor_controller_state_pub_->msg_.header.stamp = ros::Time::now(); + motor_controller_state_pub_->msg_.joint_names[i] = joint.getName(); + motor_controller_state_pub_->msg_.motor_current[i] = motor_controller_state->motor_current; + motor_controller_state_pub_->msg_.controller_voltage[i] = motor_controller_state->controller_voltage; + motor_controller_state_pub_->msg_.motor_voltage[i] = motor_controller_state->motor_voltage; + motor_controller_state_pub_->msg_.absolute_encoder_value[i] = motor_controller_state->absolute_encoder_value; + motor_controller_state_pub_->msg_.incremental_encoder_value[i] = motor_controller_state->incremental_encoder_value; + motor_controller_state_pub_->msg_.absolute_velocity[i] = motor_controller_state->absolute_velocity; + motor_controller_state_pub_->msg_.incremental_velocity[i] = motor_controller_state->incremental_velocity; + motor_controller_state_pub_->msg_.error_status[i] = motor_controller_state->getErrorStatus(); } - imc_state_pub_->unlockAndPublish(); + motor_controller_state_pub_->unlockAndPublish(); } -bool MarchHardwareInterface::iMotionCubeStateCheck(size_t joint_index) +bool MarchHardwareInterface::motorControllerStateCheck(size_t joint_index) { march::Joint& joint = march_robot_->getJoint(joint_index); - march::IMotionCubeState imc_state = joint.getIMotionCubeState(); - if (imc_state.state == march::IMCState::FAULT) + std::unique_ptr controller_states = joint.getMotorControllerStates(); + if (!controller_states->checkState()) { - ROS_ERROR("IMotionCube of joint %s is in fault state %s" - "\nMotion Error: %s (%s)" - "\nDetailed Error: %s (%s)" - "\nSecond Detailed Error: %s (%s)", - joint.getName().c_str(), imc_state.state.getString().c_str(), imc_state.motionErrorDescription.c_str(), - imc_state.motionError.c_str(), imc_state.detailedErrorDescription.c_str(), - imc_state.detailedError.c_str(), imc_state.secondDetailedErrorDescription.c_str(), - imc_state.secondDetailedError.c_str()); + std::string error_msg = + "Motor controller of joint " + joint.getName() + " is in " + controller_states->getErrorStatus(); + throw std::runtime_error(error_msg); return false; } return true;