diff --git a/march_hardware/CMakeLists.txt b/march_hardware/CMakeLists.txt index 23190f008..af1089498 100644 --- a/march_hardware/CMakeLists.txt +++ b/march_hardware/CMakeLists.txt @@ -4,29 +4,37 @@ project(march_hardware) add_compile_options(-std=c++14 -Wall -Wextra -Werror) find_package(catkin REQUIRED COMPONENTS - roscpp - soem - urdf -) + roscpp + soem + urdf + ) +find_package(PkgConfig REQUIRED) catkin_package( - INCLUDE_DIRS include - CATKIN_DEPENDS - roscpp - soem - urdf - LIBRARIES ${PROJECT_NAME} - CFG_EXTRAS - ${PROJECT_NAME}-extras.cmake + INCLUDE_DIRS include + CATKIN_DEPENDS + message_runtime + roscpp + roslib + soem + urdf + LIBRARIES ${PROJECT_NAME} + CFG_EXTRAS + ${PROJECT_NAME}-extras.cmake ) +pkg_check_modules(JSONCPP jsoncpp) +pkg_check_modules(LIBUSB1 libusb-1.0) + +link_libraries(${JSONCPP_LIBRARIES} ${LIBUSB1_LIBRARIES}) + include(cmake/${PROJECT_NAME}-extras.cmake) include_directories( - include - SYSTEM - ${catkin_INCLUDE_DIRS} - ${soem_INCLUDE_DIRS}/soem + include + SYSTEM + ${catkin_INCLUDE_DIRS} + ${soem_INCLUDE_DIRS}/soem ) # needed to circumvent LD_LIBRARY_PATH being emptied through ethercat_grant @@ -41,114 +49,128 @@ if(CATKIN_ENABLE_TESTING AND ENABLE_COVERAGE_TESTING) endif() add_library(${PROJECT_NAME} - include/${PROJECT_NAME}/encoder/absolute_encoder.h - include/${PROJECT_NAME}/encoder/encoder.h - include/${PROJECT_NAME}/encoder/incremental_encoder.h - include/${PROJECT_NAME}/error/error_type.h - include/${PROJECT_NAME}/error/hardware_exception.h - include/${PROJECT_NAME}/error/motion_error.h - include/${PROJECT_NAME}/ethercat/ethercat_master.h - include/${PROJECT_NAME}/ethercat/pdo_interface.h - include/${PROJECT_NAME}/ethercat/pdo_map.h - include/${PROJECT_NAME}/ethercat/pdo_types.h - include/${PROJECT_NAME}/ethercat/sdo_interface.h - include/${PROJECT_NAME}/ethercat/slave.h - include/${PROJECT_NAME}/motor_controller/actuation_mode.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h - include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h - include/${PROJECT_NAME}/motor_controller/motor_controller.h - include/${PROJECT_NAME}/motor_controller/motor_controller_states.h - include/${PROJECT_NAME}/joint.h - include/${PROJECT_NAME}/march_robot.h - include/${PROJECT_NAME}/power/boot_shutdown_offsets.h - include/${PROJECT_NAME}/power/high_voltage.h - include/${PROJECT_NAME}/power/low_voltage.h - include/${PROJECT_NAME}/power/net_driver_offsets.h - include/${PROJECT_NAME}/power/net_monitor_offsets.h - include/${PROJECT_NAME}/power/power_distribution_board.h - include/${PROJECT_NAME}/temperature/temperature_ges.h - include/${PROJECT_NAME}/temperature/temperature_sensor.h - src/encoder/absolute_encoder.cpp - src/encoder/encoder.cpp - src/encoder/incremental_encoder.cpp - src/error/error_type.cpp - src/error/motion_error.cpp - src/ethercat/ethercat_master.cpp - src/ethercat/pdo_interface.cpp - src/ethercat/pdo_map.cpp - src/ethercat/sdo_interface.cpp - src/imotioncube/imotioncube.cpp - src/imotioncube/imotioncube_target_state.cpp - src/joint.cpp - src/march_robot.cpp - src/power/high_voltage.cpp - src/power/low_voltage.cpp - src/power/power_distribution_board.cpp - src/temperature/temperature_ges.cpp -) + include/${PROJECT_NAME}/encoder/absolute_encoder.h + include/${PROJECT_NAME}/encoder/encoder.h + include/${PROJECT_NAME}/encoder/incremental_encoder.h + include/${PROJECT_NAME}/error/error_type.h + include/${PROJECT_NAME}/error/hardware_exception.h + include/${PROJECT_NAME}/error/motion_error.h + include/${PROJECT_NAME}/ethercat/ethercat_master.h + include/${PROJECT_NAME}/ethercat/pdo_interface.h + include/${PROJECT_NAME}/ethercat/pdo_map.h + include/${PROJECT_NAME}/ethercat/pdo_types.h + include/${PROJECT_NAME}/ethercat/sdo_interface.h + include/${PROJECT_NAME}/ethercat/slave.h + include/${PROJECT_NAME}/motor_controller/actuation_mode.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_states.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_state_of_operation.h + include/${PROJECT_NAME}/motor_controller/imotioncube/imotioncube_target_state.h + include/${PROJECT_NAME}/motor_controller/motor_controller.h + include/${PROJECT_NAME}/motor_controller/motor_controller_states.h + include/${PROJECT_NAME}/joint.h + include/${PROJECT_NAME}/march_robot.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_endpoint.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_enums.h + include/${PROJECT_NAME}/motor_controller/odrive/odrive_motor.h + include/${PROJECT_NAME}/motor_controller/odrive/usb_master.h + include/${PROJECT_NAME}/power/boot_shutdown_offsets.h + include/${PROJECT_NAME}/power/high_voltage.h + include/${PROJECT_NAME}/power/low_voltage.h + include/${PROJECT_NAME}/power/net_driver_offsets.h + include/${PROJECT_NAME}/power/net_monitor_offsets.h + include/${PROJECT_NAME}/power/power_distribution_board.h + include/${PROJECT_NAME}/temperature/temperature_ges.h + include/${PROJECT_NAME}/temperature/temperature_sensor.h + src/encoder/absolute_encoder.cpp + src/encoder/encoder.cpp + src/encoder/incremental_encoder.cpp + src/error/error_type.cpp + src/error/motion_error.cpp + src/ethercat/ethercat_master.cpp + src/ethercat/pdo_interface.cpp + src/ethercat/pdo_map.cpp + src/ethercat/sdo_interface.cpp + src/imotioncube/imotioncube.cpp + src/imotioncube/imotioncube_target_state.cpp + src/joint.cpp + src/march_robot.cpp + src/odrive/odrive.cpp + src/odrive/odrive_endpoint.cpp + src/odrive/odrive_motor.cpp + src/odrive/usb_master.cpp + src/power/high_voltage.cpp + src/power/low_voltage.cpp + src/power/power_distribution_board.cpp + src/temperature/temperature_ges.cpp + ) -target_link_libraries(${PROJECT_NAME} ${catkin_LIBRARIES} pthread) +target_link_libraries(${PROJECT_NAME} + ${catkin_LIBRARIES} + pthread + ${LIBUSB1_LIBRARIES} + ${JSONCPP_LIBRARIES} + yaml-cpp) add_executable(slave_count_check check/slave_count.cpp) target_link_libraries(slave_count_check ${PROJECT_NAME}) ros_enable_rpath(slave_count_check) install(DIRECTORY include/${PROJECT_NAME}/ - DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} -) + DESTINATION ${CATKIN_PACKAGE_INCLUDE_DESTINATION} + ) install(TARGETS ${PROJECT_NAME} - ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} - LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} -) + ARCHIVE DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + LIBRARY DESTINATION ${CATKIN_PACKAGE_LIB_DESTINATION} + ) install(TARGETS slave_count_check - RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} -) + RUNTIME DESTINATION ${CATKIN_PACKAGE_BIN_DESTINATION} + ) -install(DIRECTORY launch - DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} -) +install(DIRECTORY launch config + DESTINATION ${CATKIN_PACKAGE_SHARE_DESTINATION} + ) ## Add gtest based cpp test target and link libraries if(CATKIN_ENABLE_TESTING) catkin_add_gmock(${PROJECT_NAME}_test - test/encoder/absolute_encoder_test.cpp - test/encoder/encoder_test.cpp - test/encoder/incremental_encoder_test.cpp - test/error/hardware_exception_test.cpp - test/error/motion_error_test.cpp - test/ethercat/pdo_map_test.cpp - test/ethercat/slave_test.cpp - test/imotioncube/imotioncube_test.cpp - test/joint_test.cpp - test/mocks/mock_absolute_encoder.h - test/mocks/mock_encoder.h - test/mocks/mock_motor_controller.h - test/mocks/mock_incremental_encoder.h - test/mocks/mock_joint.h - test/mocks/mock_pdo_interface.h - test/mocks/mock_sdo_interface.h - test/mocks/mock_slave.h - test/mocks/mock_temperature_ges.h - test/power/boot_shutdown_offsets_test.cpp - test/power/high_voltage_test.cpp - test/power/low_voltage_test.cpp - test/power/net_driver_offsets_test.cpp - test/power/net_monitor_offsets_test.cpp - test/power/power_distribution_board_test.cpp - test/temperature/temperature_ges_test.cpp - test/test_runner.cpp - ) + test/encoder/absolute_encoder_test.cpp + test/encoder/encoder_test.cpp + test/encoder/incremental_encoder_test.cpp + test/error/hardware_exception_test.cpp + test/error/motion_error_test.cpp + test/ethercat/pdo_map_test.cpp + test/ethercat/slave_test.cpp + test/imotioncube/imotioncube_test.cpp + test/joint_test.cpp + test/mocks/mock_absolute_encoder.h + test/mocks/mock_encoder.h + test/mocks/mock_motor_controller.h + test/mocks/mock_incremental_encoder.h + test/mocks/mock_joint.h + test/mocks/mock_pdo_interface.h + test/mocks/mock_sdo_interface.h + test/mocks/mock_slave.h + test/mocks/mock_temperature_ges.h + test/power/boot_shutdown_offsets_test.cpp + test/power/high_voltage_test.cpp + test/power/low_voltage_test.cpp + test/power/net_driver_offsets_test.cpp + test/power/net_monitor_offsets_test.cpp + test/power/power_distribution_board_test.cpp + test/temperature/temperature_ges_test.cpp + test/test_runner.cpp + ) target_link_libraries(${PROJECT_NAME}_test ${catkin_LIBRARIES} ${PROJECT_NAME}) if(ENABLE_COVERAGE_TESTING) set(COVERAGE_EXCLUDES "*/${PROJECT_NAME}/test/*" "*/${PROJECT_NAME}/check/*") add_code_coverage( - NAME coverage_report - DEPENDENCIES ${PROJECT_NAME}_test + NAME coverage_report + DEPENDENCIES ${PROJECT_NAME}_test ) endif() endif() diff --git a/march_hardware/config/right_hip_fe.json b/march_hardware/config/right_hip_fe.json new file mode 100644 index 000000000..f00a0a926 --- /dev/null +++ b/march_hardware/config/right_hip_fe.json @@ -0,0 +1,73 @@ +[ + { + "name":"axis0.motor.config.current_lim", + "type":"float", + "value":30.0 + }, + { + "name":"axis0.encoder.config.cpr", + "type":"int32", + "value":4096 + }, + { + "name":"axis0.encoder.config.use_index", + "type":"bool", + "value":1 + }, + { + "name":"config.brake_resistance", + "type":"float", + "value":0.0 + }, + { + "name":"axis0.motor.config.pole_pairs", + "type":"int32", + "value":21 + }, + { + "name":"axis0.motor.config.calibration_current", + "type":"float", + "value":10.0 + }, + { + "name":"axis0.controller.config.vel_limit", + "type":"float", + "value":200000.0 + }, + { + "name":"axis0.motor.config.current_control_bandwidth", + "type":"float", + "value":100.0 + }, + + { + "name":"axis0.motor.config.motor_type", + "type":"uint8", + "value":0 + }, + { + "name":"axis0.controller.config.vel_gain", + "type":"float", + "value":0.01 + }, + { + "name":"axis0.controller.config.vel_integrator_gain", + "type":"float", + "value":0.05 + }, + { + "name":"axis0.controller.config.control_mode", + "type":"uint8", + "value":1 + }, + { + "name":"axis0.sensorless_estimator.config.pm_flux_linkage", + "type":"float", + "value":0.001944723 + }, + { + "name":"axis0.config.watchdog_timeout", + "type":"float", + "value":0 + }, +] diff --git a/march_hardware/config/right_knee.json b/march_hardware/config/right_knee.json new file mode 100644 index 000000000..47bd65487 --- /dev/null +++ b/march_hardware/config/right_knee.json @@ -0,0 +1,74 @@ +[ + { + "name":"axis1.motor.config.current_lim", + "type":"float", + "value":30.0 + }, + { + "name":"axis1.encoder.config.cpr", + "type":"int32", + "value":4096 + }, + { + "name":"axis1.encoder.config.use_index", + "type":"bool", + "value":1 + }, + { + "name":"config.brake_resistance", + "type":"float", + "value":0.0 + }, + { + "name":"axis1.motor.config.pole_pairs", + "type":"int32", + "value":21 + }, + { + "name":"axis1.motor.config.calibration_current", + "type":"float", + "value":10.0 + }, + { + "name":"axis1.controller.config.vel_limit", + "type":"float", + "value":200000.0 + }, + { + "name":"axis1.motor.config.current_control_bandwidth", + "type":"float", + "value":100.0 + }, + + { + "name":"axis1.motor.config.motor_type", + "type":"uint8", + "value":0 + }, + { + "name":"axis1.controller.config.vel_gain", + "type":"float", + "value":0.01 + }, + { + "name":"axis1.controller.config.vel_integrator_gain", + "type":"float", + "value":0.05 + }, + { + "name":"axis1.controller.config.control_mode", + "type":"uint8", + "value":1 + }, + { + "name":"axis1.sensorless_estimator.config.pm_flux_linkage", + "type":"float", + "value":0.001944723 + }, + + { + "name":"axis1.config.watchdog_timeout", + "type":"float", + "value":0 + } +] diff --git a/march_hardware/include/march_hardware/march_robot.h b/march_hardware/include/march_hardware/march_robot.h index 12c745efc..39b079750 100644 --- a/march_hardware/include/march_hardware/march_robot.h +++ b/march_hardware/include/march_hardware/march_robot.h @@ -20,18 +20,20 @@ class MarchRobot private: ::std::vector jointList; urdf::Model urdf_; - EthercatMaster ethercatMaster; + std::unique_ptr ethercatMaster; std::unique_ptr pdb_; public: using iterator = std::vector::iterator; - MarchRobot(::std::vector jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout); + MarchRobot(::std::vector jointList, urdf::Model urdf, std::unique_ptr ethercatMaster); MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout); + std::unique_ptr powerDistributionBoard, + std::unique_ptr ethercatMaster); + + MarchRobot(::std::vector jointList, urdf::Model urdf, + std::unique_ptr powerDistributionBoard); ~MarchRobot(); @@ -45,21 +47,21 @@ class MarchRobot void resetMotorControllers(); - void startEtherCAT(bool reset_motor_controllers); + void startCommunication(bool reset_motor_controllers); - void stopEtherCAT(); - - int getMaxSlaveIndex(); + void stopCommunication(); bool hasValidSlaves(); bool isEthercatOperational(); - std::exception_ptr getLastEthercatException() const noexcept; + bool isCommunicationOperational(); + + std::exception_ptr getLastCommunicationException() const noexcept; - void waitForPdo(); + void waitForUpdate(); - int getEthercatCycleTime() const; + int getCycleTime() const; Joint& getJoint(::std::string jointName); diff --git a/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h index bb228ba19..7b1ee7b43 100644 --- a/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h +++ b/march_hardware/include/march_hardware/motor_controller/imotioncube/imotioncube.h @@ -71,7 +71,7 @@ class IMotionCube : public MotorController, public Slave void setControlWord(uint16_t control_word); virtual void actuateRad(double target_rad) override; - virtual void actuateTorque(double target_torque) override; + virtual void actuateTorque(double target_torque_ampere) override; bool initialize(int cycle_time) override; void goToTargetState(IMotionCubeTargetState target_state); diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h new file mode 100644 index 000000000..7824c4958 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive.h @@ -0,0 +1,112 @@ +#ifndef ODRIVE_HPP_ +#define ODRIVE_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +#include "ros/ros.h" +#include "odrive_endpoint.h" +#include "odrive_enums.h" +#include "march_hardware/motor_controller/motor_controller.h" + +#define ODRIVE_OK 0; +#define ODRIVE_ERROR 1; + +static constexpr double PI_2 = 2 * M_PI; + +namespace march +{ +typedef struct odrive_json_object +{ + int id; + std::string name; + std::string type; + std::string access; +} odrive_json_object; + +class Odrive : public MotorController +{ +public: + /** + * Initialize the odrive with specified axis + */ + Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json = true); + + /** + * Import the odrive json structure from the odrive + */ + int importOdriveJson(); + + /** + * Check if given value type matched value type of odrive variable + */ + template + int validateType(const odrive_json_object& json_object, TT& value); + + /** + * Read parameter from the odrive object + */ + template + int read(const std::string& parameter_name, TT& value); + + /** + * Write parameter to the odrive object + */ + template + int write(const std::string& parameter_name, TT& value); + + /** + * Execute function on the odrive object + */ + int function(const std::string& function_name); + + /** + * Set configurations in the Json file to the Odrive + */ + int setConfigurations(const std::string& configuration_json_path); + + std::string axis_number; + +protected: + float readMotorControllerVoltage(); + float readMotorCurrent(); + float readMotorVoltage(); + + uint16_t readAxisError(); + uint16_t readAxisMotorError(); + uint8_t readAxisEncoderError(); + uint8_t readAxisControllerError(); + + int readAngleCountsAbsolute(); + double readVelocityRadAbsolute(); + + int readAngleCountsIncremental(); + double readVelocityRadIncremental(); + std::string create_command(std::string command_name); + +private: + int json_string_read(const Json::Value& json_parameter_object); + + int json_string_write(const Json::Value& json_parameter_object); + + static std::vector split_string(const std::string& str, char delimiter = '.'); + + odrive_json_object getJsonObject(const std::string& parameter_name); + + Json::Value odrive_json_; + Json::Value odrive_configuration_json_; + + std::shared_ptr odrive_endpoint_; +}; +} // namespace march +#endif diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h new file mode 100644 index 000000000..b5ecc8449 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_endpoint.h @@ -0,0 +1,186 @@ +#ifndef ODRIVE_ENDPOINT_HPP_ +#define ODRIVE_ENDPOINT_HPP_ + +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +// ODrive Device Info +#define ODRIVE_USB_VENDORID 0x1209 +#define ODRIVE_USB_PRODUCTID 0x0D32 + +// ODrive USB Protocol +#define ODRIVE_TIMEOUT 200 +#define ODRIVE_MAX_BYTES_TO_RECEIVE 64 +#define ODRIVE_MAX_RESULT_LENGTH 100 +#define ODRIVE_DEFAULT_CRC_VALUE 0x7411 +#define ODRIVE_PROTOCOL_VERION 1 + +// ODrive Comm +#define ODRIVE_COMM_SUCCESS 0 +#define ODRIVE_COMM_ERROR 1 + +// Endpoints (from target) +#define CDC_IN_EP 0x81 /* EP1 for data IN (target) */ +#define CDC_OUT_EP 0x01 /* EP1 for data OUT (target) */ +#define CDC_CMD_EP 0x82 /* EP2 for CDC commands */ +#define ODRIVE_IN_EP 0x83 /* EP3 IN: ODrive device TX endpoint */ +#define ODRIVE_OUT_EP 0x03 /* EP3 OUT: ODrive device RX endpoint */ + +// CDC Endpoints parameters +#define CDC_DATA_HS_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_DATA_FS_MAX_PACKET_SIZE 64 /* Endpoint IN & OUT Packet size */ +#define CDC_CMD_PACKET_SIZE 8 /* Control Endpoint Packet size */ + +#define USB_CDC_CONFIG_DESC_SIZ (67 + 39) +#define CDC_DATA_HS_IN_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE +#define CDC_DATA_HS_OUT_PACKET_SIZE CDC_DATA_HS_MAX_PACKET_SIZE + +#define CDC_DATA_FS_IN_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE +#define CDC_DATA_FS_OUT_PACKET_SIZE CDC_DATA_FS_MAX_PACKET_SIZE + +#define CDC_SEND_ENCAPSULATED_COMMAND 0x00 +#define CDC_GET_ENCAPSULATED_RESPONSE 0x01 +#define CDC_SET_COMM_FEATURE 0x02 +#define CDC_GET_COMM_FEATURE 0x03 +#define CDC_CLEAR_COMM_FEATURE 0x04 +#define CDC_SET_LINE_CODING 0x20 +#define CDC_GET_LINE_CODING 0x21 +#define CDC_SET_CONTROL_LINE_STATE 0x22 +#define CDC_SEND_BREAK 0x23 + +typedef std::vector commBuffer; + +namespace march +{ +class OdriveEndpoint +{ +public: + /** + * initialize USB library and local variables + */ + OdriveEndpoint(); + + /** + * release USB library + */ + ~OdriveEndpoint(); + + /** + * enumerate ODrive hardware + * @param serialNumber odrive serial number + * @return ODRIVE_COMM_SUCCESS on success else ODRIVE_COMM_ERROR + */ + int open_connection(const std::string& serialNumber); + + /** + * close ODrive device + */ + void remove(); + + /** + * Read value from ODrive + * @param id odrive ID + * @param value Data read + * @return ODRIVE_COMM_SUCCESS on success else ODRIVE_COMM_ERROR + */ + template + int getData(int id, T& value); + + /** + * Write value to Odrive + * @param id odrive ID + * @param value Data to be written + * @return ODRIVE_COMM_SUCCESS on success + */ + template + int setData(int id, const TT& value); + + /** + * Request function to ODrive + * @param id odrive ID + * @return ODRIVE_COMM_SUCCESS on success + */ + int execFunc(int id); + + /** + * Request to USB endpoint + * @param handle USB device handler + * @param endpoint_id odrive ID + * @param received_payload receive buffer + * @param received_length receive length + * @param payload data read + * @param ack request acknowledge + * @param length data length + * @param read send read address + * @param address read address + * @return LIBUSB_SUCCESS on success + */ + int endpointRequest(int endpoint_id, commBuffer& received_payload, int& received_length, const commBuffer& payload, + bool ack, int length, bool read = false, int address = 0); + + /** + * Getter for the serial number + * @return the serial number of the odrive connection + */ + std::string getSerialNumber(); + +private: + /** + * Append short data to data buffer + * @param buf data buffer + * @param value data to append + */ + static void appendShortToCommBuffer(commBuffer& buf, short value); + + /** + * Append int data to data buffer + * @param buf data buffer + * @param value data to append + */ + static void appendIntToCommBuffer(commBuffer& buf, int value); + + /** + * Decode odrive packet + * @param buf data buffer + * @param seq_no packet sequence number + * @param received_packet received buffer + * @return data buffer + */ + static commBuffer decodeODrivePacket(commBuffer& buf, short& seq_no); + + /** + * Read data buffer from Odrive hardware + * @param seq_no next sequence number + * @param endpoint_id USB endpoint ID + * @param response_size maximum data length to be read + * @param read append request address + * @param address destination address + * @param input data buffer to send + * @return data buffer read + */ + commBuffer createODrivePacket(short seq_no, int endpoint_id, short response_size, bool read, int address, + const commBuffer& input); + + libusb_context* lib_usb_context_ = nullptr; + libusb_device_handle* odrive_handle_ = nullptr; + + bool attached_to_handle_; + int crc_; + + int outbound_seq_no_ = 0; + std::string odrive_serial_number; + + std::mutex ep_lock_; +}; + +} // namespace march +#endif // ODRIVE_ENDPOINT_HPP_ diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h new file mode 100644 index 000000000..3372503d9 --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_enums.h @@ -0,0 +1,107 @@ +#ifndef ODRIVE_ENUMS_HPP_ +#define ODRIVE_ENUMS_HPP_ + +// axis command names +#define O_PM_REQUEST_STATE ".requested_state" +#define O_PM_CURRENT_STATE ".current_state" +#define O_PM_ENCODER_POSITION_UI ".encoder.pos_estimate" +#define O_PM_ENCODER_VELOCITY_UI ".encoder.vel_estimate" +#define O_PM_ODRIVE_INPUT_VOLTAGE "vbus_voltage" +#define O_PM_DESIRED_MOTOR_CURRENT ".controller.current_setpoint" +#define O_PM_ACTUAL_MOTOR_CURRENT ".motor.current_control.Iq_measured" +#define O_PM_ACTUAL_MOTOR_VOLTAGE_D ".motor.current_control.v_current_control_integral_d" +#define O_PM_ACTUAL_MOTOR_VOLTAGE_Q ".motor.current_control.v_current_control_integral_q" +#define O_PM_AXIS_ERROR ".error" +#define O_PM_AXIS_MOTOR_ERROR ".motor.error" +#define O_PM_AXIS_ENCODER_ERROR ".encoder.error" +#define O_PM_AXIS_CONTROLLER_ERROR ".controller.error" + +#define ERROR_NONE 0x00 + +// axis states +enum States : int +{ + AXIS_STATE_UNDEFINED = 0, + AXIS_STATE_IDLE = 1, + AXIS_STATE_STARTUP_SEQUENCE = 2, + AXIS_STATE_FULL_CALIBRATION_SEQUENCE = 3, + AXIS_STATE_MOTOR_CALIBRATION = 4, + AXIS_STATE_SENSORLESS_CONTROL = 5, + AXIS_STATE_ENCODER_INDEX_SEARCH = 6, + AXIS_STATE_ENCODER_OFFSET_CALIBRATION = 7, + AXIS_STATE_CLOSED_LOOP_CONTROL = 8, + AXIS_STATE_LOCKIN_SPIN = 9, + AXIS_STATE_ENCODER_DIR_FIND = 10 +}; + +enum class AxisError +{ + ERROR_INVALID_STATE = 0x01, + ERROR_DC_BUS_UNDER_VOLTAGE = 0x02, + ERROR_DC_BUS_OVER_VOLTAGE = 0x04, + ERROR_CURRENT_MEASUREMENT_TIMEOUT = 0x08, + ERROR_BRAKE_RESISTOR_DISARMED = 0x10, + ERROR_MOTOR_DISARMED = 0x20, + ERROR_MOTOR_FAILED = 0x40, + ERROR_SENSORLESS_ESTIMATOR_FAILED = 0x80, + ERROR_ENCODER_FAILED = 0x100, + ERROR_CONTROLLER_FAILED = 0x200, + ERROR_POS_CTRL_DURING_SENSORLESS = 0x400, + ERROR_WATCHDOG_TIMER_EXPIRED = 0x800, +}; + +// class motor: +enum class AxisMotorError +{ + ERROR_PHASE_RESISTANCE_OUT_OF_RANGE = 0x0001, + ERROR_PHASE_INDUCTANCE_OUT_OF_RANGE = 0x0002, + ERROR_ADC_FAILED = 0x0004, + ERROR_DRV_FAULT = 0x0008, + ERROR_CONTROL_DEADLINE_MISSED = 0x0010, + ERROR_NOT_IMPLEMENTED_MOTOR_TYPE = 0x0020, + ERROR_BRAKE_CURRENT_OUT_OF_RANGE = 0x0040, + ERROR_MODULATION_MAGNITUDE = 0x0080, + ERROR_BRAKE_DEADTIME_VIOLATION = 0x0100, + ERROR_UNEXPECTED_TIMER_CALLBACK = 0x0200, + ERROR_CURRENT_SENSE_SATURATION = 0x0400, + ERROR_CURRENT_UNSTABLE = 0x1000 +}; + +enum class AxisEncoderError +{ + ERROR_UNSTABLE_GAIN = 0x01, + ERROR_CPR_OUT_OF_RANGE = 0x02, + ERROR_NO_RESPONSE = 0x04, + ERROR_UNSUPPORTED_ENCODER_MODE = 0x08, + ERROR_ILLEGAL_HALL_STATE = 0x10, + ERROR_INDEX_NOT_FOUND_YET = 0x20 +}; + +enum class AxisControllerError +{ + ERROR_OVER_SPEED = 0x01 +}; + +enum class AxisMotorType +{ + MOTOR_TYPE_HIGH_CURRENT = 0, + MOTOR_TYPE_LOW_CURRENT = 1, + MOTOR_TYPE_GIMBAL = 2 +}; + +enum class AxisControllerMode +{ + CTRL_MODE_VOLTAGE_CONTROL = 0, + CTRL_MODE_CURRENT_CONTROL = 1, + CTRL_MODE_VELOCITY_CONTROL = 2, + CTRL_MODE_POSITION_CONTROL = 3, + CTRL_MODE_TRAJECTORY_CONTROL = 4 +}; + +enum class AxisEncoderMode +{ + ENCODER_MODE_INCREMENTAL = 0, + ENCODER_MODE_HALL = 1 +}; + +#endif diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h new file mode 100644 index 000000000..26374a7bd --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_motor.h @@ -0,0 +1,89 @@ +#ifndef ODRIVE_INTERFACE_ODRIVE_MOTOR_H +#define ODRIVE_INTERFACE_ODRIVE_MOTOR_H + +#include +#include + +#include "ros/ros.h" +#include "odrive.h" +#include "odrive_enums.h" +#include + +static constexpr double MOTOR_KV = 100; +static constexpr double CURRENT_TO_TORQUE_CONVERSION = 8.27; + +namespace march +{ +class OdriveMotor : public Odrive +{ +public: + OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, ActuationMode mode, + std::string json_config_file_path); + ~OdriveMotor(); + + bool initialize(int cycle_time) override; + void prepareActuation() override; + bool waitForIdleState(float timout = 15.0); + + void reset() override; + + void actuateRad(double target_rad) override; + void actuateTorque(double target_torque_ampere) override; + + MotorControllerStates& getStates() override; + + float getMotorControllerVoltage() override; + float getMotorCurrent() override; + float getMotorVoltage() override; + double getTorque() override; + + uint16_t getAxisError(); + uint16_t getAxisMotorError(); + uint8_t getAxisEncoderError(); + uint8_t getAxisControllerError(); + + double getAngleRadAbsolute() override; + double getVelocityRadAbsolute() override; + + double getAngleRadIncremental() override; + double getVelocityRadIncremental() override; + + bool getIncrementalMorePrecise() const override; + ActuationMode getActuationMode() const override + { + return this->mode_; + } + int getSlaveIndex() const override + { + return -1; + } + +private: + int setState(uint8_t state); + uint8_t getState(); + + int getAngleCountsAbsolute(); + int getAngleCountsIncremental(); + + ActuationMode mode_; + std::string json_config_file_path_; + + void readValues(); + + uint16_t axis_error; + uint16_t axis_motor_error; + uint8_t axis_encoder_error; + uint8_t axis_controller_error; + + float motor_controller_voltage; + float motor_current; + float motor_voltage; + + double angle_counts_absolute; + double velocity_rad_absolute; + double angle_counts_incremental; + double velocity_rad_incremental; +}; + +} // namespace march +#endif // ODRIVE_INTERFACE_ODRIVE_MOTOR_H diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h new file mode 100644 index 000000000..d854d844a --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/odrive_states.h @@ -0,0 +1,53 @@ +// Copyright 2019 Project March. +#ifndef MARCH_HARDWARE_IMOTIONCUBE_STATES_H +#define MARCH_HARDWARE_IMOTIONCUBE_STATES_H + +#include +#include "march_hardware/motor_controller/motor_controller_states.h" +#include "odrive_enums.h" + +namespace march +{ +struct OdriveStates : public MotorControllerStates +{ +public: + OdriveStates() = default; + + States state; + uint16_t axisError; + uint16_t axisMotorError; + uint16_t axisEncoderError; + uint16_t axisControllerError; + + std::string axisErrorDescription; + std::string axisMotorErrorDescription; + std::string axisEncoderErrorDescription; + std::string axisControllerErrorDescription; + + bool checkState() override + { + if (this->axisError == ERROR_NONE) + { + return true; + } + return false; + } + + std::string getErrorStatus() override + { + std::ostringstream error_stream; + + error_stream << "State: " << this->state << "\nAxis Error: " << this->axisError << " (" + << this->axisErrorDescription << ")" + << "\nMotor Error: " << this->axisMotorError << " (" << this->axisMotorErrorDescription << ")" + << "\nEncoder Error: " << this->axisEncoderError << " (" << this->axisEncoderErrorDescription << ")" + << "\nController Error: " << this->axisControllerError << " (" << this->axisControllerErrorDescription + << ")"; + + return error_stream.str(); + } +}; + +} // namespace march + +#endif // MARCH_HARDWARE_IMOTIONCUBE_STATES_H diff --git a/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h new file mode 100644 index 000000000..4c4847f3e --- /dev/null +++ b/march_hardware/include/march_hardware/motor_controller/odrive/usb_master.h @@ -0,0 +1,26 @@ +// +// Created by roel on 16-08-20. +// + +#ifndef MARCH_HARDWARE_USB_MASTER_H +#define MARCH_HARDWARE_USB_MASTER_H + +#include "odrive_endpoint.h" +#include +#include +#include + +namespace march +{ +class UsbMaster +{ +public: + UsbMaster() = default; + std::shared_ptr getSerialConnection(const std::string& serial_number); + +private: + std::vector> odrive_endpoints_; +}; + +} // namespace march +#endif // MARCH_HARDWARE_USB_MASTER_H diff --git a/march_hardware/package.xml b/march_hardware/package.xml index 1dae600ad..53f87be21 100644 --- a/march_hardware/package.xml +++ b/march_hardware/package.xml @@ -16,6 +16,10 @@ roscpp soem urdf + roslib + yaml-cpp + + message_runtime rosunit code_coverage diff --git a/march_hardware/src/march_robot.cpp b/march_hardware/src/march_robot.cpp index 980329911..15cc43804 100644 --- a/march_hardware/src/march_robot.cpp +++ b/march_hardware/src/march_robot.cpp @@ -14,26 +14,31 @@ namespace march { -MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, ::std::string ifName, int ecatCycleTime, - int ecatSlaveTimeout) +MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, std::unique_ptr ethercatMaster) + : jointList(std::move(jointList)), urdf_(std::move(urdf)), ethercatMaster(std::move(ethercatMaster)), pdb_(nullptr) +{ +} + +MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, + std::unique_ptr powerDistributionBoard, + std::unique_ptr ethercatMaster) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) - , pdb_(nullptr) + , ethercatMaster(std::move(ethercatMaster)) + , pdb_(std::move(powerDistributionBoard)) { } MarchRobot::MarchRobot(::std::vector jointList, urdf::Model urdf, - std::unique_ptr powerDistributionBoard, ::std::string ifName, - int ecatCycleTime, int ecatSlaveTimeout) + std::unique_ptr powerDistributionBoard) : jointList(std::move(jointList)) , urdf_(std::move(urdf)) - , ethercatMaster(ifName, this->getMaxSlaveIndex(), ecatCycleTime, ecatSlaveTimeout) + , ethercatMaster(nullptr) , pdb_(std::move(powerDistributionBoard)) { } -void MarchRobot::startEtherCAT(bool reset_motor_controllers) +void MarchRobot::startCommunication(bool reset_motor_controllers) { if (!hasValidSlaves()) { @@ -42,35 +47,41 @@ void MarchRobot::startEtherCAT(bool reset_motor_controllers) ROS_INFO("Slave configuration is non-conflicting"); - if (ethercatMaster.isOperational()) + if (this->isCommunicationOperational()) { - ROS_WARN("Trying to start EtherCAT while it is already active."); + ROS_WARN("Trying to start Communication while it is already active."); return; } - bool config_overwritten = ethercatMaster.start(this->jointList); - - if (reset_motor_controllers || config_overwritten) + if (ethercatMaster != nullptr) { - ROS_DEBUG("Resetting all motor controllers due to either: reset arg: %d or downloading of configuration file: %d", - reset_motor_controllers, config_overwritten); - resetMotorControllers(); + bool sw_reset = ethercatMaster->start(this->jointList); + + if (reset_motor_controllers || sw_reset) + { + ROS_DEBUG("Resetting all MotorControllers due to either: reset arg: %d or downloading of .sw fie: %d", + reset_motor_controllers, sw_reset); + resetMotorControllers(); - ROS_INFO("Restarting the EtherCAT Master"); - ethercatMaster.stop(); - config_overwritten = ethercatMaster.start(this->jointList); + ROS_INFO("Restarting the EtherCAT Master"); + ethercatMaster->stop(); + sw_reset = ethercatMaster->start(this->jointList); + } } } -void MarchRobot::stopEtherCAT() +void MarchRobot::stopCommunication() { - if (!ethercatMaster.isOperational()) + if (!this->isCommunicationOperational()) { - ROS_WARN("Trying to stop EtherCAT while it is not active."); + ROS_WARN("Trying to stop Communication while it is not active."); return; } - ethercatMaster.stop(); + if (ethercatMaster != nullptr) + { + ethercatMaster->stop(); + } } void MarchRobot::resetMotorControllers() @@ -81,28 +92,6 @@ void MarchRobot::resetMotorControllers() } } -int MarchRobot::getMaxSlaveIndex() -{ - int maxSlaveIndex = -1; - - for (Joint& joint : jointList) - { - int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); - if (temperatureSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = temperatureSlaveIndex; - } - - int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; - - if (motorControllerSlaveIndex > maxSlaveIndex) - { - maxSlaveIndex = motorControllerSlaveIndex; - } - } - return maxSlaveIndex; -} - bool MarchRobot::hasValidSlaves() { ::std::vector motorControllerIndices; @@ -154,35 +143,54 @@ bool MarchRobot::hasValidSlaves() bool MarchRobot::isEthercatOperational() { - return ethercatMaster.isOperational(); + return ethercatMaster != nullptr && ethercatMaster->isOperational(); } -std::exception_ptr MarchRobot::getLastEthercatException() const noexcept +bool MarchRobot::isCommunicationOperational() { - return this->ethercatMaster.getLastException(); + return this->isEthercatOperational(); } -void MarchRobot::waitForPdo() +std::exception_ptr MarchRobot::getLastCommunicationException() const noexcept { - this->ethercatMaster.waitForPdo(); + if (ethercatMaster != nullptr) + { + return this->ethercatMaster->getLastException(); + } + return nullptr; } -int MarchRobot::getEthercatCycleTime() const +void MarchRobot::waitForUpdate() { - return this->ethercatMaster.getCycleTime(); + if (ethercatMaster != nullptr) + { + this->ethercatMaster->waitForPdo(); + } } -Joint& MarchRobot::getJoint(::std::string jointName) +int MarchRobot::getCycleTime() const { - if (!ethercatMaster.isOperational()) + if (ethercatMaster != nullptr) { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); + return this->ethercatMaster->getCycleTime(); } + return 0; +} + +Joint& MarchRobot::getJoint(::std::string jointName) +{ for (auto& joint : jointList) { if (joint.getName() == jointName) { + if (joint.getMotorControllerSlaveIndex() != -1) + { + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } + } return joint; } } @@ -192,12 +200,18 @@ Joint& MarchRobot::getJoint(::std::string jointName) Joint& MarchRobot::getJoint(size_t index) { - if (!ethercatMaster.isOperational()) + Joint& joint = this->jointList.at(index); + + if (joint.getMotorControllerSlaveIndex() != -1) { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } } - return this->jointList.at(index); + + return joint; } size_t MarchRobot::size() const @@ -207,12 +221,18 @@ size_t MarchRobot::size() const MarchRobot::iterator MarchRobot::begin() { - if (!ethercatMaster.isOperational()) + auto joint = this->jointList.begin(); + + if (joint[0].getMotorControllerSlaveIndex() != -1) { - ROS_WARN("Trying to access joints while ethercat is not operational. This " - "may lead to incorrect sensor data."); + if (!this->isCommunicationOperational()) + { + ROS_WARN("Trying to access joints while ethercat is not operational. This " + "may lead to incorrect sensor data."); + } } - return this->jointList.begin(); + + return joint; } MarchRobot::iterator MarchRobot::end() @@ -232,7 +252,7 @@ PowerDistributionBoard* MarchRobot::getPowerDistributionBoard() const MarchRobot::~MarchRobot() { - stopEtherCAT(); + stopCommunication(); } const urdf::Model& MarchRobot::getUrdf() const diff --git a/march_hardware/src/odrive/odrive.cpp b/march_hardware/src/odrive/odrive.cpp new file mode 100644 index 000000000..e5b69e25c --- /dev/null +++ b/march_hardware/src/odrive/odrive.cpp @@ -0,0 +1,561 @@ +#include "march_hardware/motor_controller/odrive/odrive.h" + +namespace march +{ +Odrive::Odrive(const std::string& axis_number, std::shared_ptr odrive_endpoint, bool import_json) +{ + this->axis_number = axis_number; + this->odrive_endpoint_ = std::move(odrive_endpoint); + + if (import_json) + { + if (this->importOdriveJson()) + { + ROS_ERROR("Odrive %s error getting JSON", odrive_endpoint_->getSerialNumber().c_str()); + } + } +} + +std::vector Odrive::split_string(const std::string& string_name, char delimiter) +{ + std::vector split_string; + + std::stringstream ss(string_name); + std::string token; + + while (std::getline(ss, token, delimiter)) + { + split_string.push_back(token); + } + + return split_string; +} + +odrive_json_object Odrive::getJsonObject(const std::string& parameter_name) +{ + odrive_json_object json_object; + std::vector parameter_list = this->split_string(parameter_name); + + Json::Value odrive_json_member_object; + Json::Value odrive_json_member_list = this->odrive_json_; + + for (std::string& split_parameter_name : parameter_list) + { + for (auto& json_parameter : odrive_json_member_list) + { + if (json_parameter["name"].asCString() == split_parameter_name) + { + odrive_json_member_object = json_parameter; + odrive_json_member_list = json_parameter["members"]; + break; + } + } + } + + if (!odrive_json_member_object or odrive_json_member_object["name"].asCString() != parameter_list.back()) + { + json_object.id = -1; + ROS_ERROR("Odrive object with name %s is not found in parsed json file", parameter_name.c_str()); + } + else + { + json_object.id = odrive_json_member_object["id"].asInt(); + json_object.name = odrive_json_member_object["name"].asString(); + json_object.type = odrive_json_member_object["type"].asString(); + json_object.access = odrive_json_member_object["access"].asString(); + } + + return json_object; +} + +int Odrive::importOdriveJson() +{ + commBuffer rx; + commBuffer tx; + + int len; + int address = 0; + + std::string json; + + do + { + this->odrive_endpoint_->endpointRequest(0, rx, len, tx, true, 512, true, address); + address = address + len; + json.append((const char*)&rx[0], (size_t)len); + } while (len > 0); + + Json::Reader reader; + bool res = reader.parse(json, this->odrive_json_); + + if (!res) + { + return ODRIVE_ERROR; + } + return ODRIVE_OK; +} + +int Odrive::function(const std::string& function_name) +{ + odrive_json_object json_object = this->getJsonObject(function_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.type != "function") + { + ROS_ERROR("Error: Given parameter %s is not a function on the Odrive", function_name.c_str()); + return ODRIVE_ERROR; + } + + this->odrive_endpoint_->execFunc(json_object.id); + return 0; +} + +template +int Odrive::validateType(const odrive_json_object& json_object, TT& value) +{ + if (json_object.type == "float") + { + if (sizeof(value) != sizeof(float)) + { + ROS_ERROR("Error value for %s is not float", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint8") + { + if (sizeof(value) != sizeof(uint8_t)) + { + ROS_ERROR("Error value for %s is not uint8_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint16") + { + if (sizeof(value) != sizeof(uint16_t)) + { + ROS_ERROR("Error value for %s is not uint16_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint32") + { + if (sizeof(value) != sizeof(uint32_t)) + { + ROS_ERROR("Error value for %s is not uint32_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "uint64") + { + if (sizeof(value) != sizeof(uint64_t)) + { + ROS_ERROR("Error value for %s is not uint64_t", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "int8") + { + if (sizeof(value) != sizeof(short)) + { + ROS_ERROR("Error value for %s is not short", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "int16") + { + if (sizeof(value) != sizeof(short)) + { + ROS_ERROR("Error value for %s is not short", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else if (json_object.type == "int32") + { + if (sizeof(value) != sizeof(int)) + { + ROS_ERROR("Error value for %s is not int", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + + else if (json_object.type == "bool") + { + if (sizeof(value) != sizeof(bool)) + { + ROS_ERROR("Error value for %s is not bool", json_object.name.c_str()); + return ODRIVE_ERROR; + } + } + else + { + ROS_ERROR("Error: invalid type for %s", json_object.name.c_str()); + return ODRIVE_ERROR; + } + + return ODRIVE_OK; +} + +template +int Odrive::read(const std::string& parameter_name, TT& value) +{ + odrive_json_object json_object = this->getJsonObject(parameter_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.access.find('r') == std::string::npos) + { + ROS_ERROR("Error: invalid read access for %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } + + if (this->validateType(json_object, value) == 1) + { + return ODRIVE_ERROR; + } + + return this->odrive_endpoint_->getData(json_object.id, value); +} + +template +int Odrive::write(const std::string& parameter_name, TT& value) +{ + odrive_json_object json_object = this->getJsonObject(parameter_name); + + if (json_object.id == -1) + { + return ODRIVE_ERROR; + } + + if (json_object.access.find('w') == std::string::npos) + { + ROS_ERROR("Error: invalid read access for %s", parameter_name.c_str()); + return ODRIVE_ERROR; + } + + if (this->validateType(json_object, value) == 1) + { + return ODRIVE_ERROR; + } + + return this->odrive_endpoint_->setData(json_object.id, value); +} + +int Odrive::json_string_read(const Json::Value& json_parameter_object) +{ + std::string parameter_name = json_parameter_object["name"].asString(); + std::string type_name = json_parameter_object["type"].asString(); + Json::Value value = json_parameter_object["value"]; + + if (type_name == "uint8") + { + uint8_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "uint16") + { + uint16_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "uint32") + { + uint32_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "int8") + { + int8_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "int16") + { + int16_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "int32") + { + int32_t casted_value = value.asUInt(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "float") + { + float casted_value = value.asFloat(); + return this->read(parameter_name, casted_value); + } + else if (type_name == "bool") + { + bool casted_value = value.asBool(); + return this->read(parameter_name, casted_value); + } + else + { + ROS_ERROR("Error converting string for reading, invalid type %s", parameter_name.c_str()); + return ODRIVE_ERROR + } +} + +int Odrive::json_string_write(const Json::Value& json_parameter_object) +{ + std::string parameter_name = json_parameter_object["name"].asString(); + std::string type_name = json_parameter_object["type"].asString(); + Json::Value value = json_parameter_object["value"]; + + if (type_name == "uint8") + { + uint8_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "uint16") + { + uint16_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "uint32") + { + uint32_t casted_value = value.asUInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "int8") + { + int8_t casted_value = value.asInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "int16") + { + int16_t casted_value = value.asInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "int32") + { + int32_t casted_value = value.asInt(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "float") + { + float casted_value = value.asFloat(); + return this->write(parameter_name, casted_value); + } + else if (type_name == "bool") + { + bool casted_value = value.asBool(); + return this->write(parameter_name, casted_value); + } + else + { + ROS_ERROR("Error converting string for writing, invalid type %s", parameter_name.c_str()); + return ODRIVE_ERROR + } +} + +int Odrive::setConfigurations(const std::string& configuration_json_path) +{ + std::ifstream cfg; + std::string line, json; + cfg.open(configuration_json_path, std::ios::in); + + if (cfg.is_open()) + { + while (getline(cfg, line)) + { + json.append(line); + } + } + cfg.close(); + + Json::Reader reader; + bool res = reader.parse(json, this->odrive_configuration_json_); + + if (!res) + { + ROS_INFO("Error parsing odrive configuration, error"); + return ODRIVE_ERROR + } + + for (auto& parameter : this->odrive_configuration_json_) + { + ROS_DEBUG("Setting %s to %s", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + int result = this->json_string_write(parameter); + + if (result != LIBUSB_SUCCESS) + { + ROS_WARN("Setting %s to %s failed", parameter["name"].asString().c_str(), parameter["value"].asString().c_str()); + continue; + } + } + return ODRIVE_OK +} + +uint16_t Odrive::readAxisError() +{ + uint16_t axis_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->read(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not retrieve axis error"); + return ODRIVE_ERROR; + } + + return axis_error; +} + +uint16_t Odrive::readAxisMotorError() +{ + uint16_t axis_motor_error; + std::string command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->read(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not retrieve axis motor error"); + return ODRIVE_ERROR; + } + + return axis_motor_error; +} + +uint8_t Odrive::readAxisEncoderError() +{ + uint8_t axis_encoder_error; + std::string command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->read(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not retrieve axis encoder error"); + return ODRIVE_ERROR; + } + + return axis_encoder_error; +} + +uint8_t Odrive::readAxisControllerError() +{ + uint8_t axis_controller_error; + std::string command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->read(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not retrieve axis controller error"); + return ODRIVE_ERROR; + } + + return axis_controller_error; +} + +float Odrive::readMotorControllerVoltage() +{ + float motor_controller_voltage; + std::string command_name_ = this->create_command(O_PM_ODRIVE_INPUT_VOLTAGE); + if (this->read(command_name_, motor_controller_voltage) == 1) + { + ROS_ERROR("Could not retrieve motor controller voltage"); + return ODRIVE_ERROR; + } + return motor_controller_voltage; +} + +float Odrive::readMotorCurrent() +{ + float motor_current; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_CURRENT); + if (this->read(command_name_, motor_current) == 1) + { + ROS_ERROR("Could not retrieve motor current"); + return ODRIVE_ERROR; + } + + return motor_current; +} + +float Odrive::readMotorVoltage() +{ + float motor_voltage; + std::string command_name_ = this->create_command(O_PM_ACTUAL_MOTOR_VOLTAGE_D); + if (this->read(command_name_, motor_voltage) == 1) + { + ROS_ERROR("Could not retrieve motor voltage"); + return ODRIVE_ERROR; + } + + return motor_voltage; +} + +int Odrive::readAngleCountsAbsolute() +{ + return 0; +} + +double Odrive::readVelocityRadAbsolute() +{ + return 0; +} + +int Odrive::readAngleCountsIncremental() +{ + float iu_position; + std::string command_name_ = this->create_command(O_PM_ENCODER_POSITION_UI); + if (this->read(command_name_, iu_position) == 1) + { + ROS_ERROR("Could not retrieve incremental position of the encoder"); + return ODRIVE_ERROR + } + return iu_position; +} + +double Odrive::readVelocityRadIncremental() +{ + float iu_velocity; + std::string command_name_ = this->create_command(O_PM_ENCODER_VELOCITY_UI); + if (this->read(command_name_, iu_velocity) == 1) + { + ROS_ERROR("Could not retrieve incremental velocity of the encoder"); + return ODRIVE_ERROR + } + + double angle_rad = iu_velocity * PI_2 / (std::pow(2, 12) * 101); + return angle_rad; +} + +std::string Odrive::create_command(std::string command_name) +{ + if (command_name.at(0) == '.') + { + return this->axis_number + command_name; + } + return command_name; +} + +template int Odrive::validateType(const odrive_json_object& json_object, int8_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int16_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int32_t&); +template int Odrive::validateType(const odrive_json_object& json_object, int64_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint8_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint16_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint32_t&); +template int Odrive::validateType(const odrive_json_object& json_object, uint64_t&); +template int Odrive::validateType(const odrive_json_object& json_object, float&); +template int Odrive::validateType(const odrive_json_object& json_object, bool&); + +template int Odrive::read(const std::string& parameter_name, int8_t&); +template int Odrive::read(const std::string& parameter_name, int16_t&); +template int Odrive::read(const std::string& parameter_name, int32_t&); +template int Odrive::read(const std::string& parameter_name, int64_t&); +template int Odrive::read(const std::string& parameter_name, uint8_t&); +template int Odrive::read(const std::string& parameter_name, uint16_t&); +template int Odrive::read(const std::string& parameter_name, uint32_t&); +template int Odrive::read(const std::string& parameter_name, uint64_t&); +template int Odrive::read(const std::string& parameter_name, float&); +template int Odrive::read(const std::string& parameter_name, bool&); + +template int Odrive::write(const std::string& parameter_name, int8_t&); +template int Odrive::write(const std::string& parameter_name, int16_t&); +template int Odrive::write(const std::string& parameter_name, int32_t&); +template int Odrive::write(const std::string& parameter_name, int64_t&); +template int Odrive::write(const std::string& parameter_name, uint8_t&); +template int Odrive::write(const std::string& parameter_name, uint16_t&); +template int Odrive::write(const std::string& parameter_name, uint32_t&); +template int Odrive::write(const std::string& parameter_name, uint64_t&); +template int Odrive::write(const std::string& parameter_name, float&); +template int Odrive::write(const std::string& parameter_name, bool&); +} // namespace march diff --git a/march_hardware/src/odrive/odrive_endpoint.cpp b/march_hardware/src/odrive/odrive_endpoint.cpp new file mode 100644 index 000000000..b2cd6a8ff --- /dev/null +++ b/march_hardware/src/odrive/odrive_endpoint.cpp @@ -0,0 +1,343 @@ +#include "march_hardware/motor_controller/odrive/odrive.h" +#include "march_hardware/motor_controller/odrive/odrive_endpoint.h" + +using namespace std; + +namespace march +{ +OdriveEndpoint::OdriveEndpoint() +{ + if (libusb_init(&lib_usb_context_) != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive could not connect to endpoint USB."); + } +} + +OdriveEndpoint::~OdriveEndpoint() +{ + this->remove(); + + if (lib_usb_context_ != nullptr) + { + libusb_exit(lib_usb_context_); + lib_usb_context_ = nullptr; + } +} + +void OdriveEndpoint::remove() +{ + if (odrive_handle_ != nullptr) + { + libusb_release_interface(odrive_handle_, 2); + libusb_close(odrive_handle_); + odrive_handle_ = nullptr; + } +} + +int OdriveEndpoint::open_connection(const std::string& serial_number) +{ + libusb_device** usb_device_list; + + ssize_t device_count = libusb_get_device_list(lib_usb_context_, &usb_device_list); + if (device_count <= 0) + { + ROS_WARN("No attached odrives found"); + return ODRIVE_COMM_ERROR; + } + + for (size_t i = 0; i < size_t(device_count); ++i) + { + libusb_device* device = usb_device_list[i]; + libusb_device_descriptor desc = {}; + + int result = libusb_get_device_descriptor(device, &desc); + if (result != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error getting device descriptor", serial_number.c_str()); + continue; + } + + if (desc.idVendor == ODRIVE_USB_VENDORID && desc.idProduct == ODRIVE_USB_PRODUCTID) + { + libusb_device_handle* device_handle; + struct libusb_config_descriptor* config; + + this->attached_to_handle_ = false; + unsigned char buf[128]; + + if (libusb_open(device, &device_handle) != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error opening USB device", serial_number.c_str()); + continue; + } + + result = libusb_get_config_descriptor(device, 0, &config); + if (result != LIBUSB_SUCCESS) + { + ROS_WARN("Odrive %s error getting device descriptor", serial_number.c_str()); + continue; + } + + int ifNumber = 2; // config->bNumInterfaces; + + if ((libusb_kernel_driver_active(device_handle, ifNumber) != LIBUSB_SUCCESS) && + (libusb_detach_kernel_driver(device_handle, ifNumber) != LIBUSB_SUCCESS)) + { + libusb_close(device_handle); + + ROS_ERROR("Odrive %s driver error", serial_number.c_str()); + continue; + } + + if (libusb_claim_interface(device_handle, ifNumber) != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error claiming device", serial_number.c_str()); + libusb_close(device_handle); + continue; + } + else + { + result = libusb_get_string_descriptor_ascii(device_handle, desc.iSerialNumber, buf, 127); + if (result <= 0) + { + libusb_release_interface(device_handle, ifNumber); + libusb_close(device_handle); + + ROS_ERROR("Odrive %s error getting data", serial_number.c_str()); + continue; + } + else + { + std::stringstream stream; + stream << uppercase << std::hex << stoull(serial_number, nullptr, 16); + std::string sn(stream.str()); + + if (sn.compare(0, strlen((const char*)buf), (const char*)buf) == 0) + { + ROS_INFO("Odrive with serial number; %s found", serial_number.c_str()); + this->attached_to_handle_ = true; + this->odrive_serial_number = serial_number; + + odrive_handle_ = device_handle; + break; + } + } + if (!this->attached_to_handle_) + { + libusb_release_interface(device_handle, ifNumber); + libusb_close(device_handle); + } + } + } + } + + libusb_free_device_list(usb_device_list, 1); + + return ODRIVE_COMM_SUCCESS; +} + +template +int OdriveEndpoint::getData(int id, T& value) +{ + commBuffer tx; + commBuffer rx; + int rx_size; + + int result = this->endpointRequest(id, rx, rx_size, tx, true, sizeof(value)); + if (result != ODRIVE_COMM_SUCCESS) + { + return result; + } + + memcpy(&value, &rx[0], sizeof(value)); + + return ODRIVE_COMM_SUCCESS; +} + +template +int OdriveEndpoint::setData(int endpoint_id, const TT& value) +{ + commBuffer tx; + commBuffer rx; + int rx_length; + + for (int i = 0; i < int(sizeof(value)); i++) + { + tx.push_back(((unsigned char*)&value)[i]); + } + + return this->endpointRequest(endpoint_id, rx, rx_length, tx, true, 0); +} + +int OdriveEndpoint::execFunc(int endpoint_id) +{ + commBuffer tx; + commBuffer rx; + int rx_length; + + return this->endpointRequest(endpoint_id, rx, rx_length, tx, false, 0); +} + +int OdriveEndpoint::endpointRequest(int endpoint_id, commBuffer& received_payload, int& received_length, + const commBuffer& payload, bool ack, int length, bool read, int address) +{ + commBuffer send_buffer; + commBuffer receive_buffer; + + unsigned char receive_bytes[ODRIVE_MAX_RESULT_LENGTH] = { 0 }; + + int sent_bytes = 0; + int received_bytes = 0; + short received_seq_no = 0; + + this->ep_lock_.lock(); + + // Prepare sequence number + if (ack) + { + endpoint_id |= 0x8000; + } + outbound_seq_no_ = (outbound_seq_no_ + 1) & 0x7fff; + outbound_seq_no_ |= LIBUSB_ENDPOINT_IN; + short seq_no = outbound_seq_no_; + + // Create request packet + commBuffer packet = createODrivePacket(seq_no, endpoint_id, length, read, address, payload); + + // Transfer packet to target + int transfer_result = + libusb_bulk_transfer(odrive_handle_, ODRIVE_OUT_EP, packet.data(), packet.size(), &sent_bytes, ODRIVE_TIMEOUT); + if (transfer_result != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error in transferring data to USB, error id %i", this->odrive_serial_number.c_str(), + transfer_result); + + this->ep_lock_.unlock(); + return transfer_result; + } + + else if (int(packet.size()) != sent_bytes) + { + ROS_ERROR("Odrive %s error not all data transferring to USB was successful", this->odrive_serial_number.c_str()); + } + + // Get response + if (ack) + { + int ack_result = libusb_bulk_transfer(odrive_handle_, ODRIVE_IN_EP, receive_bytes, ODRIVE_MAX_BYTES_TO_RECEIVE, + &received_bytes, ODRIVE_TIMEOUT); + if (ack_result != LIBUSB_SUCCESS) + { + ROS_ERROR("Odrive %s error in acknowledging response from USB, error id %i", this->odrive_serial_number.c_str(), + ack_result); + + this->ep_lock_.unlock(); + return ack_result; + } + + // Push received data to buffer + for (int i = 0; i < received_bytes; i++) + { + receive_buffer.push_back(receive_bytes[i]); + } + + received_payload = decodeODrivePacket(receive_buffer, received_seq_no); + if (received_seq_no != seq_no) + { + ROS_ERROR("Odrive %s error received data out of order", this->odrive_serial_number.c_str()); + } + received_length = received_payload.size(); + } + + this->ep_lock_.unlock(); + return LIBUSB_SUCCESS; +} + +std::string OdriveEndpoint::getSerialNumber() +{ + return this->odrive_serial_number; +} + +void OdriveEndpoint::appendShortToCommBuffer(commBuffer& buf, short value) +{ + buf.push_back((value >> 0) & 0xFF); + buf.push_back((value >> 8) & 0xFF); +} + +void OdriveEndpoint::appendIntToCommBuffer(commBuffer& buf, const int value) +{ + buf.push_back((value >> 0) & 0xFF); + buf.push_back((value >> 8) & 0xFF); + buf.push_back((value >> 16) & 0xFF); + buf.push_back((value >> 24) & 0xFF); +} + +commBuffer OdriveEndpoint::decodeODrivePacket(commBuffer& buf, short& seq_no) +{ + commBuffer payload; + + memcpy(&seq_no, &buf[0], sizeof(short)); + seq_no &= 0x7fff; + + for (commBuffer::size_type i = 2; i < buf.size(); ++i) + { + payload.push_back(buf[i]); + } + return payload; +} + +commBuffer OdriveEndpoint::createODrivePacket(short seq_no, int endpoint_id, short response_size, bool read, + int address, const commBuffer& input) +{ + commBuffer packet; + this->crc_ = 0; + + if ((endpoint_id & 0x7fff) == 0) + { + this->crc_ = ODRIVE_PROTOCOL_VERION; + } + else + { + this->crc_ = ODRIVE_DEFAULT_CRC_VALUE; + } + + appendShortToCommBuffer(packet, seq_no); + appendShortToCommBuffer(packet, endpoint_id); + appendShortToCommBuffer(packet, response_size); + if (read) + { + appendIntToCommBuffer(packet, address); + } + + for (uint8_t b : input) + { + packet.push_back(b); + } + + appendShortToCommBuffer(packet, this->crc_); + + return packet; +} + +template int OdriveEndpoint::getData(int, bool&); +template int OdriveEndpoint::getData(int, float&); +template int OdriveEndpoint::getData(int, uint8_t&); +template int OdriveEndpoint::getData(int, uint16_t&); +template int OdriveEndpoint::getData(int, uint32_t&); +template int OdriveEndpoint::getData(int, uint64_t&); +template int OdriveEndpoint::getData(int, int8_t&); +template int OdriveEndpoint::getData(int, int16_t&); +template int OdriveEndpoint::getData(int, int32_t&); +template int OdriveEndpoint::getData(int, int64_t&); + +template int OdriveEndpoint::setData(int, const bool&); +template int OdriveEndpoint::setData(int, const float&); +template int OdriveEndpoint::setData(int, const uint8_t&); +template int OdriveEndpoint::setData(int, const uint16_t&); +template int OdriveEndpoint::setData(int, const uint32_t&); +template int OdriveEndpoint::setData(int, const uint64_t&); +template int OdriveEndpoint::setData(int, const int8_t&); +template int OdriveEndpoint::setData(int, const int16_t&); +template int OdriveEndpoint::setData(int, const int32_t&); +template int OdriveEndpoint::setData(int, const int64_t&); +} // namespace march diff --git a/march_hardware/src/odrive/odrive_motor.cpp b/march_hardware/src/odrive/odrive_motor.cpp new file mode 100644 index 000000000..40ae398a2 --- /dev/null +++ b/march_hardware/src/odrive/odrive_motor.cpp @@ -0,0 +1,265 @@ +#include "march_hardware/motor_controller/odrive/odrive_motor.h" +#include "march_hardware/motor_controller/odrive/odrive_states.h" + +#include + +namespace march +{ +OdriveMotor::OdriveMotor(const std::string& axisNumber, std::shared_ptr odriveEndpoint, + ActuationMode mode, std::string json_config_file_path) + : Odrive(axisNumber, std::move(odriveEndpoint), false), mode_(mode), json_config_file_path_(json_config_file_path) +{ +} + +OdriveMotor::~OdriveMotor() +{ + if (this->setState(States::AXIS_STATE_IDLE) == 1) + { + ROS_FATAL("Not set to idle when closed"); + return; + } +} + +bool OdriveMotor::initialize(int cycle_time) +{ + return cycle_time; +} + +void OdriveMotor::prepareActuation() +{ + this->importOdriveJson(); + + if (this->setConfigurations(this->json_config_file_path_) == 1) + { + ROS_FATAL("Setting configurations was not finished successfully"); + } + + this->reset(); + + if (this->setState(States::AXIS_STATE_FULL_CALIBRATION_SEQUENCE) == 1) + { + ROS_FATAL("Calibration sequence was not finished successfully"); + return; + } + + this->waitForIdleState(); + + if (this->setState(States::AXIS_STATE_CLOSED_LOOP_CONTROL) == 1) + { + ROS_FATAL("Setting closed loop control was not finished successfully"); + return; + } + this->readValues(); +} + +bool OdriveMotor::waitForIdleState(float timeout) +{ + float current_time = 0; + while (this->getState() != States::AXIS_STATE_IDLE) + { + ros::Duration(0.5).sleep(); + current_time += 0.5; + + if (current_time == timeout) + { + ROS_FATAL("Odrive axis did not return to idle state, current state is %i", this->getState()); + return false; + } + } + return true; +} + +// to be implemented +void OdriveMotor::reset() +{ + uint16_t axis_error = 0; + std::string command_name_ = this->create_command(O_PM_AXIS_ERROR); + if (this->write(command_name_, axis_error) == 1) + { + ROS_ERROR("Could not reset axis"); + } + + uint16_t axis_motor_error = 0; + command_name_ = this->create_command(O_PM_AXIS_MOTOR_ERROR); + if (this->write(command_name_, axis_motor_error) == 1) + { + ROS_ERROR("Could not reset motor axis"); + } + + uint8_t axis_encoder_error = 0; + command_name_ = this->create_command(O_PM_AXIS_ENCODER_ERROR); + if (this->write(command_name_, axis_encoder_error) == 1) + { + ROS_ERROR("Could not reset encoder axis"); + } + + uint8_t axis_controller_error = 0; + command_name_ = this->create_command(O_PM_AXIS_CONTROLLER_ERROR); + if (this->write(command_name_, axis_controller_error) == 1) + { + ROS_ERROR("Could not reset controller axis"); + } +} + +void OdriveMotor::actuateRad(double target_rad) +{ + ROS_INFO("Actuating rad %f", target_rad); + return; +} + +void OdriveMotor::actuateTorque(double target_torque_ampere) +{ + float target_torque_ampere_float = (float)target_torque_ampere; + + std::string command_name_ = this->create_command(O_PM_DESIRED_MOTOR_CURRENT); + if (this->write(command_name_, target_torque_ampere_float) == 1) + { + ROS_ERROR("Could net set target torque; %f to the axis", target_torque_ampere); + } + + this->readValues(); +} + +MotorControllerStates& OdriveMotor::getStates() +{ + static OdriveStates states; + + states.motorCurrent = this->getMotorCurrent(); + states.controllerVoltage = this->getMotorControllerVoltage(); + states.motorVoltage = this->getMotorVoltage(); + + states.absoluteEncoderValue = this->getAngleCountsAbsolute(); + states.incrementalEncoderValue = this->getAngleCountsIncremental(); + states.absoluteVelocity = this->getVelocityRadAbsolute(); + states.incrementalVelocity = this->getVelocityRadIncremental(); + + states.axisError = this->getAxisError(); + states.axisMotorError = this->getAxisMotorError(); + states.axisEncoderError = this->getAxisEncoderError(); + states.axisControllerError = this->getAxisControllerError(); + + states.state = States(this->getState()); + + return states; +} + +void OdriveMotor::readValues() +{ + this->axis_error = this->readAxisError(); + this->axis_motor_error = this->readAxisMotorError(); + this->axis_encoder_error = this->readAxisEncoderError(); + this->axis_controller_error = this->readAxisControllerError(); + + this->motor_controller_voltage = this->readMotorControllerVoltage(); + this->motor_current = this->readMotorCurrent(); + this->motor_voltage = this->readMotorVoltage(); + + this->angle_counts_absolute = this->readAngleCountsAbsolute(); + this->velocity_rad_absolute = this->readVelocityRadAbsolute(); + this->angle_counts_incremental = this->readAngleCountsIncremental(); + this->velocity_rad_incremental = this->readVelocityRadIncremental(); +} + +uint16_t OdriveMotor::getAxisError() +{ + return this->axis_error; +} + +uint16_t OdriveMotor::getAxisMotorError() +{ + return this->axis_motor_error; +} + +uint8_t OdriveMotor::getAxisEncoderError() +{ + return this->axis_encoder_error; +} + +uint8_t OdriveMotor::getAxisControllerError() +{ + return this->axis_controller_error; +} + +float OdriveMotor::getMotorControllerVoltage() +{ + return this->motor_controller_voltage; +} + +float OdriveMotor::getMotorCurrent() +{ + return this->motor_current; +} + +float OdriveMotor::getMotorVoltage() +{ + return this->motor_voltage; +} + +double OdriveMotor::getTorque() +{ + return this->getMotorCurrent(); +} + +int OdriveMotor::getAngleCountsAbsolute() +{ + return this->angle_counts_absolute; +} + +double OdriveMotor::getAngleRadAbsolute() +{ + double angle_rad = this->getAngleCountsAbsolute() * PI_2 / (std::pow(2, 12) * 101); + return angle_rad; +} + +double OdriveMotor::getVelocityRadAbsolute() +{ + return this->velocity_rad_absolute; +} + +bool OdriveMotor::getIncrementalMorePrecise() const +{ + return true; +} + +int OdriveMotor::getAngleCountsIncremental() +{ + return this->angle_counts_incremental; +} + +double OdriveMotor::getAngleRadIncremental() +{ + double angle_rad = this->getAngleCountsIncremental() * PI_2 / (std::pow(2, 12) * 101); + return angle_rad; +} + +double OdriveMotor::getVelocityRadIncremental() +{ + double velocity_rad_incremental_double = this->velocity_rad_incremental * -1; + return velocity_rad_incremental_double; +} + +int OdriveMotor::setState(uint8_t state) +{ + std::string command_name_ = this->create_command(O_PM_REQUEST_STATE); + if (this->write(command_name_, state) == 1) + { + ROS_ERROR("Could net set state; %i to the axis", state); + return ODRIVE_ERROR; + } + return ODRIVE_OK +} + +uint8_t OdriveMotor::getState() +{ + uint8_t axis_state; + std::string command_name_ = this->create_command(O_PM_CURRENT_STATE); + if (this->read(command_name_, axis_state) == 1) + { + ROS_ERROR("Could not retrieve state of the motor"); + return ODRIVE_ERROR; + } + + return axis_state; +} + +} // namespace march diff --git a/march_hardware/src/odrive/usb_master.cpp b/march_hardware/src/odrive/usb_master.cpp new file mode 100644 index 000000000..dbc8ab04f --- /dev/null +++ b/march_hardware/src/odrive/usb_master.cpp @@ -0,0 +1,25 @@ +// Copyright 2019 Project March. +#include "march_hardware/motor_controller/odrive/usb_master.h" +#include + +namespace march +{ +std::shared_ptr UsbMaster::getSerialConnection(const std::string& serial_number) +{ + for (uint i = 0; i < this->odrive_endpoints_.size(); ++i) + { + if (this->odrive_endpoints_[i]->getSerialNumber().compare(serial_number) == 0) + { + return this->odrive_endpoints_[i]; + } + } + + std::shared_ptr odrive_endpoint = std::make_shared(); + ROS_INFO("Open serial connection %s", serial_number.c_str()); + odrive_endpoint->open_connection(serial_number); + + this->odrive_endpoints_.push_back(odrive_endpoint); + + return odrive_endpoint; +} +} // namespace march diff --git a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h index 6264f4bf7..3f093a61c 100644 --- a/march_hardware_builder/include/march_hardware_builder/allowed_robot.h +++ b/march_hardware_builder/include/march_hardware_builder/allowed_robot.h @@ -15,8 +15,10 @@ class AllowedRobot march4, march3, test_joint_rotational, + odrive_test_joint_rotational, test_joint_linear, pdb, + two_odrive_joints, }; AllowedRobot() = default; @@ -34,6 +36,14 @@ class AllowedRobot { this->value = test_joint_rotational; } + else if (robot_name == "odrive_test_joint_rotational") + { + this->value = odrive_test_joint_rotational; + } + else if (robot_name == "two_odrive_joints") + { + this->value = two_odrive_joints; + } else if (robot_name == "test_joint_linear") { this->value = test_joint_linear; @@ -62,7 +72,15 @@ class AllowedRobot } else if (this->value == AllowedRobot::test_joint_rotational) { - return base_path.append("/robots/test_joint_rotational.yaml"); + return base_path.append("/robots/odrive_test_joint_rotational.yaml"); + } + else if (this->value == AllowedRobot::two_odrive_joints) + { + return base_path.append("/robots/two_odrive_joints.yaml"); + } + else if (this->value == AllowedRobot::odrive_test_joint_rotational) + { + return base_path.append("/robots/odrive_test_joint_rotational.yaml"); } else if (this->value == AllowedRobot::test_joint_linear) { @@ -105,6 +123,12 @@ class AllowedRobot case test_joint_rotational: out << "test_joint_rotational"; break; + case odrive_test_joint_rotational: + out << "odrive_test_joint_rotational"; + break; + case two_odrive_joints: + out << "two_odrive_joints"; + break; case pdb: out << "pdb"; break; diff --git a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h index 823b8c785..63a3a6886 100644 --- a/march_hardware_builder/include/march_hardware_builder/hardware_builder.h +++ b/march_hardware_builder/include/march_hardware_builder/hardware_builder.h @@ -21,6 +21,9 @@ #include #include +#include +#include + /** * @brief Creates a MarchRobot from a robot yaml and URDF. */ @@ -68,7 +71,7 @@ class HardwareBuilder static march::Joint createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface); + march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master); static std::unique_ptr createAbsoluteEncoder(const YAML::Node& absolute_encoder_config, const urdf::JointConstSharedPtr& urdf_joint); static std::unique_ptr @@ -84,9 +87,14 @@ class HardwareBuilder createPowerDistributionBoard(const YAML::Node& power_distribution_board_config, march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface); + static std::unique_ptr createOdrive(const YAML::Node& imc_config, march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::UsbMaster& usb_master); + static const std::vector INCREMENTAL_ENCODER_REQUIRED_KEYS; static const std::vector ABSOLUTE_ENCODER_REQUIRED_KEYS; static const std::vector IMOTIONCUBE_REQUIRED_KEYS; + static const std::vector ODRIVE_REQUIRED_KEYS; static const std::vector TEMPERATUREGES_REQUIRED_KEYS; static const std::vector POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS; static const std::vector JOINT_REQUIRED_KEYS; @@ -105,7 +113,7 @@ class HardwareBuilder * @return list of created joints */ std::vector createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface) const; + march::SdoInterfacePtr sdo_interface, march::UsbMaster& usb_master) const; YAML::Node robot_config_; urdf::Model urdf_; @@ -117,4 +125,9 @@ class HardwareBuilder */ std::string convertSWFileToString(std::ifstream& sw_file); +/** + * Returns the highest slave index of motor controllers and GESs in joints + */ +int getMaxSlaveIndex(std::vector& jointList); + #endif // MARCH_HARDWARE_BUILDER_HARDWARE_BUILDER_H diff --git a/march_hardware_builder/robots/march3.yaml b/march_hardware_builder/robots/march3.yaml index 15b819537..44d760365 100644 --- a/march_hardware_builder/robots/march3.yaml +++ b/march_hardware_builder/robots/march3.yaml @@ -1,6 +1,6 @@ march3: ifName: enp3s0 - ecatCycleTime: 4 + cycleTime: 4 joints: - right_hip: actuationMode: position diff --git a/march_hardware_builder/robots/march4.yaml b/march_hardware_builder/robots/march4.yaml index c1ee0b158..c895c200b 100644 --- a/march_hardware_builder/robots/march4.yaml +++ b/march_hardware_builder/robots/march4.yaml @@ -1,8 +1,8 @@ # For convenience it is easiest if the joint order is maintained, it is chosen to sort the joints alphabetically. march4: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - left_ankle: actuationMode: torque diff --git a/march_hardware_builder/robots/odrive_test_joint_rotational.yaml b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml new file mode 100644 index 000000000..f7158555c --- /dev/null +++ b/march_hardware_builder/robots/odrive_test_joint_rotational.yaml @@ -0,0 +1,17 @@ +testsetup: + cycleTime: 4 + slaveTimeout: 50 + joints: + - rotational_joint: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 97242 + maxPositionIU: 119617 + incrementalEncoder: + resolution: 12 + transmission: 101 diff --git a/march_hardware_builder/robots/pdb.yaml b/march_hardware_builder/robots/pdb.yaml index fa355356f..115a9270c 100644 --- a/march_hardware_builder/robots/pdb.yaml +++ b/march_hardware_builder/robots/pdb.yaml @@ -1,6 +1,6 @@ pdb: ifName: enp2s0 - ecatCycleTime: 4 + cycleTime: 4 powerDistributionBoard: slaveIndex: 1 bootShutdownOffsets: diff --git a/march_hardware_builder/robots/test_joint_linear.yaml b/march_hardware_builder/robots/test_joint_linear.yaml index 72f44c9b8..7da655726 100644 --- a/march_hardware_builder/robots/test_joint_linear.yaml +++ b/march_hardware_builder/robots/test_joint_linear.yaml @@ -1,7 +1,7 @@ testjoint_linear: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - linear_joint: actuationMode: torque diff --git a/march_hardware_builder/robots/test_joint_rotational.yaml b/march_hardware_builder/robots/test_joint_rotational.yaml index 90fbdd145..2cea384c8 100644 --- a/march_hardware_builder/robots/test_joint_rotational.yaml +++ b/march_hardware_builder/robots/test_joint_rotational.yaml @@ -1,7 +1,7 @@ testsetup: ifName: enp2s0 - ecatCycleTime: 4 - ecatSlaveTimeout: 50 + cycleTime: 4 + slaveTimeout: 50 joints: - rotational_joint: actuationMode: torque @@ -10,8 +10,8 @@ testsetup: slaveIndex: 1 absoluteEncoder: resolution: 17 - minPositionIU: 97242 - maxPositionIU: 119617 + minPositionIU: 29266 + maxPositionIU: 52948 incrementalEncoder: resolution: 12 transmission: 101 diff --git a/march_hardware_builder/robots/two_odrive_joints.yaml b/march_hardware_builder/robots/two_odrive_joints.yaml new file mode 100644 index 000000000..7579ecf8b --- /dev/null +++ b/march_hardware_builder/robots/two_odrive_joints.yaml @@ -0,0 +1,32 @@ +# For convenience it is easiest if the joint order is maintained, it is chosen to sort the joints alphabetically. +march4: + cycleTime: 4 + slaveTimeout: 50 + joints: + - right_hip_fe: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis0" + absoluteEncoder: + resolution: 17 + minPositionIU: 2045 + maxPositionIU: 45632 + incrementalEncoder: + resolution: 12 + transmission: 101 + + - right_knee: + actuationMode: torque + allowActuation: true + odrive: + serial_number: "0x2084387E304E" + axis: "axis1" + absoluteEncoder: + resolution: 17 + minPositionIU: 41533 + maxPositionIU: 85298 + incrementalEncoder: + resolution: 12 + transmission: 101 diff --git a/march_hardware_builder/src/hardware_builder.cpp b/march_hardware_builder/src/hardware_builder.cpp index 5972dc57a..90b4d6632 100644 --- a/march_hardware_builder/src/hardware_builder.cpp +++ b/march_hardware_builder/src/hardware_builder.cpp @@ -14,6 +14,7 @@ #include #include #include +#include "march_hardware/ethercat/ethercat_master.h" #include // clang-format off @@ -26,6 +27,10 @@ const std::vector HardwareBuilder::IMOTIONCUBE_REQUIRED_KEYS = { "slaveIndex", "incrementalEncoder", "absoluteEncoder" }; +const std::vector HardwareBuilder::ODRIVE_REQUIRED_KEYS = + { + "serial_number", "axis", "incrementalEncoder", "absoluteEncoder" + }; const std::vector HardwareBuilder::TEMPERATUREGES_REQUIRED_KEYS = { "slaveIndex", "byteOffset" }; const std::vector HardwareBuilder::POWER_DISTRIBUTION_BOARD_REQUIRED_KEYS = { @@ -55,30 +60,45 @@ HardwareBuilder::HardwareBuilder(const std::string& yaml_path, urdf::Model urdf) std::unique_ptr HardwareBuilder::createMarchRobot() { this->initUrdf(); - auto pdo_interface = march::PdoInterfaceImpl::create(); - auto sdo_interface = march::SdoInterfaceImpl::create(); const auto robot_name = this->robot_config_.begin()->first.as(); ROS_DEBUG_STREAM("Starting creation of robot " << robot_name); // Remove top level robot name key YAML::Node config = this->robot_config_[robot_name]; - const auto if_name = config["ifName"].as(); - const auto cycle_time = config["ecatCycleTime"].as(); - const auto slave_timeout = config["ecatSlaveTimeout"].as(); + const auto cycle_time = config["cycleTime"].as(); + const auto slave_timeout = config["slaveTimeout"].as(); + + auto pdo_interface = march::PdoInterfaceImpl::create(); + auto sdo_interface = march::SdoInterfaceImpl::create(); + + march::UsbMaster usb_master = march::UsbMaster(); - std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface); + std::vector joints = this->createJoints(config["joints"], pdo_interface, sdo_interface, usb_master); ROS_INFO_STREAM("Robot config:\n" << config); YAML::Node pdb_config = config["powerDistributionBoard"]; auto pdb = HardwareBuilder::createPowerDistributionBoard(pdb_config, pdo_interface, sdo_interface); - return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), if_name, cycle_time, - slave_timeout); + + if (config["ifName"]) + { + const auto if_name = config["ifName"].as(); + auto ethercat_master = + std::make_unique(if_name, getMaxSlaveIndex(joints), cycle_time, slave_timeout); + + return std::make_unique(std::move(joints), this->urdf_, std::move(pdb), + std::move(ethercat_master)); + } + else + { + return std::make_unique(std::move(joints), this->urdf_, std::move(pdb)); + } } march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const std::string& joint_name, const urdf::JointConstSharedPtr& urdf_joint, - march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface) + march::PdoInterfacePtr pdo_interface, march::SdoInterfacePtr sdo_interface, + march::UsbMaster& usb_master) { ROS_DEBUG("Starting creation of joint %s", joint_name.c_str()); if (!urdf_joint) @@ -112,6 +132,10 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const controller = HardwareBuilder::createIMotionCube(joint_config["imotioncube"], mode, urdf_joint, pdo_interface, sdo_interface); } + if (joint_config["odrive"]) + { + controller = HardwareBuilder::createOdrive(joint_config["odrive"], mode, urdf_joint, usb_master); + } if (!controller) { ROS_FATAL("Joint %s does not have a configuration for a motor controller", joint_name.c_str()); @@ -125,6 +149,43 @@ march::Joint HardwareBuilder::createJoint(const YAML::Node& joint_config, const return { joint_name, net_number, allow_actuation, std::move(controller), std::move(ges) }; } +std::unique_ptr HardwareBuilder::createOdrive(const YAML::Node& odrive_config, + march::ActuationMode mode, + const urdf::JointConstSharedPtr& urdf_joint, + march::UsbMaster& usb_master) +{ + if (!odrive_config || !urdf_joint) + { + return nullptr; + } + + HardwareBuilder::validateRequiredKeysExist(odrive_config, HardwareBuilder::ODRIVE_REQUIRED_KEYS, "odrive"); + + YAML::Node incremental_encoder_config = odrive_config["incrementalEncoder"]; + YAML::Node absolute_encoder_config = odrive_config["absoluteEncoder"]; + std::string axis = odrive_config["axis"].as(); + std::string serial_number = odrive_config["serial_number"].as(); + + std::string configuration_path = + ros::package::getPath("march_hardware").append("/config/" + urdf_joint->name + ".json"); + + std::ifstream file(configuration_path); + + if (file.fail()) + { + throw std::runtime_error("Could not open configuration file for the odrive"); + } + + if (file.is_open()) + { + file.close(); + } + + auto odrive_endpoint = usb_master.getSerialConnection(serial_number); + + return std::make_unique(axis, odrive_endpoint, mode, configuration_path); +} + std::unique_ptr HardwareBuilder::createIMotionCube(const YAML::Node& imc_config, march::ActuationMode mode, const urdf::JointConstSharedPtr& urdf_joint, @@ -281,7 +342,8 @@ void HardwareBuilder::initUrdf() std::vector HardwareBuilder::createJoints(const YAML::Node& joints_config, march::PdoInterfacePtr pdo_interface, - march::SdoInterfacePtr sdo_interface) const + march::SdoInterfacePtr sdo_interface, + march::UsbMaster& usb_master) const { std::vector joints; for (const YAML::Node& joint_config : joints_config) @@ -292,8 +354,8 @@ std::vector HardwareBuilder::createJoints(const YAML::Node& joints { ROS_WARN("Joint %s is fixed in the URDF, but defined in the robot yaml", joint_name.c_str()); } - joints.push_back( - HardwareBuilder::createJoint(joint_config[joint_name], joint_name, urdf_joint, pdo_interface, sdo_interface)); + joints.push_back(HardwareBuilder::createJoint(joint_config[joint_name], joint_name, urdf_joint, pdo_interface, + sdo_interface, usb_master)); } for (const auto& urdf_joint : this->urdf_.joints_) @@ -317,3 +379,28 @@ std::string convertSWFileToString(std::ifstream& sw_file) { return std::string(std::istreambuf_iterator(sw_file), std::istreambuf_iterator()); } + +/** + * Returns the highest slave index of motor controllers and GESs in joints + */ +int getMaxSlaveIndex(std::vector& jointList) +{ + int maxSlaveIndex = -1; + + for (march::Joint& joint : jointList) + { + int temperatureSlaveIndex = joint.getTemperatureGESSlaveIndex(); + if (temperatureSlaveIndex > maxSlaveIndex) + { + maxSlaveIndex = temperatureSlaveIndex; + } + + int motorControllerSlaveIndex = joint.getMotorControllerSlaveIndex() > -1; + + if (motorControllerSlaveIndex > maxSlaveIndex) + { + maxSlaveIndex = motorControllerSlaveIndex; + } + } + return maxSlaveIndex; +} diff --git a/march_hardware_builder/test/joint_builder_test.cpp b/march_hardware_builder/test/joint_builder_test.cpp index 3d22b4ea0..04e6cc843 100644 --- a/march_hardware_builder/test/joint_builder_test.cpp +++ b/march_hardware_builder/test/joint_builder_test.cpp @@ -12,6 +12,7 @@ #include #include #include +#include class JointBuilderTest : public ::testing::Test { @@ -45,9 +46,10 @@ TEST_F(JointBuilderTest, ValidJointHip) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 1.9; + auto usb_master = march::UsbMaster(); const std::string name = "test_joint_hip"; march::Joint created = - HardwareBuilder::createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface); + HardwareBuilder::createJoint(config, name, this->joint, this->pdo_interface, this->sdo_interface, usb_master); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -70,8 +72,9 @@ 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 usb_master = march::UsbMaster(); + march::Joint created = HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, + this->sdo_interface, usb_master); auto absolute_encoder = std::make_unique( 16, 22134, 43436, this->joint->limits->lower, this->joint->limits->upper, this->joint->safety->soft_lower_limit, @@ -90,16 +93,19 @@ TEST_F(JointBuilderTest, NoActuate) { YAML::Node config = this->loadTestYaml("/joint_no_actuate.yaml"); + auto usb_master = march::UsbMaster(); + ASSERT_THROW(HardwareBuilder::createJoint(config, "test_joint_no_actuate", this->joint, this->pdo_interface, - this->sdo_interface), + this->sdo_interface, usb_master), MissingKeyException); } TEST_F(JointBuilderTest, NoIMotionCube) { + auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_no_imotioncube.yaml"); march::Joint joint = HardwareBuilder::createJoint(config, "test_joint_no_imotioncube", this->joint, - this->pdo_interface, this->sdo_interface); + this->pdo_interface, this->sdo_interface, usb_master); ASSERT_FALSE(joint.hasMotorController()); } @@ -112,8 +118,10 @@ TEST_F(JointBuilderTest, NoTemperatureGES) this->joint->safety->soft_lower_limit = 0.1; this->joint->safety->soft_upper_limit = 0.15; + auto usb_master = march::UsbMaster(); + ASSERT_NO_THROW(HardwareBuilder::createJoint(config, "test_joint_no_temperature_ges", this->joint, - this->pdo_interface, this->sdo_interface)); + this->pdo_interface, this->sdo_interface, usb_master)); } TEST_F(JointBuilderTest, ValidActuationMode) @@ -124,8 +132,9 @@ 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); + auto usb_master = march::UsbMaster(); + march::Joint created = HardwareBuilder::createJoint(config, "test_joint_hip", this->joint, this->pdo_interface, + this->sdo_interface, usb_master); march::Joint expected("test_joint_hip", -1, false, std::make_unique( @@ -141,14 +150,18 @@ TEST_F(JointBuilderTest, ValidActuationMode) TEST_F(JointBuilderTest, EmptyJoint) { YAML::Node config; - ASSERT_THROW( - HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, this->sdo_interface), - MissingKeyException); + + auto usb_master = march::UsbMaster(); + ASSERT_THROW(HardwareBuilder::createJoint(config, "test_joint_empty", this->joint, this->pdo_interface, + this->sdo_interface, usb_master), + MissingKeyException); } TEST_F(JointBuilderTest, NoUrdfJoint) { + auto usb_master = march::UsbMaster(); YAML::Node config = this->loadTestYaml("/joint_correct.yaml"); - ASSERT_THROW(HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface), - march::error::HardwareException); + ASSERT_THROW( + HardwareBuilder::createJoint(config, "test", nullptr, this->pdo_interface, this->sdo_interface, usb_master), + march::error::HardwareException); } diff --git a/march_hardware_interface/config/test_joint_rotational/controllers.yaml b/march_hardware_interface/config/test_joint_rotational/controllers.yaml index 90861bc5c..a364f96cb 100644 --- a/march_hardware_interface/config/test_joint_rotational/controllers.yaml +++ b/march_hardware_interface/config/test_joint_rotational/controllers.yaml @@ -10,7 +10,7 @@ march: type: march_pdb_state_controller/MarchPdbStateController publish_rate: 50 trajectory: - type: position_controllers/JointTrajectoryController + type: effort_controllers/JointTrajectoryController joints: - rotational_joint constraints: @@ -18,3 +18,5 @@ march: margin_soft_limit_error: 0.5 trajectory: 0.305 goal: 0.305 + gains: # Required because we're controlling an effort interface + rotational_joint: {p: 150, i: 0, d: 10, i_clamp: 100, publish_state: true, antiwindup: true} diff --git a/march_hardware_interface/config/two_odrive_joints/controllers.yaml b/march_hardware_interface/config/two_odrive_joints/controllers.yaml new file mode 100644 index 000000000..e038f46c2 --- /dev/null +++ b/march_hardware_interface/config/two_odrive_joints/controllers.yaml @@ -0,0 +1,29 @@ +march: + controller: + pdb_state: + type: march_pdb_state_controller/MarchPdbStateController + publish_rate: 50 + joint_state: + type: joint_state_controller/JointStateController + publish_rate: 250 + temperature_sensor: + type: march_temperature_sensor_controller/MarchTemperatureSensorController + publish_rate: 1 + trajectory: + type: effort_controllers/JointTrajectoryController + allow_partial_joints_goal: true + joints: + - right_hip_fe + - right_knee + gains: # Required because we're controlling an effort interface + right_hip_fe: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} + right_knee: {p: 0, i: 0, d: 0, i_clamp: 100, publish_state: true, antiwindup: true} + constraints: + right_hip_fe: + margin_soft_limit_error: 0.5 + trajectory: 0.305 + goal: 0.305 + right_knee: + margin_soft_limit_error: 0.5 + trajectory: 0.305 + goal: 0.305 diff --git a/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h b/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h index 27e0888b1..abf144307 100644 --- a/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h +++ b/march_hardware_interface/include/march_hardware_interface/march_hardware_interface.h @@ -64,14 +64,14 @@ class MarchHardwareInterface : public hardware_interface::RobotHW void write(const ros::Time& time, const ros::Duration& elapsed_time) override; /** - * Returns the ethercat cycle time in milliseconds. + * Returns the communication cycle time in milliseconds. */ - int getEthercatCycleTime() const; + int getCycleTime() const; /** - * Wait for received PDO. + * Wait for received communication update. */ - void waitForPdo(); + void waitForUpdate(); private: void uploadJointNames(ros::NodeHandle& nh) const; diff --git a/march_hardware_interface/src/march_hardware_interface.cpp b/march_hardware_interface/src/march_hardware_interface.cpp index 73cea5731..667c38a84 100644 --- a/march_hardware_interface/src/march_hardware_interface.cpp +++ b/march_hardware_interface/src/march_hardware_interface.cpp @@ -48,8 +48,8 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot this->reserveMemory(); - // Start ethercat cycle in the hardware - this->march_robot_->startEtherCAT(this->reset_motor_controllers_); + // Start communication cycle in the hardware + this->march_robot_->startCommunication(this->reset_motor_controllers_); for (size_t i = 0; i < num_joints_; ++i) { @@ -144,7 +144,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot &joint_temperature_variance_[i]); march_temperature_interface_.registerHandle(temperature_sensor_handle); - // Prepare Motor Controllers for actuations + // Prepare Motor Controllers for actuation if (joint.canActuate()) { joint.prepareActuation(); @@ -178,7 +178,7 @@ bool MarchHardwareInterface::init(ros::NodeHandle& nh, ros::NodeHandle& /* robot void MarchHardwareInterface::validate() { - const auto last_exception = this->march_robot_->getLastEthercatException(); + const auto last_exception = this->march_robot_->getLastCommunicationException(); if (last_exception) { std::rethrow_exception(last_exception); @@ -195,14 +195,14 @@ void MarchHardwareInterface::validate() } if (fault_state) { - this->march_robot_->stopEtherCAT(); + this->march_robot_->stopCommunication(); throw std::runtime_error("One or more motor controllers are in fault state"); } } -void MarchHardwareInterface::waitForPdo() +void MarchHardwareInterface::waitForUpdate() { - this->march_robot_->waitForPdo(); + this->march_robot_->waitForUpdate(); } void MarchHardwareInterface::read(const ros::Time& /* time */, const ros::Duration& elapsed_time) @@ -290,9 +290,9 @@ void MarchHardwareInterface::write(const ros::Time& /* time */, const ros::Durat } } -int MarchHardwareInterface::getEthercatCycleTime() const +int MarchHardwareInterface::getCycleTime() const { - return this->march_robot_->getEthercatCycleTime(); + return this->march_robot_->getCycleTime(); } void MarchHardwareInterface::uploadJointNames(ros::NodeHandle& nh) const diff --git a/march_hardware_interface/src/march_hardware_interface_node.cpp b/march_hardware_interface/src/march_hardware_interface_node.cpp index df99b6a7d..4f9d7279f 100644 --- a/march_hardware_interface/src/march_hardware_interface_node.cpp +++ b/march_hardware_interface/src/march_hardware_interface_node.cpp @@ -54,7 +54,7 @@ int main(int argc, char** argv) { try { - march.waitForPdo(); + march.waitForUpdate(); const ros::Time now = ros::Time::now(); ros::Duration elapsed_time = now - last_update_time;