diff --git a/.gitignore b/.gitignore index 11ac0bf..f2972a6 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,11 @@ **/build/ **/log/ **/install/ +rosbag2_*/ **/*~ -.vscode \ No newline at end of file +.vscode +.claude +CLAUDE.md +plans/ +SOMANET Circulo Safe Motion.pdf +docs/ \ No newline at end of file diff --git a/rise_motion_dev_ws/build.sh b/rise_motion_dev_ws/build.sh new file mode 100755 index 0000000..098f072 --- /dev/null +++ b/rise_motion_dev_ws/build.sh @@ -0,0 +1,3 @@ +colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +sudo setcap cap_net_admin,cap_net_raw+eip build/rise_motion/rise_motion_main +source install/setup.bash diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index aa620a5..693ddb1 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -7,10 +7,66 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) -# uncomment the following section in order to fill in -# further dependencies manually. -# find_package( REQUIRED) +find_package(rclcpp REQUIRED) +find_package(rise_motion_messages REQUIRED) +find_package(std_msgs REQUIRED) +include_directories(include) + +add_subdirectory(SOEM) + +add_executable( + rise_motion_main + src/main.cpp + src/ec_manager.cpp + src/ethercat_node.cpp + src/cia402.cpp +) +ament_target_dependencies( + rise_motion_main + rclcpp + rise_motion_messages +) +target_link_libraries( + rise_motion_main + soem +) + +#target_link_options(rise_motion_main PRIVATE -Wl,-rpath,/hello/home/rise/Projects/rise-motion/rise_motion_dev_ws/install/rise_motion_messages/lib) +install(TARGETS + rise_motion_main + DESTINATION lib/${PROJECT_NAME} +) + +add_executable(rise_motion_mock + src/main_mock.cpp + src/ec_manager_mock.cpp + src/ethercat_node.cpp +) +ament_target_dependencies( + rise_motion_mock + rclcpp + rise_motion_messages +) +install(TARGETS + rise_motion_mock + DESTINATION lib/${PROJECT_NAME} +) + + +add_executable(testing_node + src/testing_node.cpp +) + +target_link_libraries(testing_node PUBLIC + ${rise_motion_messages_TARGETS} + rclcpp::rclcpp +) + +install(TARGETS + testing_node + DESTINATION lib/${PROJECT_NAME} +) if(BUILD_TESTING) find_package(ament_lint_auto REQUIRED) # the following line skips the linter which checks for copyrights diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/apsa.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/apsa.hpp new file mode 100755 index 0000000..dd527a1 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/apsa.hpp @@ -0,0 +1,123 @@ +#pragma once + +#include +#include +#include + + +template +class APSA { +public: + APSA() + : c_mem(std::make_unique()), // Allocate communication buffer + a_mem(std::make_unique()), // Allocate atomic/shared buffer + p_mem(std::make_unique()), // Allocate performance buffer + c_p(nullptr), // Communication pointer starts null + a_p(nullptr), // Atomic pointer starts null + p_p(nullptr) // Performance pointer starts null + { + // No initialization needed - pointers start null as per APSA spec + } + + bool comm_write(const T& data) { + // Copy new data into our communication buffer + *c_mem = data; + + // Point c_p to the buffer we just wrote + c_p.store(c_mem.get(), std::memory_order_release); + + T* desired = c_p.load(std::memory_order_acquire); + + // Perform the atomic swap + T* old_a_p = a_p.exchange(desired, std::memory_order_acq_rel); + + // Update c_p to point to what a_p was pointing to + c_p.store(old_a_p, std::memory_order_release); + + return true; + } + + + bool perf_read(T& data) { + // Check if new data is available in atomic pointer + // memory_order_acquire ensures we see the write from comm_write + T* a_ptr = a_p.load(std::memory_order_acquire); + + if (a_ptr != nullptr) { + // New data is available! Swap p_p with a_p to claim it + T* old_p_p = p_p.exchange(a_p.load(std::memory_order_acquire), + std::memory_order_acq_rel); + + // Update a_p atomically + a_p.store(old_p_p, std::memory_order_release); + + // Copy the data from our performance buffer + // p_p now points to the buffer with fresh data + T* p_ptr = p_p.load(std::memory_order_acquire); + if (p_ptr != nullptr) { + data = *p_ptr; + + // This tells the communication thread we're done with this data + p_p.store(nullptr, std::memory_order_release); + + return true; // New data was read + } + } + + return false; // No new data available + } + + + bool perf_write(const T& data) { + // Copy data into our performance buffer + *p_mem = data; + + // Point p_p to the buffer we just wrote + p_p.store(p_mem.get(), std::memory_order_release); + + // Atomically swap p_p with a_p + T* old_a_p = a_p.exchange(p_p.load(std::memory_order_acquire), + std::memory_order_acq_rel); + + // Update p_p to point to what a_p was pointing to + p_p.store(old_a_p, std::memory_order_release); + + return true; + } + + + bool comm_read(T& data) { + // Check if new data is available in atomic pointer + T* a_ptr = a_p.load(std::memory_order_acquire); + + if (a_ptr != nullptr) { + // New data is available! Swap c_p with a_p to claim it + T* old_c_p = c_p.exchange(a_p.load(std::memory_order_acquire), + std::memory_order_acq_rel); + + // Update a_p atomically + a_p.store(old_c_p, std::memory_order_release); + + // Copy the data from our communication buffer + T* c_ptr = c_p.load(std::memory_order_acquire); + if (c_ptr != nullptr) { + data = *c_ptr; + + c_p.store(nullptr, std::memory_order_release); + + return true; // New data was read + } + } + + return false; // No new data available + } + +private: + std::unique_ptr c_mem; // Communication thread's buffer (ROS side) + std::unique_ptr a_mem; // Atomic/shared buffer (exchange point) + std::unique_ptr p_mem; // Performance thread's buffer (EtherCAT side) + + std::atomic c_p; // Communication pointer (ROS side access) + std::atomic a_p; // Atomic pointer (shared exchange point) + std::atomic p_p; // Performance pointer (EtherCAT side access) +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp new file mode 100755 index 0000000..514190f --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp @@ -0,0 +1,151 @@ +#pragma once +#include +#include +#include + +#include + +OSAL_PACKED_BEGIN +typedef struct OSAL_PACKED { + uint16_t Statusword; + int8_t OpModeDisplay; + int32_t PositionValue; + int32_t VelocityValue; + int16_t TorqueValue; + uint16_t AnalogInput1; + uint16_t AnalogInput2; + uint16_t AnalogInput3; + uint16_t AnalogInput4; + uint32_t TuningStatus; + uint32_t DigitalInputs; + uint32_t UserMISO; + uint32_t Timestamp; + int32_t PositionDemandInternalValue; + int32_t VelocityDemandValue; + int16_t TorqueDemand; +} CiA402_Inputs; +OSAL_PACKED_END + +OSAL_PACKED_BEGIN +typedef struct OSAL_PACKED { + uint16_t Controlword; + int8_t OpMode; + int16_t TargetTorque; + int32_t TargetPosition; + int32_t TargetVelocity; + int16_t TorqueOffset; + uint32_t TuningCommand; + uint32_t PhysicalOutputs; + uint32_t BitMask; + uint32_t UserMOSI; + int32_t VelocityOffset; +} CiA402_Outputs; +OSAL_PACKED_END + +class CiA402Motor { +public: + enum class State { + NOT_READY_TO_SWITCH_ON, + SWITCH_ON_DISABLED, + READY_TO_SWITCH_ON, + SWITCHED_ON, + OPERATION_ENABLED, + QUICK_STOP_ACTIVE, + FAULT_REACTION_ACTIVE, + FAULT, + }; + + enum class Operation { + SHUTDOWN, + SWITCH_ON, + DISABLE_VOLTAGE, + QUICK_STOP, + DISABLE_OPERATION, + ENABLE_OPERATION, + FAULT_RESET + }; + + // https://doc.synapticon.com/circulo/sw5.4/objects_html/6xxx/6060.html + enum class ModeOfOperation : int8_t { + ImpedanceMode = -6, + JointTorqueMode = -5, + SystemIdentificationMode = -4, + OpenLoopFieldMode = -3, + DiagnosticsMode = -2, + CoggingCompensationRecordingMode = -1, + ProfilePositionMode = 1, + ProfileVelocityMode = 3, + TorqueProfileMode = 4, + HomingMode = 6, + CyclicSyncPositionMode = 8, + CyclicSyncVelocityMode = 9, + CyclicSyncTorqueMode = 10 + }; + + CiA402Motor(CiA402_Inputs *inputs, CiA402_Outputs *outputs); + + /** + * @brief Gets the motor's current state based on its status word. + * + * Compares the status word against predefined patterns. Returns the matching + * state, or `std::nullopt` if no match is found. + * + * @return The motor's state if a match is found; otherwise, `std::nullopt`. + * + * @warning Returning `std::nullopt` indicates an unrecognized status word. + * Callers must handle this case to avoid undefined behavior. + * + * @see CiA402Motor::State, state_patterns + */ + std::optional get_state() const; + std::string state_as_string() const; + bool is_state(State s) const; + bool is_operation_enabled() const; + bool is_fault() const; + + void to_operation_enabled(); + void to_switch_on_disabled(); + void transition_to(State s); + void reset_fault(); + void set_control_word(Operation op); + void set_mode_of_operation(ModeOfOperation m); + + CiA402_Inputs *inputs; + CiA402_Outputs *outputs; + +private: + struct StatePattern { + uint16_t mask; + uint16_t value; + State state; + }; + + static constexpr StatePattern state_patterns[] = { + // https://doc.synapticon.com/circulo/system_integration/status_and_controlword.html + // mask value state + {0b1001111, 0b0000000, State::NOT_READY_TO_SWITCH_ON}, + {0b1001111, 0b1000000, State::SWITCH_ON_DISABLED}, + {0b1101111, 0b0100001, State::READY_TO_SWITCH_ON}, + {0b1101111, 0b0100011, State::SWITCHED_ON}, + {0b1101111, 0b0100111, State::OPERATION_ENABLED}, + {0b1101111, 0b0000111, State::QUICK_STOP_ACTIVE}, + {0b1001111, 0b0001111, State::FAULT_REACTION_ACTIVE}, + {0b1001111, 0b0001000, State::FAULT}}; + + struct ControlPattern { + uint16_t mask; + uint16_t value; + Operation op; + }; + + static constexpr ControlPattern control_patterns[] = { + // https://doc.synapticon.com/circulo/system_integration/status_and_controlword.html + // mask value operation + {0b10000111, 0b00000110, Operation::SHUTDOWN}, + {0b10001111, 0b00000111, Operation::SWITCH_ON}, + {0b10000010, 0b00000000, Operation::DISABLE_VOLTAGE}, + {0b10000110, 0b00000010, Operation::QUICK_STOP}, + {0b10001111, 0b00000111, Operation::DISABLE_OPERATION}, + {0b10001111, 0b00001111, Operation::ENABLE_OPERATION}, + {0b10000000, 0b10000000, Operation::FAULT_RESET}}; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp new file mode 100755 index 0000000..35e3fc8 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp @@ -0,0 +1,78 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#include +#include +#include + +#define IOMAP_SIZE 4096 + +class ECManager : public IECManager { +public: + ECManager(); + ECManager(const std::string interface, int cycle_period_ms); + + int init_ec() override; + void cyclic_loop() override; + bool is_running() override; + void stop() override; + + // APSA-based motor value transfer (lock-free) + bool get_motor_values_apsa(std::vector& motor_values) override; + bool set_motor_values_apsa(const std::vector& motor_values) override; + bool get_full_feedback_apsa(std::vector& feedback) override; + bool set_motor_velocity_apsa(const std::vector& velocities) override; + bool set_torque_offset_apsa(const std::vector& offsets) override; + bool set_motor_torque_apsa(const std::vector& torques) override; + void set_operation_mode(int8_t mode) override; + + // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread + bool sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; + bool sdo_write(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; + +private: + uint16 transition_ec(uint16 state); + bool transition_motors_to(CiA402Motor::State state); + void shutdown(); + + // EtherCAT context and configuration + int expectedWKC; + ecx_contextt ctx; + uint8_t IOMap[IOMAP_SIZE]; + std::vector motors; + + std::atomic running_{false}; + const std::string interface; + const rclcpp::Logger logger; + + std::chrono::time_point next; + const std::chrono::duration> period; // period in ms + + // APSA instances for lock-free communication + // cmd_apsa: ROS → EtherCAT (position commands, Mode 8) + APSA> cmd_apsa; + + // vel_cmd_apsa: ROS → EtherCAT (velocity commands, Mode 9) + APSA> vel_cmd_apsa; + + // torque_offset_apsa: ROS → EtherCAT (0x60B2 torque offset, ‰ rated torque) + APSA> torque_offset_apsa; + + // torque_cmd_apsa: ROS → EtherCAT (TargetTorque, Mode 10, ‰ rated torque) + APSA> torque_cmd_apsa; + + // feedback_apsa: EtherCAT → ROS (motor positions only, für /motor_feedback) + APSA> feedback_apsa; + + // full_feedback_apsa: EtherCAT → ROS (alle PDO-Felder, für /motor_feedback_full) + APSA> full_feedback_apsa; + + // target_mode: 8 = CyclicSyncPositionMode (default), 9 = CyclicSyncVelocityMode, 10 = CyclicSyncTorqueMode + // Atomic: written by ROS thread (set_operation_mode), read by EtherCAT thread (cyclic_loop) + std::atomic target_mode_{8}; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager_mock.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager_mock.hpp new file mode 100644 index 0000000..59c625e --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager_mock.hpp @@ -0,0 +1,53 @@ +#pragma once +#include +#include +#include +#include +#include + +#include +#include + +class MockECManager : public IECManager { +public: + explicit MockECManager(int cycle_period_ms = 1, int num_motors = 1, + float alpha = 1.0f); + + int init_ec() override; + void cyclic_loop() override; + bool is_running() override; + void stop() override; + + bool get_motor_values_apsa(std::vector& motor_values) override; + bool set_motor_values_apsa(const std::vector& motor_values) override; + bool get_full_feedback_apsa(std::vector& feedback) override; + bool set_motor_velocity_apsa(const std::vector& velocities) override; + bool set_torque_offset_apsa(const std::vector& offsets) override; + bool set_motor_torque_apsa(const std::vector& torques) override; + void set_operation_mode(int8_t mode) override; + + bool sdo_read(uint16_t device_id, uint16_t index, + uint8_t subindex, std::vector& value) override; + bool sdo_write(uint16_t device_id, uint16_t index, + uint8_t subindex, std::vector& value) override; + +private: + const int num_motors_; + const float alpha_; + std::atomic running_{false}; + const rclcpp::Logger logger_; + + std::vector mock_positions_; + std::vector mock_positions_float_; + std::vector motor_commands_; + std::vector velocity_commands_; + uint64_t tick_count_{0}; + + std::chrono::time_point next_; + const std::chrono::duration> period_; + + APSA> cmd_apsa_; + APSA> vel_cmd_apsa_; + APSA> feedback_apsa_; + APSA> full_feedback_apsa_; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ethercat_node.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ethercat_node.hpp new file mode 100755 index 0000000..03c0771 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ethercat_node.hpp @@ -0,0 +1,61 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include +#include + +class EthercatNode : public rclcpp::Node { +public: + explicit EthercatNode(IECManager& ec_manager); + ~EthercatNode(); + +private: + IECManager& ec_manager_; + std::unique_ptr ec_thread_; + bool ethercat_enabled_{false}; + + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Subscription::SharedPtr torque_offset_sub_; + rclcpp::Subscription::SharedPtr torque_cmd_sub_; + rclcpp::Publisher::SharedPtr feedback_pub_; + rclcpp::TimerBase::SharedPtr feedback_timer_; + rclcpp::Publisher::SharedPtr full_feedback_pub_; + rclcpp::TimerBase::SharedPtr full_feedback_timer_; + rclcpp::Service::SharedPtr enable_srv_; + rclcpp::Service::SharedPtr sdo_read_srv_; + rclcpp::Service::SharedPtr sdo_write_srv_; + rclcpp::Service::SharedPtr mode_srv_; + + + void commandCallback(const rise_motion_messages::msg::MotorPositions::SharedPtr msg); + void velocityCommandCallback(rise_motion_messages::msg::MotorVelocity::SharedPtr msg); + void torqueOffsetCallback(rise_motion_messages::msg::MotorTorqueOffset::SharedPtr msg); + void torqueCommandCallback(rise_motion_messages::msg::MotorTorqueOffset::SharedPtr msg); + void publishFeedback(); + void publishFullFeedback(); + void enableServiceCallback( + const std::shared_ptr request, + std::shared_ptr response); + void sdoReadServiceCallback( + const std::shared_ptr + request, + std::shared_ptr + response); + void sdoWriteServiceCallback( + const std::shared_ptr + request, + std::shared_ptr + response); + void setOperationModeCallback( + std::shared_ptr request, + std::shared_ptr response); +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/iec_manager.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/iec_manager.hpp new file mode 100644 index 0000000..dd38720 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/iec_manager.hpp @@ -0,0 +1,29 @@ +#pragma once +#include +#include + +#include + +class IECManager { +public: + virtual ~IECManager() = default; + + virtual int init_ec() = 0; + virtual void cyclic_loop() = 0; + virtual bool is_running() = 0; + virtual void stop() = 0; + + virtual bool get_motor_values_apsa(std::vector& motor_values) = 0; + virtual bool set_motor_values_apsa(const std::vector& motor_values) = 0; + virtual bool get_full_feedback_apsa(std::vector& feedback) = 0; + + virtual bool set_motor_velocity_apsa(const std::vector& velocities) = 0; + virtual bool set_torque_offset_apsa(const std::vector& offsets) = 0; + virtual bool set_motor_torque_apsa(const std::vector& torques) = 0; + virtual void set_operation_mode(int8_t mode) = 0; + + virtual bool sdo_read(uint16_t device_id, uint16_t index, + uint8_t subindex, std::vector& value) = 0; + virtual bool sdo_write(uint16_t device_id, uint16_t index, + uint8_t subindex, std::vector& value) = 0; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/motor_feedback_data.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/motor_feedback_data.hpp new file mode 100644 index 0000000..74f27d5 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/motor_feedback_data.hpp @@ -0,0 +1,21 @@ +#pragma once +#include + +struct MotorFeedbackData { + uint16_t statusword; + int8_t op_mode_display; + int32_t position; + int32_t velocity_value; + int16_t torque_value; + uint16_t analog_input1; // Kraftsensor Kanal 1 (ADC-Ticks, 0=Umin, 65535=Umax) + uint16_t analog_input2; + uint16_t analog_input3; + uint16_t analog_input4; + uint32_t tuning_status; + uint32_t digital_inputs; + uint32_t user_miso; + uint32_t timestamp; + int32_t position_demand_internal_value; + int32_t velocity_demand_value; + int16_t torque_demand; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/package.xml b/rise_motion_dev_ws/src/rise_motion/package.xml index da1008f..9c5ac6c 100644 --- a/rise_motion_dev_ws/src/rise_motion/package.xml +++ b/rise_motion_dev_ws/src/rise_motion/package.xml @@ -9,6 +9,9 @@ ament_cmake + rclcpp + rise_motion_messages + ament_lint_auto ament_lint_common diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp new file mode 100755 index 0000000..cbf79a1 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -0,0 +1,152 @@ +#include +#include +#include + +CiA402Motor::CiA402Motor(CiA402_Inputs *inputs, CiA402_Outputs *outputs) + : inputs(inputs), outputs(outputs) {} + +std::optional CiA402Motor::get_state() const { + const uint16_t sw = inputs->Statusword; + for (const auto &p : state_patterns) { + if ((sw & p.mask) == p.value) + return p.state; + } + return std::nullopt; +} + +bool CiA402Motor::is_state(State s) const { + std::optional actual_state = get_state(); + if (actual_state.has_value() && actual_state.value() == s) { + return true; + } else { + return false; + } +} + +void CiA402Motor::transition_to(State s) { + switch (s) { + case State::SWITCH_ON_DISABLED: + to_switch_on_disabled(); + break; + case State::OPERATION_ENABLED: + to_operation_enabled(); + break; + case State::NOT_READY_TO_SWITCH_ON: + case State::READY_TO_SWITCH_ON: + case State::SWITCHED_ON: + case State::QUICK_STOP_ACTIVE: + case State::FAULT_REACTION_ACTIVE: + case State::FAULT: + break; + } +} + +bool CiA402Motor::is_operation_enabled() const { + return is_state(State::OPERATION_ENABLED); +} + +void CiA402Motor::to_operation_enabled() { + std::optional s = get_state(); + + if (!s.has_value()) { + return; + } + + std::optional next_op = std::nullopt; + switch (s.value()) { + case State::SWITCH_ON_DISABLED: + next_op = Operation::SHUTDOWN; + break; + case State::READY_TO_SWITCH_ON: + next_op = Operation::SWITCH_ON; + break; + case State::SWITCHED_ON: + next_op = Operation::ENABLE_OPERATION; + break; + case State::FAULT: + case State::OPERATION_ENABLED: + case State::QUICK_STOP_ACTIVE: + case State::FAULT_REACTION_ACTIVE: + default: + return; + } + + if (next_op.has_value()) { + set_control_word(next_op.value()); + } +} + +void CiA402Motor::to_switch_on_disabled() { + std::optional s = get_state(); + + if (!s.has_value()) { + return; + } + + std::optional next_op = std::nullopt; + switch (s.value()) { + case State::OPERATION_ENABLED: + next_op = Operation::DISABLE_OPERATION; + break; + case State::SWITCHED_ON: + case State::QUICK_STOP_ACTIVE: + next_op = Operation::SHUTDOWN; + break; + case State::READY_TO_SWITCH_ON: + next_op = Operation::DISABLE_VOLTAGE; + break; + case State::FAULT: + case State::FAULT_REACTION_ACTIVE: + case State::SWITCH_ON_DISABLED: + default: + return; + } + + if (next_op.has_value()) { + set_control_word(next_op.value()); + } +} + +bool CiA402Motor::is_fault() const { return is_state(State::FAULT); } + +void CiA402Motor::reset_fault() { set_control_word(Operation::FAULT_RESET); } + +void CiA402Motor::set_control_word(Operation op) { + for (auto &p : control_patterns) { + if (p.op == op) { + outputs->Controlword = (outputs->Controlword & ~p.mask) | p.value; + break; + } + } +} + +void CiA402Motor::set_mode_of_operation(CiA402Motor::ModeOfOperation m) { + outputs->OpMode = static_cast(m); +} + +std::string CiA402Motor::state_as_string() const { + std::optional s = get_state(); + if (s == std::nullopt) { + return "UNKNOWN"; + } + + switch (s.value()) { + case State::NOT_READY_TO_SWITCH_ON: + return "NOT_READY_TO_SWITCH_ON"; + case State::SWITCH_ON_DISABLED: + return "SWITCH_ON_DISABLED"; + case State::READY_TO_SWITCH_ON: + return "READY_TO_SWITCH_ON"; + case State::SWITCHED_ON: + return "SWITCHED_ON"; + case State::OPERATION_ENABLED: + return "OPERATION_ENABLED"; + case State::QUICK_STOP_ACTIVE: + return "QUICK_STOP_ACTIVE"; + case State::FAULT_REACTION_ACTIVE: + return "FAULT_REACTION_ACTIVE"; + case State::FAULT: + return "FAULT"; + } + return "UNKNOWN"; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp new file mode 100755 index 0000000..a925204 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -0,0 +1,451 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +#include +#include +#include +#include +#include + +// expected config, needs to be retrieved from config node +struct { + int slavecount = 1; +} config; + +ECManager::ECManager(const std::string interface, int cycle_time) + : interface(interface), logger(rclcpp::get_logger("ECManager")), + next(std::chrono::steady_clock::now()), + period(std::chrono::milliseconds(cycle_time)) {} + +int ECManager::init_ec() { + int ret; + + memset(&ctx, 0, sizeof(ctx)); + memset(IOMap, 0, sizeof(IOMap)); + + RCLCPP_INFO(logger, "Connecting to %s", interface.c_str()); + ret = ecx_init(&ctx, interface.c_str()); + if (ret <= 0) { + RCLCPP_ERROR(logger, "Couldn't initialize SOEM context"); + return EXIT_FAILURE; + } + + RCLCPP_INFO(logger, "Discovering EC Nodes"); + ret = ecx_config_init(&ctx); // also requests PreOP state + if (ret <= 0) { + RCLCPP_ERROR(logger, "EC Nodes Discovery failed"); + return EXIT_FAILURE; + } + + // All nodes should be in EC_STATE_PRE_OP according to tutorial + if (ecx_statecheck(&ctx, 0, EC_STATE_PRE_OP, EC_TIMEOUTSTATE * 4) != + EC_STATE_PRE_OP) { + RCLCPP_ERROR(logger, "Not all nodes in EC_STATE_PRE_OP"); + return EXIT_FAILURE; + } + + // TODO: More extensive verification of network + if (ctx.slavecount != config.slavecount) { + RCLCPP_ERROR(logger, "Expected %d devices, but discovered %d", + config.slavecount, ctx.slavecount); + return EXIT_FAILURE; + } + + RCLCPP_INFO(logger, "Mapping IO"); + ret = ecx_config_map_group(&ctx, IOMap, 0); // also requests SafeOP state + if (ret > IOMAP_SIZE) { + RCLCPP_ERROR(logger, "Couldn't map IO: Buffer to small"); + return EXIT_FAILURE; + } + + // All nodes should be in EC_STATE_SAFE_OP according to tutorial + if (ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4) != + EC_STATE_SAFE_OP) { + RCLCPP_ERROR(logger, "Not all nodes in EC_STATE_SAFE_OP"); + return EXIT_FAILURE; + } + + RCLCPP_INFO(logger, "Using %d of %lu bytes in IOMap", ret, sizeof(IOMap)); + + // Enable mailboxes for SDO + for (int i = 1; i <= ctx.slavecount; i++) { + if (ctx.slavelist[i].CoEdetails > 0) { + ecx_slavembxcyclic(&ctx, i); + RCLCPP_INFO(logger, "Enabled mailbox for drive %d", i); + } + } + + expectedWKC = ctx.grouplist[0].outputsWKC * 2 + ctx.grouplist[0].inputsWKC; + + RCLCPP_INFO(logger, "Configuring distributed clock"); + ecx_configdc(&ctx); + + // Create motor classes + motors.clear(); + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Outputs *motor_outputs = (CiA402_Outputs *)ctx.slavelist[i].outputs; + CiA402_Inputs *motor_inputs = (CiA402_Inputs *)ctx.slavelist[i].inputs; + CiA402Motor m{motor_inputs, motor_outputs}; + motors.push_back(m); + } + return EXIT_SUCCESS; +} + +void ECManager::cyclic_loop() { + // Still in SAFE_OP, PDO transmission is available + next = std::chrono::steady_clock::now(); + running_ = true; + + std::vector motor_commands(ctx.slavecount, 0); + std::vector motor_velocities(ctx.slavecount, 0); + std::vector torque_offsets(ctx.slavecount, 0); + std::vector motor_torques(ctx.slavecount, 0); + std::vector motor_feedback(ctx.slavecount, 0); + std::vector full_feedback(ctx.slavecount); + int wkc_error_count = 0; + static constexpr int kMaxWkcErrors = 5; + + // Transition to OPERATIONAL + // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED + uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); + if (reached_state != EC_STATE_OPERATIONAL) { + shutdown(); + return; + } + + // Configuring Drives + for (size_t i = 0; i < motors.size(); i++) { + CiA402Motor &m = motors[i]; + // Setting ModeOfOperation to CyclicSyncPositionMode + m.set_mode_of_operation( + CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + + // Set Position to Current Position + motor_commands[i] = m.inputs->PositionValue; + m.outputs->TargetPosition = m.inputs->PositionValue; + RCLCPP_INFO(logger, "Configured Motor %zu: Init Position(%d)", i + 1, + m.inputs->PositionValue); + } + + // Transitioning CiA402 State Machine to OPERATION_ENABLED + if (!transition_motors_to(CiA402Motor::State::OPERATION_ENABLED)) { + RCLCPP_ERROR(logger, "Couldn't transition all motors to OPERATION_ENABLED"); + shutdown(); + return; + } + + RCLCPP_INFO(logger, "All motors in operation_enabled"); + + RCLCPP_INFO(logger, "Entering Cyclic Loop"); + next = std::chrono::steady_clock::now(); + while (running_) { + int wkc; + next += period; + + ecx_send_processdata(&ctx); + wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + ecx_mbxhandler(&ctx, 0, 4); + + if (wkc != expectedWKC) { + wkc_error_count++; + RCLCPP_WARN(logger, "WKC mismatch (%d/%d): got %d, expected %d", + wkc_error_count, kMaxWkcErrors, wkc, expectedWKC); + if (wkc_error_count >= kMaxWkcErrors) { + RCLCPP_ERROR(logger, "Not all nodes responded"); + shutdown(); + return; + } + } else { + wkc_error_count = 0; + } + + // Iterate over connected drives + for (size_t i = 0; i < motors.size(); i++) { + CiA402Motor &m = motors[i]; + + if (!m.get_state().has_value()) { + RCLCPP_ERROR(logger, "Motor %zu has no state", i + 1); + shutdown(); + return; + } else if (m.get_state().value() != + CiA402Motor::State::OPERATION_ENABLED) { + RCLCPP_ERROR(logger, "Motor %zu is not in OPERATION_ENABLED", i + 1); + shutdown(); + return; + } + + // CiA402 Mode-Switching: + // 1. Write target mode every cycle; drive confirms via OpModeDisplay (1-10ms delay) + // 2. During transition (OpModeDisplay != target): hold position, zero velocity (safe) + // 3. Once confirmed: send commands for the active mode + cmd_apsa.perf_read(motor_commands); + vel_cmd_apsa.perf_read(motor_velocities); + torque_offset_apsa.perf_read(torque_offsets); + torque_cmd_apsa.perf_read(motor_torques); + const int8_t target = target_mode_.load(std::memory_order_relaxed); + m.outputs->OpMode = target; + const bool mode_confirmed = (m.inputs->OpModeDisplay == target); + if (!mode_confirmed) { + m.outputs->TargetVelocity = 0; + m.outputs->TargetTorque = 0; + m.outputs->TargetPosition = m.inputs->PositionValue; + } else if (target == 10) { // CyclicSyncTorqueMode + m.outputs->TargetTorque = motor_torques[i]; + } else if (target == 9) { // CyclicSyncVelocityMode + m.outputs->TargetVelocity = motor_velocities[i]; + } else { // CyclicSyncPositionMode (default) + m.outputs->TargetPosition = motor_commands[i]; + } + m.outputs->TorqueOffset = torque_offsets[i]; + motor_feedback[i] = m.inputs->PositionValue; + + full_feedback[i].statusword = m.inputs->Statusword; + full_feedback[i].op_mode_display = m.inputs->OpModeDisplay; + full_feedback[i].position = m.inputs->PositionValue; + full_feedback[i].velocity_value = m.inputs->VelocityValue; + full_feedback[i].torque_value = m.inputs->TorqueValue; + full_feedback[i].analog_input1 = m.inputs->AnalogInput1; + full_feedback[i].analog_input2 = m.inputs->AnalogInput2; + full_feedback[i].analog_input3 = m.inputs->AnalogInput3; + full_feedback[i].analog_input4 = m.inputs->AnalogInput4; + full_feedback[i].tuning_status = m.inputs->TuningStatus; + full_feedback[i].digital_inputs = m.inputs->DigitalInputs; + full_feedback[i].user_miso = m.inputs->UserMISO; + full_feedback[i].timestamp = m.inputs->Timestamp; + full_feedback[i].position_demand_internal_value = m.inputs->PositionDemandInternalValue; + full_feedback[i].velocity_demand_value = m.inputs->VelocityDemandValue; + full_feedback[i].torque_demand = m.inputs->TorqueDemand; + + RCLCPP_DEBUG(logger, + "Motor Outputs:\n" + "\tControlword: 0x%04X\n" + "\tOpMode: %d\n" + "\tTargetTorque: %d\n" + "\tTargetPosition: %d\n" + "\tTargetVelocity: %d\n" + "\tTorqueOffset: %d\n" + "\tTuningCommand: %d\n" + "\tPhysicalOutputs: %d\n" + "\tBitMask: 0x%08X\n" + "\tUserMOSI: 0x%08X\n" + "\tVelocityOffset: %d\n", + m.outputs->Controlword, m.outputs->OpMode, + m.outputs->TargetTorque, m.outputs->TargetPosition, + m.outputs->TargetVelocity, m.outputs->TorqueOffset, + m.outputs->TuningCommand, m.outputs->PhysicalOutputs, + m.outputs->BitMask, m.outputs->UserMOSI, + m.outputs->VelocityOffset); + RCLCPP_DEBUG(logger, + "Motor Inputs:\n" + "\tStatusword: 0x%04X\n" + "\tOpModeDisplay: %d\n" + "\tPositionValue: %d\n" + "\tVelocityValue: %d\n" + "\tTorqueValue: %d\n" + "\tAnalogInput1: %u\n" + "\tAnalogInput2: %u\n" + "\tAnalogInput3: %u\n" + "\tAnalogInput4: %u\n" + "\tTuningStatus: 0x%08X\n" + "\tDigitalInputs: 0x%08X\n" + "\tUserMISO: 0x%08X\n" + "\tTimestamp: %u\n" + "\tPositionDemandInternalValue: %d\n" + "\tVelocityDemandValue: %d\n" + "\tTorqueDemand: %d\n", + m.inputs->Statusword, m.inputs->OpModeDisplay, + m.inputs->PositionValue, m.inputs->VelocityValue, + m.inputs->TorqueValue, m.inputs->AnalogInput1, + m.inputs->AnalogInput2, m.inputs->AnalogInput3, + m.inputs->AnalogInput4, m.inputs->TuningStatus, + m.inputs->DigitalInputs, m.inputs->UserMISO, + m.inputs->Timestamp, m.inputs->PositionDemandInternalValue, + m.inputs->VelocityDemandValue, m.inputs->TorqueDemand); + } + + // Make feedback available to ROS publisher (wait-free) + feedback_apsa.perf_write(motor_feedback); + full_feedback_apsa.perf_write(full_feedback); + + // Sleep until next cycle (maintains 1kHz frequency) + std::this_thread::sleep_until(next); + } + + shutdown(); + return; +} + +void ECManager::shutdown() { + RCLCPP_INFO(logger, "Shutting down"); + if (!transition_motors_to(CiA402Motor::State::SWITCH_ON_DISABLED)) { + RCLCPP_ERROR(logger, + "Couldn't transition all motors to SWITCH_ON_DISABLED"); + } + + transition_ec(EC_STATE_PRE_OP); + transition_ec(EC_STATE_SAFE_OP); + transition_ec(EC_STATE_INIT); + + ecx_close(&ctx); + running_ = false; +} + +void ECManager::stop() { running_ = false; } + +bool ECManager::is_running() { return running_; } + +bool ECManager::get_motor_values_apsa(std::vector &motor_values) { + // comm_read() returns true if new data is available, false otherwise + return feedback_apsa.comm_read(motor_values); +} + +bool ECManager::set_motor_values_apsa( + const std::vector &motor_values) { + // comm_write() queues the data for the EtherCAT loop to pick up + return cmd_apsa.comm_write(motor_values); +} + +bool ECManager::get_full_feedback_apsa(std::vector &feedback) { + return full_feedback_apsa.comm_read(feedback); +} + +bool ECManager::set_motor_velocity_apsa(const std::vector& velocities) { + return vel_cmd_apsa.comm_write(velocities); +} + +bool ECManager::set_torque_offset_apsa(const std::vector& offsets) { + return torque_offset_apsa.comm_write(offsets); +} + +bool ECManager::set_motor_torque_apsa(const std::vector& torques) { + return torque_cmd_apsa.comm_write(torques); +} + +void ECManager::set_operation_mode(int8_t mode) { + target_mode_.store(mode, std::memory_order_relaxed); + RCLCPP_INFO(logger, "Operation mode target set to %d", mode); +} + +uint16 ECManager::transition_ec(uint16 state) { + // Get state_string + std::string state_string = "Unknown"; + { + switch (state) { + case EC_STATE_INIT: + state_string = "EC_STATE_INIT"; + break; + case EC_STATE_PRE_OP: + state_string = "EC_STATE_PRE_OP"; + break; + case EC_STATE_SAFE_OP: + state_string = "EC_STATE_SAFE_OP"; + break; + case EC_STATE_OPERATIONAL: + state_string = "EC_STATE_OPERATIONAL"; + break; + } + } + + RCLCPP_INFO(logger, "Transition Ethercat State to %s", state_string.c_str()); + + ctx.slavelist[0].state = state; + ecx_writestate(&ctx, 0); + int chk = 200; + uint16 reached_state; + next = std::chrono::steady_clock::now(); + do { + next += period; + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + reached_state = ecx_statecheck(&ctx, 0, state, EC_TIMEOUTSTATE * 4); + std::this_thread::sleep_until(next); + } while (chk-- && (ctx.slavelist[0].state != state)); + if (reached_state != state) { + RCLCPP_WARN(logger, "Couldn't transition to %s", state_string.c_str()); + ecx_readstate(&ctx); + for (int i = 1; i <= ctx.slavecount; i++) { + if (ctx.slavelist[i].state != state) { + RCLCPP_WARN(logger, "Node %d State=%2x StatusCode=%4x : %s", i, + ctx.slavelist[i].state, ctx.slavelist[i].ALstatuscode, + ec_ALstatuscode2string(ctx.slavelist[i].ALstatuscode)); + } + } + } + return reached_state; +} + +bool ECManager::sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, + std::vector &value) { + int psize = 64; + uint8_t *buf = new uint8_t[psize]; + + boolean CA = FALSE; + int wkc = ecx_SDOread(&ctx, device_id, index, subindex, CA, &psize, + (void *)buf, EC_TIMEOUTRXM); + + value.clear(); + for (int i = 0; i < psize; i++) { + value.push_back(buf[i]); + } + RCLCPP_INFO(logger, "%d:%d", wkc, expectedWKC); + // return (wkc == expectedWKC); + return true; +} + +bool ECManager::sdo_write(uint16_t device_id, uint16_t index, uint8_t subindex, + std::vector &value) { + int psize = value.size(); + uint8_t *buf = new uint8_t[psize]; + + boolean CA = FALSE; + int wkc = ecx_SDOwrite(&ctx, device_id, index, subindex, CA, psize, + (void *)buf, EC_TIMEOUTRXM); + RCLCPP_INFO(logger, "%d:%d", wkc, expectedWKC); + // return (wkc == expectedWKC); + return true; +} + +bool ECManager::transition_motors_to(CiA402Motor::State state) { + int tries_left = 1000; + int continue_flag = 1; + next = std::chrono::steady_clock::now(); + while (continue_flag && tries_left > 0) { + next += period; + tries_left--; + continue_flag = 0; + for (size_t i = 0; i < motors.size(); i++) { + CiA402Motor &m = motors[i]; + RCLCPP_DEBUG(logger, "State of Motor %zu: %s", i + 1, + m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + continue_flag = 1; + RCLCPP_WARN(logger, "Motor %zu has no decodable state: 0x%04X", i + 1, + m.inputs->Statusword); + } else if (m.get_state().value() == CiA402Motor::State::FAULT) { + RCLCPP_ERROR(logger, "Motor %zu in fault", i + 1); + return false; + } else if (m.get_state().value() == CiA402Motor::State::QUICK_STOP_ACTIVE) { + RCLCPP_WARN(logger, "Motor %zu in quick stop, transitioning to SWITCH_ON_DISABLED", i + 1); + m.transition_to(CiA402Motor::State::SWITCH_ON_DISABLED); + continue_flag = 1; + } else if (m.get_state().value() != state) { + m.transition_to(state); + continue_flag = 1; + } + } + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + std::this_thread::sleep_until(next); + } + if (continue_flag) { + RCLCPP_ERROR(logger, "Couldn't transition motors in time"); + return false; + } + return true; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp new file mode 100644 index 0000000..1546504 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp @@ -0,0 +1,103 @@ +#include +#include +#include +#include + +#include + +MockECManager::MockECManager(int cycle_period_ms, int num_motors, float alpha) + : num_motors_(num_motors), + alpha_(alpha), + logger_(rclcpp::get_logger("MockECManager")), + mock_positions_(num_motors, 0), + mock_positions_float_(num_motors, 0.0), + motor_commands_(num_motors, 0), + velocity_commands_(num_motors, 0), + period_(cycle_period_ms) {} + +int MockECManager::init_ec() { + RCLCPP_INFO(logger_, "Mock ECManager: %d motor(s) ready", num_motors_); + return EXIT_SUCCESS; +} + +void MockECManager::cyclic_loop() { + running_ = true; + next_ = std::chrono::steady_clock::now(); + + RCLCPP_INFO(logger_, "Mock ECManager: entering cyclic loop at %d ms period", + (int)period_.count()); + + while (running_) { + next_ += period_; + + // Pull latest commands from ROS thread (wait-free) + cmd_apsa_.perf_read(motor_commands_); + vel_cmd_apsa_.perf_read(velocity_commands_); + + // Integrate velocity commands into position with floating-point precision + const double dt = period_.count() * 1e-3; + for (int i = 0; i < num_motors_; ++i) { + mock_positions_float_[i] += static_cast(velocity_commands_[i]) * dt; + mock_positions_[i] = static_cast(std::round(mock_positions_float_[i])); + } + + // Publish feedback to ROS thread (wait-free) + feedback_apsa_.perf_write(mock_positions_); + + std::vector full_feedback(num_motors_); + for (int i = 0; i < num_motors_; ++i) { + full_feedback[i].position = mock_positions_[i]; + } + full_feedback_apsa_.perf_write(full_feedback); + + tick_count_++; + std::this_thread::sleep_until(next_); + } +} + +bool MockECManager::is_running() { + return running_; +} + +void MockECManager::stop() { + running_ = false; +} + +bool MockECManager::get_motor_values_apsa(std::vector& motor_values) { + return feedback_apsa_.comm_read(motor_values); +} + +bool MockECManager::set_motor_values_apsa(const std::vector& motor_values) { + return cmd_apsa_.comm_write(motor_values); +} + +bool MockECManager::get_full_feedback_apsa(std::vector& feedback) { + return full_feedback_apsa_.comm_read(feedback); +} + +bool MockECManager::set_motor_velocity_apsa(const std::vector& velocities) { + return vel_cmd_apsa_.comm_write(velocities); +} + +bool MockECManager::set_torque_offset_apsa(const std::vector& /*offsets*/) { + return true; +} + +bool MockECManager::set_motor_torque_apsa(const std::vector& /*torques*/) { + return true; +} + +void MockECManager::set_operation_mode(int8_t mode) { + RCLCPP_INFO(logger_, "Mock: operation mode set to %d", mode); +} + +bool MockECManager::sdo_read(uint16_t /*device_id*/, uint16_t /*index*/, + uint8_t /*subindex*/, std::vector& value) { + value = {0}; + return true; +} + +bool MockECManager::sdo_write(uint16_t /*device_id*/, uint16_t /*index*/, + uint8_t /*subindex*/, std::vector& /*value*/) { + return true; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp new file mode 100755 index 0000000..e9833a2 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -0,0 +1,258 @@ +#include +#include +#include +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +EthercatNode::EthercatNode(IECManager &ec_manager) + : Node("ethercat_node"), ec_manager_(ec_manager) { + + cmd_sub_ = create_subscription( + "motor_commands", 10, + std::bind(&EthercatNode::commandCallback, this, _1)); + + cmd_vel_sub_ = create_subscription( + "motor_commands_vel", 10, + std::bind(&EthercatNode::velocityCommandCallback, this, _1)); + + torque_offset_sub_ = create_subscription( + "motor_torque_offset", 10, + std::bind(&EthercatNode::torqueOffsetCallback, this, _1)); + + torque_cmd_sub_ = create_subscription( + "motor_commands_torque", 10, + std::bind(&EthercatNode::torqueCommandCallback, this, _1)); + + feedback_pub_ = create_publisher( + "motor_feedback", 10); + + feedback_timer_ = + create_wall_timer(std::chrono::milliseconds(10), + std::bind(&EthercatNode::publishFeedback, this)); + + full_feedback_pub_ = + create_publisher( + "motor_feedback_full", 10); + + full_feedback_timer_ = + create_wall_timer(std::chrono::milliseconds(10), + std::bind(&EthercatNode::publishFullFeedback, this)); + + enable_srv_ = create_service( + "enable_ethercat", + std::bind(&EthercatNode::enableServiceCallback, this, _1, _2)); + + mode_srv_ = create_service( + "set_operation_mode", + std::bind(&EthercatNode::setOperationModeCallback, this, _1, _2)); + + sdo_read_srv_ = create_service( + "sdo_read", + std::bind(&EthercatNode::sdoReadServiceCallback, this, _1, _2)); + + sdo_write_srv_ = create_service( + "sdo_write", + std::bind(&EthercatNode::sdoWriteServiceCallback, this, _1, _2)); + + RCLCPP_INFO(get_logger(), "EtherCAT node initialized"); +} + +EthercatNode::~EthercatNode() { + if (ethercat_enabled_) { + ec_manager_.stop(); + if (ec_thread_ && ec_thread_->joinable()) { + ec_thread_->join(); + } + } +} + +/** + * @brief ROS subscriber callback for motor commands + * + * Communication Thread (ROS side) - SEND PATH: ROS → EtherCAT + * + * Uses APSA's comm_write() for lock-free transfer to the 1kHz EtherCAT loop. + * Never blocks the EtherCAT loop. + */ +void EthercatNode::commandCallback( + const rise_motion_messages::msg::MotorPositions::SharedPtr msg) { + std::vector positions(msg->positions.begin(), msg->positions.end()); + + if (!ec_manager_.set_motor_values_apsa(positions)) { + RCLCPP_WARN(get_logger(), "Failed to queue motor commands"); + } +} + +/** + * @brief ROS publisher timer callback for motor feedback + * + * Communication Thread (ROS side) - RECEIVE PATH: EtherCAT → ROS + * + * Uses APSA's comm_read() for lock-free transfer from the 1kHz EtherCAT loop. + * Only publishes when NEW feedback is available. + */ +void EthercatNode::publishFeedback() { + std::vector positions; + + if (ethercat_enabled_ && ec_manager_.get_motor_values_apsa(positions)) { + auto msg = rise_motion_messages::msg::MotorPositions(); + msg.positions.assign(positions.begin(), positions.end()); + feedback_pub_->publish(msg); + } +} + +void EthercatNode::enableServiceCallback( + const std::shared_ptr + request, + std::shared_ptr + response) { + if (request->enable && !ethercat_enabled_) { + RCLCPP_INFO(get_logger(), "Enabling EtherCAT communication"); + if (ec_manager_.init_ec() == EXIT_FAILURE) { + RCLCPP_ERROR(get_logger(), "Couldn't init ethercat"); + } else { + ec_thread_ = + std::make_unique([this]() { ec_manager_.cyclic_loop(); }); + ethercat_enabled_ = true; + } + } else if (!request->enable && ethercat_enabled_) { + RCLCPP_INFO(get_logger(), "Disabling EtherCAT communication"); + ec_manager_.stop(); + if (ec_thread_ && ec_thread_->joinable()) { + ec_thread_->join(); + } + ethercat_enabled_ = false; + } + + response->status_enable = ethercat_enabled_ ? 1 : 0; +} + +void EthercatNode::sdoReadServiceCallback( + const std::shared_ptr + request, + std::shared_ptr response) { + + if (!ethercat_enabled_ || !ec_manager_.is_running()) { + response->status_code = 0; + return; + } + + std::vector value; + bool success = ec_manager_.sdo_read(request->device_id, request->index, + request->subindex, value); + + if (!success) { + RCLCPP_WARN(get_logger(), "Read failed"); + response->status_code = 0; + return; + } + + response->status_code = 1; + response->device_id = request->device_id; + response->index = request->index; + response->subindex = request->subindex; + response->value = value; + response->value_type = 0; +} +void EthercatNode::velocityCommandCallback( + rise_motion_messages::msg::MotorVelocity::SharedPtr msg) { + std::vector velocities(msg->velocities.begin(), msg->velocities.end()); + if (!ec_manager_.set_motor_velocity_apsa(velocities)) { + RCLCPP_WARN(get_logger(), "Failed to queue velocity commands"); + } +} + +void EthercatNode::torqueOffsetCallback( + rise_motion_messages::msg::MotorTorqueOffset::SharedPtr msg) { + std::vector offsets(msg->torque_offsets.begin(), msg->torque_offsets.end()); + if (!ec_manager_.set_torque_offset_apsa(offsets)) { + RCLCPP_WARN(get_logger(), "Failed to queue torque offsets"); + } +} + +void EthercatNode::torqueCommandCallback( + rise_motion_messages::msg::MotorTorqueOffset::SharedPtr msg) { + std::vector torques(msg->torque_offsets.begin(), msg->torque_offsets.end()); + if (!ec_manager_.set_motor_torque_apsa(torques)) { + RCLCPP_WARN(get_logger(), "Failed to queue torque commands"); + } +} + +void EthercatNode::setOperationModeCallback( + std::shared_ptr request, + std::shared_ptr response) { + if (!ethercat_enabled_) { + response->success = false; + response->message = "EtherCAT not enabled"; + return; + } + if (request->mode != 8 && request->mode != 9 && request->mode != 10) { + response->success = false; + response->message = "Invalid mode " + std::to_string(request->mode) + ". Supported: 8 (Position), 9 (Velocity), 10 (Torque)."; + RCLCPP_WARN(get_logger(), "Rejected unsupported operation mode %d", request->mode); + return; + } + ec_manager_.set_operation_mode(request->mode); + response->success = true; + if (request->mode == 10) response->message = "Torque mode active (mode 10)"; + else if (request->mode == 9) response->message = "Velocity mode active (mode 9)"; + else response->message = "Position mode active (mode 8)"; + RCLCPP_INFO(get_logger(), "Operation mode set to %d", request->mode); +} + +void EthercatNode::publishFullFeedback() { + std::vector feedback; + + if (ethercat_enabled_ && ec_manager_.get_full_feedback_apsa(feedback)) { + auto msg = rise_motion_messages::msg::MotorFeedbackFull(); + msg.header.stamp = now(); + for (const auto& f : feedback) { + msg.statusword.push_back(f.statusword); + msg.op_mode_display.push_back(f.op_mode_display); + msg.positions.push_back(f.position); + msg.velocity_value.push_back(f.velocity_value); + msg.torque_value.push_back(f.torque_value); + msg.analog_input1.push_back(f.analog_input1); + msg.analog_input2.push_back(f.analog_input2); + msg.analog_input3.push_back(f.analog_input3); + msg.analog_input4.push_back(f.analog_input4); + msg.tuning_status.push_back(f.tuning_status); + msg.digital_inputs.push_back(f.digital_inputs); + msg.user_miso.push_back(f.user_miso); + msg.timestamp.push_back(f.timestamp); + msg.position_demand_internal_value.push_back(f.position_demand_internal_value); + msg.velocity_demand_value.push_back(f.velocity_demand_value); + msg.torque_demand.push_back(f.torque_demand); + } + full_feedback_pub_->publish(msg); + } +} + +void EthercatNode::sdoWriteServiceCallback( + const std::shared_ptr + request, + std::shared_ptr + response) { + if (!ethercat_enabled_ || !ec_manager_.is_running()) { + response->status_code = 0; + return; + } + RCLCPP_INFO(get_logger(), "Got sdo_write request"); + bool success = ec_manager_.sdo_write(request->device_id, request->index, + request->subindex, request->value); + RCLCPP_INFO(get_logger(), "%d", success); + if (!success) { + RCLCPP_INFO(get_logger(), "Write failed"); + response->status_code = 0; + return; + } + + response->status_code = 1; + response->device_id = request->device_id; + response->index = request->index; + response->subindex = request->subindex; + response->value_type = 0; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/main.cpp b/rise_motion_dev_ws/src/rise_motion/src/main.cpp new file mode 100755 index 0000000..5687e8e --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/main.cpp @@ -0,0 +1,16 @@ +#include +#include +#include +#include + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + ECManager ec_manager("enp5s0", 1); // TODO: Connect config from rise-os-core + auto node = std::make_shared(ec_manager); + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp b/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp new file mode 100644 index 0000000..3bfefb7 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp @@ -0,0 +1,26 @@ +#include +#include +#include +#include + +int main(int argc, char *argv[]) { + // num_motors mirrors real EtherCAT slave discovery: determined at startup, not runtime. + // Pass as first non-ROS argument: ros2 run rise_motion rise_motion_mock -- 2 + int num_motors = 6; + for (int i = 1; i < argc; ++i) { + if (argv[i][0] != '-' && std::isdigit(static_cast(argv[i][0]))) { + num_motors = std::atoi(argv[i]); + break; + } + } + + rclcpp::init(argc, argv); + + MockECManager ec_manager(/*cycle_period_ms=*/1, num_motors); + auto node = std::make_shared(ec_manager); + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} diff --git a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp new file mode 100755 index 0000000..d7c51e7 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -0,0 +1,119 @@ +#include +#include +#include +#include +#include +#include +#include +#include +#include + + +class TestNode : public rclcpp::Node { +public: + TestNode(int incs) : Node("test_node"), valid_positions(false), increment(incs) { + // Subscribe to the input topic + RCLCPP_INFO(get_logger(), "Starting TestNode"); + input_sub = + this->create_subscription( + "motor_feedback", 10, + [this](rise_motion_messages::msg::MotorPositions msg) { + if (!valid_positions) { + RCLCPP_INFO(get_logger(), "Got feedback"); + valid_positions = true; + } + motor_pos = msg.positions; + }); + publish_timer_ = create_wall_timer(std::chrono::milliseconds(1), [this]() { + if (!valid_positions) + return; + auto response = rise_motion_messages::msg::MotorPositions(); + response.positions.resize(motor_pos.size()); + for (size_t i = 0; i < motor_pos.size(); i++) { + response.positions[i] = motor_pos[i] + increment; + } + + output_pub->publish(response); + }); + + output_pub = + this->create_publisher( + "motor_commands", 10); + + client = this->create_client( + "enable_ethercat"); + } + ~TestNode() { + RCLCPP_INFO(get_logger(), "Bye :)"); + } + int request_enable_ethercat() { + RCLCPP_INFO(get_logger(), "Incrementing motor position with %d", increment); + RCLCPP_INFO(get_logger(), "Requesting Enable Ethercat"); + while (!client->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(this->get_logger(), + "client interrupted while waiting for service to appear."); + return 1; + } + RCLCPP_INFO(this->get_logger(), "waiting for service to appear..."); + } + auto request = std::make_shared< + rise_motion_messages::srv::EnableEthercatSrv::Request>(); + request->enable = true; + auto result_future = client->async_send_request(request); + if (rclcpp::spin_until_future_complete(this->shared_from_this(), + result_future) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(this->get_logger(), "service call failed :("); + client->remove_pending_request(result_future); + return 1; + } + auto result = result_future.get(); + RCLCPP_INFO(get_logger(), "Done requesting"); + return result->status_enable; + } + +private: + std::vector motor_pos; + void print_motor_positions(std::vector const &motor_pos, + std::string const &prefix) { + std::ostringstream oss; + oss << "["; + for (size_t i = 0; i < motor_pos.size(); ++i) { + oss << motor_pos[i]; + if (i != motor_pos.size() - 1) { + oss << ", "; + } + } + oss << "]"; + + RCLCPP_INFO(this->get_logger(), "%s: %s", prefix.c_str(), + oss.str().c_str()); + } + + bool valid_positions; + rclcpp::Subscription::SharedPtr + input_sub; + rclcpp::Publisher::SharedPtr + output_pub; + + rclcpp::Client::SharedPtr + client; + rclcpp::TimerBase::SharedPtr publish_timer_; + int increment; +}; + +int main(int argc, char **argv) { + int incs = 10; + if (argc >= 2) { + incs = atoi(argv[1]); + } + rclcpp::init(argc, argv); + auto node = std::make_shared(incs); + + while (!node->request_enable_ethercat()) { + } + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt old mode 100644 new mode 100755 index 38eed8b..c14fc24 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -8,11 +8,21 @@ endif() # find dependencies find_package(ament_cmake REQUIRED) find_package(rosidl_default_generators REQUIRED) +find_package(std_msgs REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateCurrentMsg.msg" "msg/StateNoticeMsg.msg" + "msg/MotorPositions.msg" + "msg/MotorFeedbackFull.msg" + "msg/AdmittanceDebug.msg" + "msg/MotorVelocity.msg" + "msg/MotorTorqueOffset.msg" "srv/EnableEthercatSrv.srv" + "srv/SDOReadSrv.srv" + "srv/SDOWriteSrv.srv" + "srv/SetOperationModeSrv.srv" + DEPENDENCIES std_msgs ) if(BUILD_TESTING) diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg new file mode 100644 index 0000000..5b66380 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -0,0 +1,49 @@ +# Per-motor debug data for admittance controller +# Arrays are indexed by motor number +std_msgs/Header header # ROS timestamp when debug data was computed + + +# Force sensor calibration chain +float64[] adc_voltage_raw # [V] ADC ticks -> voltage (uncalibrated, 0..5V range) +float64[] adc_voltage # [V] ADC ticks -> voltage (centered: 0V = no force) +float64[] force # [N] voltage -> force (before clipping) +float64[] force_clipped # [N] after f_max clipping +float64[] force_filtered # [N] after EMA low-pass filter (input to controller) +float64[] torque_raw # [Nm] force_filtered * lever_arm -> input to controller + +# Absolute motor position (encoder ticks / enc_per_rad) — same reference as pos_min/max limits +float64[] pos_rad # [rad] absolute encoder position + +# Position relative to a user-settable zero point (tare), for experiments where the raw +# absolute encoder origin is meaningless (e.g. sinus-reference tracking). Zeroed via the +# ~/zero_position service (captures current pos_rad as the new origin). Purely informational +# — never used for pos_min/max limit checks or any control law, those stay on pos_rad. +float64[] pos_rel_rad # [rad] pos_rad - offset from last ~/zero_position call +float64[] pos_rel_deg # [deg] same, in degrees + +# Admittance integration (rad) +float64[] vdot # [rad/s^2] angular acceleration +float64[] velocity_raw # [rad/s] after Euler integration (before safety) +float64[] velocity_output # [rad/s] after jerk limit + vel clamp (published value) +float64[] displacement # [rad] integrated displacement (for K_v > 0) + +# Same values in degrees (for readability) +float64[] vdot_deg # [deg/s^2] +float64[] velocity_raw_deg # [deg/s] +float64[] velocity_output_deg # [deg/s] +float64[] displacement_deg # [deg] + +# Measured joint velocity (velocity_fb converted from motor RPM to joint rad/s) +float64[] velocity_fb_joint # [rad/s] velocity_value [RPM] * 2π/60 / gear_ratio + +# Compensation torques at joint level [Nm] — observer only, same formula as comp functions +float64[] tau_gravity_nm # [Nm] mgl * cos(pos_rad + angle_offset); 0 if disabled +float64[] tau_friction_nm # [Nm] (tau_c * tanh(k*v) + b*v) * scale; 0 if disabled or |v|>max_vel + +# Observed joint acceleration (differentiated velocity_fb, no controller feedback) +float64[] accel_raw # [rad/s^2] (v_fb[k] - v_fb[k-1]) / dt — joint-level +float64[] accel_filtered # [rad/s^2] EMA low-pass (accel_filter_cutoff_hz) + +# Measured motor torque (0x6077), converted to joint [Nm] +# = torque_value [‰ rated] / 1000 * rated_torque_motor [Nm] * gear_ratio +float64[] torque_fb # [Nm] at joint diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg new file mode 100644 index 0000000..a3583e0 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg @@ -0,0 +1,19 @@ +# All CiA402_Inputs PDO fields as parallel arrays, index = motor +std_msgs/Header header # ROS timestamp when message was published + +uint16[] statusword +int8[] op_mode_display +int32[] positions +int32[] velocity_value +int16[] torque_value +uint16[] analog_input1 # Force sensor channel 1 (ADC ticks, 0=Umin, 65535=Umax) +uint16[] analog_input2 +uint16[] analog_input3 +uint16[] analog_input4 +uint32[] tuning_status +uint32[] digital_inputs +uint32[] user_miso +uint32[] timestamp +int32[] position_demand_internal_value +int32[] velocity_demand_value +int16[] torque_demand diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorPositions.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorPositions.msg new file mode 100755 index 0000000..d8ac2d1 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorPositions.msg @@ -0,0 +1 @@ +int32[] positions diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorTorqueOffset.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorTorqueOffset.msg new file mode 100644 index 0000000..b76f3cc --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorTorqueOffset.msg @@ -0,0 +1,2 @@ +std_msgs/Header header +int16[] torque_offsets # per motor, unit: per-mille (‰) of rated torque (CiA402 0x60B2) diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg new file mode 100644 index 0000000..7ab4417 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg @@ -0,0 +1,2 @@ +std_msgs/Header header # ROS timestamp when command was computed +int32[] velocities # velocity commands in RPM (0x60FF target velocity), one per motor diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/StateCurrentMsg.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/StateCurrentMsg.msg old mode 100644 new mode 100755 diff --git a/rise_motion_dev_ws/src/rise_motion_messages/msg/StateNoticeMsg.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/StateNoticeMsg.msg old mode 100644 new mode 100755 diff --git a/rise_motion_dev_ws/src/rise_motion_messages/package.xml b/rise_motion_dev_ws/src/rise_motion_messages/package.xml old mode 100644 new mode 100755 diff --git a/rise_motion_dev_ws/src/rise_motion_messages/srv/EnableEthercatSrv.srv b/rise_motion_dev_ws/src/rise_motion_messages/srv/EnableEthercatSrv.srv old mode 100644 new mode 100755 diff --git a/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv new file mode 100644 index 0000000..e36b472 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv @@ -0,0 +1,11 @@ +uint16 device_id +uint16 index +uint8 subindex +uint8 value_type +--- +int8 status_code +uint16 device_id +uint16 index +uint8 subindex +uint8 value_type +uint8[] value diff --git a/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOWriteSrv.srv b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOWriteSrv.srv new file mode 100644 index 0000000..26bc031 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/srv/SDOWriteSrv.srv @@ -0,0 +1,11 @@ +uint16 device_id +uint16 index +uint8 subindex +uint8 value_type +uint8[] value +--- +int8 status_code +uint16 device_id +uint16 index +uint8 subindex +uint8 value_type \ No newline at end of file diff --git a/rise_motion_dev_ws/src/rise_motion_messages/srv/SetOperationModeSrv.srv b/rise_motion_dev_ws/src/rise_motion_messages/srv/SetOperationModeSrv.srv new file mode 100644 index 0000000..47c6cb4 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/srv/SetOperationModeSrv.srv @@ -0,0 +1,4 @@ +int8 mode # 8 = CyclicSyncPositionMode, 9 = CyclicSyncVelocityMode +--- +bool success +string message