From 763896e55815a1af03197afb3584d1a2ade24609 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Wed, 11 Feb 2026 14:12:58 +0100 Subject: [PATCH 01/83] Merge all changes from initial testing issues. --- .../src/rise_motion/CMakeLists.txt | 45 ++++- .../rise_motion/include/rise_motion/apsa.hpp | 123 ++++++++++++ .../include/rise_motion/cia402.hpp | 148 ++++++++++++++ .../include/rise_motion/ec_manager.hpp | 46 +++++ .../include/rise_motion/ec_structs.hpp | 37 ++++ .../include/rise_motion/ethercat_node.hpp | 29 +++ .../src/rise_motion/package.xml | 3 + .../src/rise_motion/src/cia402.cpp | 103 ++++++++++ .../src/rise_motion/src/ec_manager.cpp | 182 ++++++++++++++++++ .../src/rise_motion/src/ethercat_node.cpp | 93 +++++++++ .../src/rise_motion/src/main.cpp | 16 ++ .../src/rise_motion/src/testing_node.cpp | 100 ++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../msg/MotorPositions.msg | 1 + .../msg/StateCurrentMsg.msg | 0 .../msg/StateNoticeMsg.msg | 0 .../src/rise_motion_messages/package.xml | 0 .../srv/EnableEthercatSrv.srv | 0 18 files changed, 924 insertions(+), 3 deletions(-) create mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/apsa.hpp create mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp create mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp create mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp create mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/ethercat_node.hpp create mode 100755 rise_motion_dev_ws/src/rise_motion/src/cia402.cpp create mode 100755 rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp create mode 100755 rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp create mode 100755 rise_motion_dev_ws/src/rise_motion/src/main.cpp create mode 100755 rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp mode change 100644 => 100755 rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt create mode 100755 rise_motion_dev_ws/src/rise_motion_messages/msg/MotorPositions.msg mode change 100644 => 100755 rise_motion_dev_ws/src/rise_motion_messages/msg/StateCurrentMsg.msg mode change 100644 => 100755 rise_motion_dev_ws/src/rise_motion_messages/msg/StateNoticeMsg.msg mode change 100644 => 100755 rise_motion_dev_ws/src/rise_motion_messages/package.xml mode change 100644 => 100755 rise_motion_dev_ws/src/rise_motion_messages/srv/EnableEthercatSrv.srv diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index aa620a5..ce8a2e3 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -7,10 +7,49 @@ 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) +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(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..5bc5908 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp @@ -0,0 +1,148 @@ +#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; + int32_t TuningCommand; + int32_t PhysicalOutputs; + int32_t BitMask; + int32_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 reset_fault(); + void set_control_word(Operation op); + void set_mode_of_operation(ModeOfOperation m); + +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}}; + + CiA402_Inputs *inputs; + CiA402_Outputs *outputs; +}; 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..a4eca17 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager.hpp @@ -0,0 +1,46 @@ +#pragma once +#include +#include +#include +#include +#include +#include +#include "apsa.hpp" + +#define IOMAP_SIZE 4096 + +class ECManager { +public: + ECManager(); + ECManager(const std::string interface); + void init_ec(); + void cyclic_loop(); + void stop(); + + // APSA-based motor value transfer (lock-free) + bool get_motor_values_apsa(std::vector& motor_values); + bool set_motor_values_apsa(const std::vector& motor_values); + + // Legacy mutex-based methods (deprecated) + void get_motor_values(std::vector& motor_values); + void set_motor_values(std::vector& motor_values); + +private: + void transition_to_operational(); + + // EtherCAT context and configuration + int expectedWKC; + ecx_contextt ctx; + uint8_t IOMap[IOMAP_SIZE]; + std::mutex ctx_mutex; // Only used for legacy methods and state transitions + std::atomic running_{false}; + const std::string interface; + static rclcpp::Logger logger; + + // APSA instances for lock-free communication + // cmd_apsa: ROS → EtherCAT (motor commands) + APSA> cmd_apsa; + + // feedback_apsa: EtherCAT → ROS (motor feedback) + APSA> feedback_apsa; +}; diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp new file mode 100755 index 0000000..069fde6 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp @@ -0,0 +1,37 @@ +#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; +} outputs; +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; + int32_t TuningCommand; + int32_t PhysicalOutputs; + int32_t BitMask; + int32_t UserMOSI; + int32_t VelocityOffset; +} inputs; +OSAL_PACKED_END 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..2e41427 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ethercat_node.hpp @@ -0,0 +1,29 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +class EthercatNode : public rclcpp::Node { +public: + explicit EthercatNode(ECManager& ec_manager); + ~EthercatNode(); + +private: + ECManager& ec_manager_; + std::unique_ptr ec_thread_; + bool ethercat_enabled_{false}; + + rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Publisher::SharedPtr feedback_pub_; + rclcpp::TimerBase::SharedPtr feedback_timer_; + rclcpp::Service::SharedPtr enable_srv_; + + void commandCallback(const rise_motion_messages::msg::MotorPositions::SharedPtr msg); + void publishFeedback(); + void enableServiceCallback( + const std::shared_ptr request, + std::shared_ptr response); +}; 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..d8354d8 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -0,0 +1,103 @@ +#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; + } +} + +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::OPERATION_ENABLED: + case State::QUICK_STOP_ACTIVE: + case State::FAULT_REACTION_ACTIVE: + case State::FAULT: + 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..8330dce --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -0,0 +1,182 @@ +#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; + ec_slavet slavelist[1] = {{.name = "a name"}}; +} config; + +ECManager::ECManager(const std::string interface) : interface(interface) {} + +void ECManager::init_ec() { + int ret; + RCLCPP_INFO(logger, "Connecting to %s", interface.c_str()); + ret = ecx_init(&ctx, interface.c_str()); + RCLCPP_INFO(logger, "Connecting to %d", ret); + if (ret <= 0) { + RCLCPP_WARN(logger, "Couldn't initialize SOEM context"); + std::exit(EXIT_FAILURE); + } + + RCLCPP_INFO(logger, "Discovering EC Nodes"); + ret = ecx_config_init(&ctx); + if (ret <= 0) { + RCLCPP_WARN(logger, "EC Nodes Discovery failed"); + std::exit(EXIT_FAILURE); + } + + if (ctx.slavecount != config.slavecount) { + RCLCPP_WARN(logger, "Expected %d devices, but discovered %d", + config.slavecount, ctx.slavecount); + std::exit(EXIT_FAILURE); + } + + RCLCPP_INFO(logger, "Mapping IO"); + ret = ecx_config_map_group(&ctx, IOMap, 0); + if (ret > IOMAP_SIZE) { + RCLCPP_WARN(logger, "Couldn't map IO: Buffer to small"); + std::exit(EXIT_FAILURE); + } + + expectedWKC = ctx.grouplist[0].outputsWKC * 2 + ctx.grouplist[0].inputsWKC; + RCLCPP_INFO(logger, "Configuring ditributed clock"); + ecx_configdc(&ctx); + + ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + + // Check if nodes have valid outputs + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + // TODO: Check if nodes have valid outputs + for (int i = 1; i <= ctx.slavecount; i++) { + if (strcmp(config.slavelist[i].name, ctx.slavelist[i].name)) { + RCLCPP_WARN(logger, "Node %d: Name does not match: %s != %s", i, config.slavelist[i].name, ctx.slavelist[i].name); + } + } + // Now all nodes should be in safe op +} +void ECManager::transition_to_operational() { + std::unique_lock lk(ctx_mutex); + RCLCPP_INFO(logger, "Entering operational mode"); + ctx.slavelist[0].state = EC_STATE_OPERATIONAL; + ecx_writestate(&ctx, 0); + + // check if nodes entered operational mode + int chk = 200; + do { + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 50000); + } while (chk-- && (ctx.slavelist[0].state != EC_STATE_OPERATIONAL)); + if (ctx.slavelist[0].state != EC_STATE_OPERATIONAL) { + RCLCPP_WARN(logger, "Couldn't transition to operational"); + std::exit(EXIT_FAILURE); + } +} +void ECManager::cyclic_loop() { + transition_to_operational(); + running_ = true; + int wkc; + auto next = std::chrono::steady_clock::now(); + auto period = std::chrono::milliseconds(1); + + // Local buffer for motor commands + std::vector motor_commands(config.slavecount, 0); + + // Local buffer for motor feedback + std::vector motor_feedback(config.slavecount, 0); + int flag = 1; + std::cout << "Going to operation_enabled\n"; + while (flag) { + flag = 0; + for (int i = 1; i < config.slavecount; i++) { + CiA402Motor m{(CiA402_Inputs*)ctx.slavelist[i].inputs, (CiA402_Outputs*)ctx.slavelist[i].outputs}; + if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + m.to_operation_enabled(); + flag = 1; + } else { + m.set_mode_of_operation(CiA402Motor::ModeOfOperation::ProfilePositionMode); + } + } + } + std::cout << "All motor in operation_enabled\n"; + + while (running_) { + next += period; + // perf_read() is wait-free - returns immediately if no new data + if (cmd_apsa.perf_read(motor_commands)) { + // New commands received! Apply them to EtherCAT slaves + RCLCPP_INFO(logger, "New Motor Positions"); + for (int i = 0; i < config.slavecount; i++) { + RCLCPP_INFO(logger, "Writing %d to Motor %d", motor_commands[i], i+1); + inputs *motor_inputs = (inputs *)ctx.slavelist[i + 1].inputs; + motor_inputs->TargetPosition = motor_commands[i]; + } + } + // If no new commands, EtherCAT slaves keep executing previous commands + + ecx_send_processdata(&ctx); + wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + + if (wkc != expectedWKC) { + RCLCPP_WARN(logger, "Not all nodes responded"); + } + + for (int i = 0; i < config.slavecount; i++) { + outputs *motor_outputs = (outputs *)ctx.slavelist[i + 1].outputs; + motor_feedback[i] = motor_outputs->PositionValue; + } + + // Make feedback available to ROS publisher (wait-free) + feedback_apsa.perf_write(motor_feedback); + + // Sleep until next cycle (maintains 1kHz frequency) + std::this_thread::sleep_until(next); + } +} +void ECManager::stop() { + running_ = false; +} + +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); +} + + +void ECManager::get_motor_values(std::vector &motor_values) { + std::unique_lock lk(ctx_mutex); + for (int i = 0; i < config.slavecount; i++) { + outputs *motor_outputs = (outputs *)ctx.slavelist[i + 1].outputs; + motor_values[i] = motor_outputs->PositionValue; + } +} + + +void ECManager::set_motor_values(std::vector &motor_values) { + std::unique_lock lk(ctx_mutex); + for (int i = 0; i < config.slavecount; i++) { + inputs *motor_inputs = (inputs *)ctx.slavelist[i + 1].inputs; + motor_inputs->TargetPosition = motor_values[i]; + } +} + +rclcpp::Logger ECManager::logger = rclcpp::get_logger("ECManager"); 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..8f8eb38 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -0,0 +1,93 @@ +#include +#include +#include + +using std::placeholders::_1; +using std::placeholders::_2; + +EthercatNode::EthercatNode(ECManager& ec_manager) + : Node("ethercat_node"), ec_manager_(ec_manager) { + + cmd_sub_ = create_subscription( + "motor_commands", 10, + std::bind(&EthercatNode::commandCallback, this, _1)); + + feedback_pub_ = create_publisher( + "motor_feedback", 10); + + feedback_timer_ = create_wall_timer( + std::chrono::milliseconds(10), + std::bind(&EthercatNode::publishFeedback, this)); + + enable_srv_ = create_service( + "enable_ethercat", + std::bind(&EthercatNode::enableServiceCallback, this, std::placeholders::_1, std::placeholders::_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 (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"); + ec_manager_.init_ec(); + ec_thread_ = std::make_unique(&ECManager::cyclic_loop, &ec_manager_); + 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; +} 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..2b59fe0 --- /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("enp2s0f0"); // 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/testing_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp new file mode 100755 index 0000000..2aca674 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -0,0 +1,100 @@ +#include +#include +#include +#include +#include +#include +#include + +class TestNode : public rclcpp::Node { +public: + TestNode() : Node("test_node") { + // 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) { + auto const &motor_pos = msg.positions; + + print_motor_positions(motor_pos, "Received"); + + 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] + 100; + } + + output_pub->publish(response); + print_motor_positions(response.positions, "Published"); + }); + + output_pub = + this->create_publisher( + "motor_commands", 10); + + client = this->create_client( + "enable_ethercat"); + } + + int request_enable_ethercat() { + 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(); + return result->status_enable; + } + +private: + 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()); + } + rclcpp::Subscription::SharedPtr + input_sub; + rclcpp::Publisher::SharedPtr + output_pub; + + rclcpp::Client::SharedPtr + client; +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + + 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..b4f5fa9 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -12,6 +12,7 @@ find_package(rosidl_default_generators REQUIRED) rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateCurrentMsg.msg" "msg/StateNoticeMsg.msg" + "msg/MotorPositions.msg" "srv/EnableEthercatSrv.srv" ) 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/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 From 7b49639cc9023f0faa941d34c957677d68da64b2 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:17:26 +0100 Subject: [PATCH 02/83] rise_motion: cia402.hpp: Fix signedness of CiA402_Outputs. --- .../src/rise_motion/include/rise_motion/cia402.hpp | 8 ++++---- 1 file changed, 4 insertions(+), 4 deletions(-) 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 index 5bc5908..2170347 100755 --- 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 @@ -33,10 +33,10 @@ typedef struct OSAL_PACKED { int32_t TargetPosition; int32_t TargetVelocity; int16_t TorqueOffset; - int32_t TuningCommand; - int32_t PhysicalOutputs; - int32_t BitMask; - int32_t UserMOSI; + uint32_t TuningCommand; + uint32_t PhysicalOutputs; + uint32_t BitMask; + uint32_t UserMOSI; int32_t VelocityOffset; } CiA402_Outputs; OSAL_PACKED_END From 238ae55fea92ccfd60cbbc531a2a31f9ed636f61 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:18:51 +0100 Subject: [PATCH 03/83] rise_motion: Add CiA402::to_switch_on_disabled. --- .../include/rise_motion/cia402.hpp | 1 + .../src/rise_motion/src/cia402.cpp | 26 +++++++++++++++++++ 2 files changed, 27 insertions(+) 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 index 2170347..8b25d53 100755 --- 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 @@ -103,6 +103,7 @@ class CiA402Motor { bool is_fault() const; void to_operation_enabled(); + void to_switch_on_disabled(); void reset_fault(); void set_control_word(Operation op); void set_mode_of_operation(ModeOfOperation m); diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp index d8354d8..67a2cf8 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -58,6 +58,32 @@ void CiA402Motor::to_operation_enabled() { } } +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: + case State::QUICK_STOP_ACTIVE: + next_op = Operation::SHUTDOWN; + case State::FAULT_REACTION_ACTIVE: + case State::FAULT: + case State::SWITCH_ON_DISABLED: + case State::READY_TO_SWITCH_ON: + case State::SWITCHED_ON: + 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); } From fa8d393db293cc5859306d0276b1b31b92d52534 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:20:10 +0100 Subject: [PATCH 04/83] rise_motion: Enable packedMode. Also removes unnecessary print statement. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index 8330dce..999abf3 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -23,8 +23,8 @@ ECManager::ECManager(const std::string interface) : interface(interface) {} void ECManager::init_ec() { int ret; RCLCPP_INFO(logger, "Connecting to %s", interface.c_str()); + ctx.packedMode = TRUE; ret = ecx_init(&ctx, interface.c_str()); - RCLCPP_INFO(logger, "Connecting to %d", ret); if (ret <= 0) { RCLCPP_WARN(logger, "Couldn't initialize SOEM context"); std::exit(EXIT_FAILURE); From 5ddbcd824e271973ee78bc7a32d8039603dff0cc Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:22:43 +0100 Subject: [PATCH 05/83] rise_motion: ec_manager: Remove obsolete functions. Removes: - get_motor_values - set_motor_values - ctx_mutex --- .../include/rise_motion/ec_manager.hpp | 1 - .../src/rise_motion/src/ec_manager.cpp | 23 +++---------------- 2 files changed, 3 insertions(+), 21 deletions(-) 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 index a4eca17..fc2cc2a 100755 --- 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 @@ -32,7 +32,6 @@ class ECManager { int expectedWKC; ecx_contextt ctx; uint8_t IOMap[IOMAP_SIZE]; - std::mutex ctx_mutex; // Only used for legacy methods and state transitions std::atomic running_{false}; const std::string interface; static rclcpp::Logger logger; 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 index 999abf3..f3ce026 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -67,9 +67,10 @@ void ECManager::init_ec() { } // Now all nodes should be in safe op } + void ECManager::transition_to_operational() { - std::unique_lock lk(ctx_mutex); - RCLCPP_INFO(logger, "Entering operational mode"); + // Transitions Ethercat State Machine to operational + RCLCPP_INFO(logger, "Transitioning to operational mode"); ctx.slavelist[0].state = EC_STATE_OPERATIONAL; ecx_writestate(&ctx, 0); @@ -161,22 +162,4 @@ bool ECManager::set_motor_values_apsa(const std::vector &motor_values) return cmd_apsa.comm_write(motor_values); } - -void ECManager::get_motor_values(std::vector &motor_values) { - std::unique_lock lk(ctx_mutex); - for (int i = 0; i < config.slavecount; i++) { - outputs *motor_outputs = (outputs *)ctx.slavelist[i + 1].outputs; - motor_values[i] = motor_outputs->PositionValue; - } -} - - -void ECManager::set_motor_values(std::vector &motor_values) { - std::unique_lock lk(ctx_mutex); - for (int i = 0; i < config.slavecount; i++) { - inputs *motor_inputs = (inputs *)ctx.slavelist[i + 1].inputs; - motor_inputs->TargetPosition = motor_values[i]; - } -} - rclcpp::Logger ECManager::logger = rclcpp::get_logger("ECManager"); From 22f1fbe57cdce471d1478d2cc0ed7b13f6aa9dfd Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:24:53 +0100 Subject: [PATCH 06/83] rise_motion: ec_manager.cpp: Add motor shutdown on cyclic loop exit. --- .../src/rise_motion/src/ec_manager.cpp | 20 ++++++++++++++++--- 1 file changed, 17 insertions(+), 3 deletions(-) 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 index f3ce026..34c6fba 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -146,10 +146,24 @@ void ECManager::cyclic_loop() { // Sleep until next cycle (maintains 1kHz frequency) std::this_thread::sleep_until(next); } + + RCLCPP_INFO(logger, "Exiting cyclic loop"); + + // Shut down motors + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, + (CiA402_Outputs *)ctx.slavelist[i].outputs}; + m.to_switch_on_disabled(); + } + ecx_send_processdata(&ctx); + wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + + if (wkc != expectedWKC) { + RCLCPP_WARN(logger, "Not all nodes responded"); + } } -void ECManager::stop() { - running_ = false; -} + +void ECManager::stop() { running_ = false; } bool ECManager::get_motor_values_apsa(std::vector &motor_values) { // comm_read() returns true if new data is available, false otherwise From 5320121b07653273c3465634871b76a76cac2801 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:26:13 +0100 Subject: [PATCH 07/83] Delete ec_structs.hpp --- .../include/rise_motion/ec_structs.hpp | 37 ------------------- 1 file changed, 37 deletions(-) delete mode 100755 rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp diff --git a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp deleted file mode 100755 index 069fde6..0000000 --- a/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_structs.hpp +++ /dev/null @@ -1,37 +0,0 @@ -#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; -} outputs; -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; - int32_t TuningCommand; - int32_t PhysicalOutputs; - int32_t BitMask; - int32_t UserMOSI; - int32_t VelocityOffset; -} inputs; -OSAL_PACKED_END From 992f0c3f80f082262b8e03582569e5337ad912f7 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:27:44 +0100 Subject: [PATCH 08/83] ec_manager.cpp: Set ModeOfOperation to CyclicSyncPositionMode Also make the size of motor_feedback/motor_commands dependent on actual amount of nodes (ctx instead of config). --- .../src/rise_motion/src/ec_manager.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) 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 index 34c6fba..fecfe52 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -93,11 +93,17 @@ void ECManager::cyclic_loop() { auto next = std::chrono::steady_clock::now(); auto period = std::chrono::milliseconds(1); - // Local buffer for motor commands - std::vector motor_commands(config.slavecount, 0); + std::vector motor_commands(ctx.slavecount, 0); + std::vector motor_feedback(ctx.slavecount, 0); + + // Setting ModeOfOperation to CyclicSyncPositionMode + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, + (CiA402_Outputs *)ctx.slavelist[i].outputs}; + m.set_mode_of_operation( + CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + } - // Local buffer for motor feedback - std::vector motor_feedback(config.slavecount, 0); int flag = 1; std::cout << "Going to operation_enabled\n"; while (flag) { From 48c422d376e4eb0c3399a2fe58a4653e4086cc4e Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:30:09 +0100 Subject: [PATCH 09/83] ec_manager.cpp: Fix Transition to OPERATION_ENABLED (CiA402). --- .../src/rise_motion/src/ec_manager.cpp | 34 +++++++++++++------ 1 file changed, 23 insertions(+), 11 deletions(-) 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 index fecfe52..f36c146 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -104,21 +104,33 @@ void ECManager::cyclic_loop() { CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); } + // Transitioning CiA402 State Machine to OPERATION_ENABLED int flag = 1; - std::cout << "Going to operation_enabled\n"; + RCLCPP_INFO(logger, "Going to operation_enabled"); while (flag) { - flag = 0; - for (int i = 1; i < config.slavecount; i++) { - CiA402Motor m{(CiA402_Inputs*)ctx.slavelist[i].inputs, (CiA402_Outputs*)ctx.slavelist[i].outputs}; - if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - m.to_operation_enabled(); - flag = 1; - } else { - m.set_mode_of_operation(CiA402Motor::ModeOfOperation::ProfilePositionMode); - } + flag = 0; + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, + (CiA402_Outputs *)ctx.slavelist[i].outputs}; + + RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + flag = 1; + RCLCPP_INFO(logger, "Motor %d has no state", i); + } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + m.to_operation_enabled(); + flag = 1; + } else if (m.get_state().value() == CiA402Motor::State::FAULT) { + RCLCPP_INFO(logger, "Motor %d in fault. Fault handling not implemented. Exiting...", i); + std::exit(EXIT_FAILURE); } + } + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); } - std::cout << "All motor in operation_enabled\n"; + + RCLCPP_INFO(logger, "All motors in operation_enabled"); + RCLCPP_INFO(logger, "Entering Cyclic Loop"); while (running_) { next += period; From 3e6f7dd0c41581bf63a5ce61eeb36411bdc56cd1 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:31:45 +0100 Subject: [PATCH 10/83] ec_manager.cpp: Fix indexing and input/output semantic. Also adds debug statements. --- .../src/rise_motion/src/ec_manager.cpp | 39 +++++++++++++++---- 1 file changed, 32 insertions(+), 7 deletions(-) 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 index f36c146..62839ba 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -136,15 +136,40 @@ void ECManager::cyclic_loop() { next += period; // perf_read() is wait-free - returns immediately if no new data if (cmd_apsa.perf_read(motor_commands)) { - // New commands received! Apply them to EtherCAT slaves - RCLCPP_INFO(logger, "New Motor Positions"); - for (int i = 0; i < config.slavecount; i++) { - RCLCPP_INFO(logger, "Writing %d to Motor %d", motor_commands[i], i+1); - inputs *motor_inputs = (inputs *)ctx.slavelist[i + 1].inputs; - motor_inputs->TargetPosition = motor_commands[i]; + // New commands received! Apply them to EtherCAT nodes + for (int i = 1; i <= config.slavecount; i++) { + CiA402_Outputs *motor_outputs = + (CiA402_Outputs *)ctx.slavelist[i].outputs; + CiA402_Inputs *motor_inputs = + (CiA402_Inputs *)ctx.slavelist[i].inputs; + + // guard statement + if (abs(motor_commands[i-1] - motor_inputs->PositionValue) > 100) {continue;} + //write value + motor_outputs->TargetPosition = motor_commands[i-1]; + //RCLCPP_INFO(logger, "TargetPosition: %d, PositionValue: %d", motor_outputs->TargetPosition, motor_inputs->PositionValue); + 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", + motor_outputs->Controlword, motor_outputs->OpMode, + motor_outputs->TargetTorque, motor_outputs->TargetPosition, + motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, + motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, + motor_outputs->BitMask, motor_outputs->UserMOSI, + motor_outputs->VelocityOffset); } } - // If no new commands, EtherCAT slaves keep executing previous commands + // If no new commands, EtherCAT nodes keep executing previous commands ecx_send_processdata(&ctx); wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); From 7b2ee72a76fcc6a2ca3210660d20aad56ad1b68f Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:32:29 +0100 Subject: [PATCH 11/83] ec_manager.cpp: Fix indexing and input/output semantic. (again) Also adds debug statements. --- .../src/rise_motion/src/ec_manager.cpp | 42 +++++++++++++++++-- 1 file changed, 39 insertions(+), 3 deletions(-) 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 index 62839ba..1e92358 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -178,9 +178,45 @@ void ECManager::cyclic_loop() { RCLCPP_WARN(logger, "Not all nodes responded"); } - for (int i = 0; i < config.slavecount; i++) { - outputs *motor_outputs = (outputs *)ctx.slavelist[i + 1].outputs; - motor_feedback[i] = motor_outputs->PositionValue; + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Inputs *motor_inputs = + (CiA402_Inputs *)ctx.slavelist[i].inputs; + motor_feedback[i-1] = motor_inputs->PositionValue; + 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", + motor_inputs->Statusword, + motor_inputs->OpModeDisplay, + motor_inputs->PositionValue, + motor_inputs->VelocityValue, + motor_inputs->TorqueValue, + motor_inputs->AnalogInput1, + motor_inputs->AnalogInput2, + motor_inputs->AnalogInput3, + motor_inputs->AnalogInput4, + motor_inputs->TuningStatus, + motor_inputs->DigitalInputs, + motor_inputs->UserMISO, + motor_inputs->Timestamp, + motor_inputs->PositionDemandInternalValue, + motor_inputs->VelocityDemandValue, + motor_inputs->TorqueDemand + ); } // Make feedback available to ROS publisher (wait-free) From cce40763290d0c7844953593b499e697dda92c68 Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:33:43 +0100 Subject: [PATCH 12/83] ec_manager.cpp: Fix headers. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 6 ++---- 1 file changed, 2 insertions(+), 4 deletions(-) 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 index 1e92358..d710fdc 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -1,12 +1,10 @@ #include #include -#include -#include +#include #include #include -#include -#include #include +#include #include #include #include From 5ce8982f8bedd25abc15f3bd1708664693a52b3a Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:34:28 +0100 Subject: [PATCH 13/83] ec_manager.cpp: Fix dummy node name check. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 7 ++++--- 1 file changed, 4 insertions(+), 3 deletions(-) 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 index d710fdc..1994849 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -13,7 +13,7 @@ // expected config, needs to be retrieved from config node struct { int slavecount = 1; - ec_slavet slavelist[1] = {{.name = "a name"}}; + ec_slavet slavelist[1] = {{.name = "WRONG_NAME"}}; } config; ECManager::ECManager(const std::string interface) : interface(interface) {} @@ -59,8 +59,9 @@ void ECManager::init_ec() { ecx_receive_processdata(&ctx, EC_TIMEOUTRET); // TODO: Check if nodes have valid outputs for (int i = 1; i <= ctx.slavecount; i++) { - if (strcmp(config.slavelist[i].name, ctx.slavelist[i].name)) { - RCLCPP_WARN(logger, "Node %d: Name does not match: %s != %s", i, config.slavelist[i].name, ctx.slavelist[i].name); + if (strcmp(config.slavelist[i-1].name, ctx.slavelist[i].name)) { + RCLCPP_WARN(logger, "Node %d: Name does not match: %s != %s", i, + config.slavelist[i-1].name, ctx.slavelist[i].name); } } // Now all nodes should be in safe op From e77aed0ecf2886ae9f7af5b0dea6613186510dcb Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:34:57 +0100 Subject: [PATCH 14/83] rise_motion: main.cpp: Set interface to enp1s0. --- rise_motion_dev_ws/src/rise_motion/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/main.cpp b/rise_motion_dev_ws/src/rise_motion/src/main.cpp index 2b59fe0..365d115 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/main.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/main.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - ECManager ec_manager("enp2s0f0"); // TODO: Connect config from rise-os-core + ECManager ec_manager("enp1s0"); // TODO: Connect config from rise-os-core auto node = std::make_shared(ec_manager); rclcpp::spin(node); From 03ec3589b7ad68b97f793421ec4e9d9e6e71b30c Mon Sep 17 00:00:00 2001 From: simeon Date: Tue, 17 Feb 2026 15:35:43 +0100 Subject: [PATCH 15/83] testing_node.cpp: Make node less noisy. --- rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp | 8 +++++--- 1 file changed, 5 insertions(+), 3 deletions(-) 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 index 2aca674..e99b669 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -6,6 +6,7 @@ #include #include +const int increment = 20; class TestNode : public rclcpp::Node { public: TestNode() : Node("test_node") { @@ -17,16 +18,16 @@ class TestNode : public rclcpp::Node { [this](rise_motion_messages::msg::MotorPositions msg) { auto const &motor_pos = msg.positions; - print_motor_positions(motor_pos, "Received"); + // print_motor_positions(motor_pos, "Received"); 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] + 100; + response.positions[i] = motor_pos[i] + increment; } output_pub->publish(response); - print_motor_positions(response.positions, "Published"); + // print_motor_positions(response.positions, "Published"); }); output_pub = @@ -38,6 +39,7 @@ class TestNode : public rclcpp::Node { } 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()) { From 332a3620d07a9a0960a85962b676f03d58ac1191 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 18 Feb 2026 18:04:48 +0100 Subject: [PATCH 16/83] rise_motion: cia402.cpp: Reset Faults (for testing). Might need to remove in real system. --- rise_motion_dev_ws/src/rise_motion/src/cia402.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp index 67a2cf8..e25306c 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -45,10 +45,12 @@ void CiA402Motor::to_operation_enabled() { case State::SWITCHED_ON: next_op = Operation::ENABLE_OPERATION; break; + case State::FAULT: + next_op = Operation::FAULT_RESET; // Remove in Production + break; case State::OPERATION_ENABLED: case State::QUICK_STOP_ACTIVE: case State::FAULT_REACTION_ACTIVE: - case State::FAULT: default: return; } @@ -70,8 +72,11 @@ void CiA402Motor::to_switch_on_disabled() { case State::OPERATION_ENABLED: case State::QUICK_STOP_ACTIVE: next_op = Operation::SHUTDOWN; - case State::FAULT_REACTION_ACTIVE: + break; case State::FAULT: + next_op = Operation::FAULT_RESET; // Remove in Production + break; + case State::FAULT_REACTION_ACTIVE: case State::SWITCH_ON_DISABLED: case State::READY_TO_SWITCH_ON: case State::SWITCHED_ON: From 7bb70e62569f0bcd842ad15ec51fd94d6cafe452 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 18 Feb 2026 18:06:06 +0100 Subject: [PATCH 17/83] rise_motion: ec_manager: Add transition_ec function. Function to transition ethercat state. --- .../include/rise_motion/ec_manager.hpp | 3 +- .../src/rise_motion/src/ec_manager.cpp | 40 ++++++++++++++++++- 2 files changed, 40 insertions(+), 3 deletions(-) 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 index fc2cc2a..b456c98 100755 --- 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 @@ -1,10 +1,10 @@ #pragma once #include #include -#include #include #include #include + #include "apsa.hpp" #define IOMAP_SIZE 4096 @@ -26,6 +26,7 @@ class ECManager { void set_motor_values(std::vector& motor_values); private: + void transition_ec(uint16 state); void transition_to_operational(); // EtherCAT context and configuration 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 index 1994849..4ed048e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -248,10 +248,46 @@ bool ECManager::get_motor_values_apsa(std::vector &motor_values) { return feedback_apsa.comm_read(motor_values); } - -bool ECManager::set_motor_values_apsa(const std::vector &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); } rclcpp::Logger ECManager::logger = rclcpp::get_logger("ECManager"); + +void ECManager::transition_ec(uint16 state) { + std::string state_string = "Unknown"; + switch (state) { + 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); + ecx_statecheck(&ctx, 0, state, EC_TIMEOUTSTATE * 4); + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + if (ctx.slavelist[0].state != state) { + RCLCPP_INFO(logger, "Not all nodes reached %d state", state); + ecx_readstate(&ctx); + for (int i = 1; i <= ctx.slavecount; i++) + { + if (ctx.slavelist[i].state != state) + { + RCLCPP_INFO(logger, "Node %d State=%2x StatusCode=%4x : %s", + i, ctx.slavelist[i].state, ctx.slavelist[i].ALstatuscode, ec_ALstatuscode2string(ctx.slavelist[i].ALstatuscode)); + } + } + std::exit(EXIT_FAILURE); + } +} From 9a31406a9b29e8a97bd9657ab7bbb69e8b48724c Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 18 Feb 2026 18:07:13 +0100 Subject: [PATCH 18/83] rise_motion: ec_manager.cpp: memset ctx and IOMap to 0. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 9 +++++++-- 1 file changed, 7 insertions(+), 2 deletions(-) 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 index 4ed048e..7e07a67 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -1,6 +1,7 @@ #include #include #include +#include #include #include #include @@ -13,15 +14,19 @@ // expected config, needs to be retrieved from config node struct { int slavecount = 1; - ec_slavet slavelist[1] = {{.name = "WRONG_NAME"}}; } config; ECManager::ECManager(const std::string interface) : interface(interface) {} void ECManager::init_ec() { int ret; + + memset(&ctx, 0, sizeof(ctx)); + memset(IOMap, 0, sizeof(IOMap)); + + ctx.packedMode = TRUE; // Not sure if necessary + RCLCPP_INFO(logger, "Connecting to %s", interface.c_str()); - ctx.packedMode = TRUE; ret = ecx_init(&ctx, interface.c_str()); if (ret <= 0) { RCLCPP_WARN(logger, "Couldn't initialize SOEM context"); From fce0c6826244a9ac9a53becd0d814597741683a4 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 18 Feb 2026 18:08:52 +0100 Subject: [PATCH 19/83] rise_motion: ec_manager.cpp: Add explicit ethercat transitions. --- .../src/rise_motion/src/ec_manager.cpp | 22 ++++++------------- 1 file changed, 7 insertions(+), 15 deletions(-) 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 index 7e07a67..40b3b43 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -42,34 +42,26 @@ void ECManager::init_ec() { if (ctx.slavecount != config.slavecount) { RCLCPP_WARN(logger, "Expected %d devices, but discovered %d", - config.slavecount, ctx.slavecount); + config.slavecount, ctx.slavecount); std::exit(EXIT_FAILURE); } + transition_ec(EC_STATE_PRE_OP); + RCLCPP_INFO(logger, "Mapping IO"); ret = ecx_config_map_group(&ctx, IOMap, 0); if (ret > IOMAP_SIZE) { RCLCPP_WARN(logger, "Couldn't map IO: Buffer to small"); std::exit(EXIT_FAILURE); } + RCLCPP_INFO(logger, "Using %d of %lu bytes in IOMap", ret, sizeof(IOMap)); expectedWKC = ctx.grouplist[0].outputsWKC * 2 + ctx.grouplist[0].inputsWKC; - RCLCPP_INFO(logger, "Configuring ditributed clock"); - ecx_configdc(&ctx); - ecx_statecheck(&ctx, 0, EC_STATE_SAFE_OP, EC_TIMEOUTSTATE * 4); + RCLCPP_INFO(logger, "Configuring distributed clock"); + ecx_configdc(&ctx); - // Check if nodes have valid outputs - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - // TODO: Check if nodes have valid outputs - for (int i = 1; i <= ctx.slavecount; i++) { - if (strcmp(config.slavelist[i-1].name, ctx.slavelist[i].name)) { - RCLCPP_WARN(logger, "Node %d: Name does not match: %s != %s", i, - config.slavelist[i-1].name, ctx.slavelist[i].name); - } - } - // Now all nodes should be in safe op + transition_ec(EC_STATE_SAFE_OP); } void ECManager::transition_to_operational() { From bf1fcd1f9a82adec3e14b2cd4ea0b128805a2500 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 18 Feb 2026 18:09:47 +0100 Subject: [PATCH 20/83] rise_motion: ec_manager.cpp: Cleanup. --- .../src/rise_motion/src/ec_manager.cpp | 194 +++++++++--------- 1 file changed, 100 insertions(+), 94 deletions(-) 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 index 40b3b43..7842def 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -66,7 +66,7 @@ void ECManager::init_ec() { void ECManager::transition_to_operational() { // Transitions Ethercat State Machine to operational - RCLCPP_INFO(logger, "Transitioning to operational mode"); + RCLCPP_INFO(logger, "To OPERATIONAL"); ctx.slavelist[0].state = EC_STATE_OPERATIONAL; ecx_writestate(&ctx, 0); @@ -81,9 +81,12 @@ void ECManager::transition_to_operational() { RCLCPP_WARN(logger, "Couldn't transition to operational"); std::exit(EXIT_FAILURE); } + RCLCPP_INFO(logger, "Exiting transition_to_operational"); } + void ECManager::cyclic_loop() { - transition_to_operational(); + // transition_to_operational(); + transition_ec(EC_STATE_OPERATIONAL); running_ = true; int wkc; auto next = std::chrono::steady_clock::now(); @@ -95,30 +98,32 @@ void ECManager::cyclic_loop() { // Setting ModeOfOperation to CyclicSyncPositionMode for (int i = 1; i <= ctx.slavecount; i++) { CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, - (CiA402_Outputs *)ctx.slavelist[i].outputs}; + (CiA402_Outputs *)ctx.slavelist[i].outputs}; m.set_mode_of_operation( - CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); } - // Transitioning CiA402 State Machine to OPERATION_ENABLED int flag = 1; RCLCPP_INFO(logger, "Going to operation_enabled"); while (flag) { flag = 0; for (int i = 1; i <= ctx.slavecount; i++) { - CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, - (CiA402_Outputs *)ctx.slavelist[i].outputs}; + CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; + CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; + CiA402Motor m{motor_inputs, motor_outputs}; RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); if (!m.get_state().has_value()) { flag = 1; - RCLCPP_INFO(logger, "Motor %d has no state", i); + RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - m.to_operation_enabled(); - flag = 1; - } else if (m.get_state().value() == CiA402Motor::State::FAULT) { - RCLCPP_INFO(logger, "Motor %d in fault. Fault handling not implemented. Exiting...", i); - std::exit(EXIT_FAILURE); + flag = 1; + if (m.get_state().value() == CiA402Motor::State::FAULT) { + RCLCPP_INFO(logger, "Motor %d in fault. Trying to recover...", i); + m.to_operation_enabled(); + } else { + m.to_operation_enabled(); + } } } ecx_send_processdata(&ctx); @@ -130,42 +135,6 @@ void ECManager::cyclic_loop() { while (running_) { next += period; - // perf_read() is wait-free - returns immediately if no new data - if (cmd_apsa.perf_read(motor_commands)) { - // New commands received! Apply them to EtherCAT nodes - for (int i = 1; i <= config.slavecount; i++) { - CiA402_Outputs *motor_outputs = - (CiA402_Outputs *)ctx.slavelist[i].outputs; - CiA402_Inputs *motor_inputs = - (CiA402_Inputs *)ctx.slavelist[i].inputs; - - // guard statement - if (abs(motor_commands[i-1] - motor_inputs->PositionValue) > 100) {continue;} - //write value - motor_outputs->TargetPosition = motor_commands[i-1]; - //RCLCPP_INFO(logger, "TargetPosition: %d, PositionValue: %d", motor_outputs->TargetPosition, motor_inputs->PositionValue); - 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", - motor_outputs->Controlword, motor_outputs->OpMode, - motor_outputs->TargetTorque, motor_outputs->TargetPosition, - motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, - motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, - motor_outputs->BitMask, motor_outputs->UserMOSI, - motor_outputs->VelocityOffset); - } - } - // If no new commands, EtherCAT nodes keep executing previous commands ecx_send_processdata(&ctx); wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); @@ -174,45 +143,73 @@ void ECManager::cyclic_loop() { RCLCPP_WARN(logger, "Not all nodes responded"); } - for (int i = 1; i <= ctx.slavecount; i++) { + // Iterate over connected drives + for (int i = 1; i <= config.slavecount; i++) { + CiA402_Outputs *motor_outputs = + (CiA402_Outputs *)ctx.slavelist[i].outputs; CiA402_Inputs *motor_inputs = - (CiA402_Inputs *)ctx.slavelist[i].inputs; + (CiA402_Inputs *)ctx.slavelist[i].inputs; + + if (cmd_apsa.perf_read(motor_commands)) { + // New commands received! Apply them to EtherCAT nodes + motor_outputs->TargetPosition = motor_commands[i-1]; + 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", + motor_outputs->Controlword, motor_outputs->OpMode, + motor_outputs->TargetTorque, motor_outputs->TargetPosition, + motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, + motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, + motor_outputs->BitMask, motor_outputs->UserMOSI, + motor_outputs->VelocityOffset); + } + motor_feedback[i-1] = motor_inputs->PositionValue; 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", - motor_inputs->Statusword, - motor_inputs->OpModeDisplay, - motor_inputs->PositionValue, - motor_inputs->VelocityValue, - motor_inputs->TorqueValue, - motor_inputs->AnalogInput1, - motor_inputs->AnalogInput2, - motor_inputs->AnalogInput3, - motor_inputs->AnalogInput4, - motor_inputs->TuningStatus, - motor_inputs->DigitalInputs, - motor_inputs->UserMISO, - motor_inputs->Timestamp, - motor_inputs->PositionDemandInternalValue, - motor_inputs->VelocityDemandValue, - motor_inputs->TorqueDemand - ); + "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", + motor_inputs->Statusword, + motor_inputs->OpModeDisplay, + motor_inputs->PositionValue, + motor_inputs->VelocityValue, + motor_inputs->TorqueValue, + motor_inputs->AnalogInput1, + motor_inputs->AnalogInput2, + motor_inputs->AnalogInput3, + motor_inputs->AnalogInput4, + motor_inputs->TuningStatus, + motor_inputs->DigitalInputs, + motor_inputs->UserMISO, + motor_inputs->Timestamp, + motor_inputs->PositionDemandInternalValue, + motor_inputs->VelocityDemandValue, + motor_inputs->TorqueDemand + ); } // Make feedback available to ROS publisher (wait-free) @@ -225,16 +222,25 @@ void ECManager::cyclic_loop() { RCLCPP_INFO(logger, "Exiting cyclic loop"); // Shut down motors - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, - (CiA402_Outputs *)ctx.slavelist[i].outputs}; - m.to_switch_on_disabled(); - } - ecx_send_processdata(&ctx); - wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + while (flag) { + flag = 0; + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; + CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; + CiA402Motor m{motor_inputs, motor_outputs}; - if (wkc != expectedWKC) { - RCLCPP_WARN(logger, "Not all nodes responded"); + RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + flag = 1; + RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); + } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + m.to_switch_on_disabled(); + flag = 1; + } + } + transition_ec(EC_STATE_PRE_OP); + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); } } From 56f9bdaf9d15ecc81b3074879fb0061fbde21c99 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Mon, 23 Feb 2026 09:35:09 +0100 Subject: [PATCH 21/83] rise_motion: ec_manager.cpp: Quality of Life Improvements - Headers sorted - Move to transition_ec - Remove potentially unnecessary send/recvs - Add some blocks to make scope of variables clear - Update Shutdown logic --- .../include/rise_motion/ec_manager.hpp | 2 +- .../src/rise_motion/src/ec_manager.cpp | 220 ++++++++++-------- 2 files changed, 122 insertions(+), 100 deletions(-) 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 index b456c98..dd8f3ec 100755 --- 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 @@ -26,7 +26,7 @@ class ECManager { void set_motor_values(std::vector& motor_values); private: - void transition_ec(uint16 state); + uint16 transition_ec(uint16 state); void transition_to_operational(); // EtherCAT context and configuration 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 index 7842def..746df6f 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -2,14 +2,16 @@ #include #include #include +#include +#include +#include + #include #include #include #include #include -#include -#include -#include + // expected config, needs to be retrieved from config node struct { @@ -24,69 +26,58 @@ void ECManager::init_ec() { memset(&ctx, 0, sizeof(ctx)); memset(IOMap, 0, sizeof(IOMap)); - ctx.packedMode = TRUE; // Not sure if necessary - RCLCPP_INFO(logger, "Connecting to %s", interface.c_str()); ret = ecx_init(&ctx, interface.c_str()); if (ret <= 0) { - RCLCPP_WARN(logger, "Couldn't initialize SOEM context"); + RCLCPP_ERROR(logger, "Couldn't initialize SOEM context"); std::exit(EXIT_FAILURE); } RCLCPP_INFO(logger, "Discovering EC Nodes"); - ret = ecx_config_init(&ctx); + ret = ecx_config_init(&ctx); // also requests PreOP state if (ret <= 0) { - RCLCPP_WARN(logger, "EC Nodes Discovery failed"); + RCLCPP_ERROR(logger, "EC Nodes Discovery failed"); std::exit(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"); + std::exit(EXIT_FAILURE); + } + + // TODO: More extensive verification of network if (ctx.slavecount != config.slavecount) { - RCLCPP_WARN(logger, "Expected %d devices, but discovered %d", + RCLCPP_ERROR(logger, "Expected %d devices, but discovered %d", config.slavecount, ctx.slavecount); std::exit(EXIT_FAILURE); } - transition_ec(EC_STATE_PRE_OP); - RCLCPP_INFO(logger, "Mapping IO"); - ret = ecx_config_map_group(&ctx, IOMap, 0); + ret = ecx_config_map_group(&ctx, IOMap, 0); // also requests SafeOP state if (ret > IOMAP_SIZE) { - RCLCPP_WARN(logger, "Couldn't map IO: Buffer to small"); + RCLCPP_ERROR(logger, "Couldn't map IO: Buffer to small"); std::exit(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"); + std::exit(EXIT_FAILURE); + } + RCLCPP_INFO(logger, "Using %d of %lu bytes in IOMap", ret, sizeof(IOMap)); expectedWKC = ctx.grouplist[0].outputsWKC * 2 + ctx.grouplist[0].inputsWKC; RCLCPP_INFO(logger, "Configuring distributed clock"); ecx_configdc(&ctx); - - transition_ec(EC_STATE_SAFE_OP); -} - -void ECManager::transition_to_operational() { - // Transitions Ethercat State Machine to operational - RCLCPP_INFO(logger, "To OPERATIONAL"); - ctx.slavelist[0].state = EC_STATE_OPERATIONAL; - ecx_writestate(&ctx, 0); - - // check if nodes entered operational mode - int chk = 200; - do { - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - ecx_statecheck(&ctx, 0, EC_STATE_OPERATIONAL, 50000); - } while (chk-- && (ctx.slavelist[0].state != EC_STATE_OPERATIONAL)); - if (ctx.slavelist[0].state != EC_STATE_OPERATIONAL) { - RCLCPP_WARN(logger, "Couldn't transition to operational"); - std::exit(EXIT_FAILURE); - } - RCLCPP_INFO(logger, "Exiting transition_to_operational"); } void ECManager::cyclic_loop() { - // transition_to_operational(); - transition_ec(EC_STATE_OPERATIONAL); + // Still in SAFE_OP, PDO transmission is available running_ = true; int wkc; auto next = std::chrono::steady_clock::now(); @@ -95,6 +86,7 @@ void ECManager::cyclic_loop() { std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); + // Configuring Drives // Setting ModeOfOperation to CyclicSyncPositionMode for (int i = 1; i <= ctx.slavecount; i++) { CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, @@ -102,37 +94,44 @@ void ECManager::cyclic_loop() { m.set_mode_of_operation( CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); } + // Transitioning CiA402 State Machine to OPERATION_ENABLED - int flag = 1; - RCLCPP_INFO(logger, "Going to operation_enabled"); - while (flag) { - flag = 0; - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; - CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; - CiA402Motor m{motor_inputs, motor_outputs}; - - RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); - if (!m.get_state().has_value()) { - flag = 1; - RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); - } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - flag = 1; - if (m.get_state().value() == CiA402Motor::State::FAULT) { - RCLCPP_INFO(logger, "Motor %d in fault. Trying to recover...", i); - m.to_operation_enabled(); - } else { - m.to_operation_enabled(); + { + int flag = 1; + RCLCPP_INFO(logger, "Going to operation_enabled"); + while (flag) { + flag = 0; + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; + CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; + CiA402Motor m{motor_inputs, motor_outputs}; + + RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + flag = 1; + RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); + } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + flag = 1; + if (m.get_state().value() == CiA402Motor::State::FAULT) { + RCLCPP_INFO(logger, "Motor %d in fault. Trying to recover...", i); + m.to_operation_enabled(); + } else { + m.to_operation_enabled(); + } } } + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); } - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + RCLCPP_INFO(logger, "All motors in operation_enabled"); } - RCLCPP_INFO(logger, "All motors in operation_enabled"); + // Transition to OPERATIONAL + uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); + if (reached_state != EC_STATE_OPERATIONAL) { + std::exit(EXIT_FAILURE); + } RCLCPP_INFO(logger, "Entering Cyclic Loop"); - while (running_) { next += period; @@ -140,11 +139,11 @@ void ECManager::cyclic_loop() { wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); if (wkc != expectedWKC) { - RCLCPP_WARN(logger, "Not all nodes responded"); + RCLCPP_ERROR(logger, "Not all nodes responded"); } // Iterate over connected drives - for (int i = 1; i <= config.slavecount; i++) { + for (int i = 1; i <= ctx.slavecount; i++) { CiA402_Outputs *motor_outputs = (CiA402_Outputs *)ctx.slavelist[i].outputs; CiA402_Inputs *motor_inputs = @@ -221,27 +220,43 @@ void ECManager::cyclic_loop() { RCLCPP_INFO(logger, "Exiting cyclic loop"); - // Shut down motors - while (flag) { - flag = 0; - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; - CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; - CiA402Motor m{motor_inputs, motor_outputs}; - - RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); - if (!m.get_state().has_value()) { - flag = 1; - RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); - } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - m.to_switch_on_disabled(); - flag = 1; + // Transitioning Motors to SWITCH_ON_DISABLED + { + int flag = 1; + while (flag) { + next += period; + flag = 0; + for (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; + CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; + CiA402Motor m{motor_inputs, motor_outputs}; + + RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + flag = 1; + RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, motor_inputs->Statusword); + } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + m.to_switch_on_disabled(); + flag = 1; + } } + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + std::this_thread::sleep_until(next); } - transition_ec(EC_STATE_PRE_OP); - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); } + + /* Go to PRE_OP */ + transition_ec(EC_STATE_PRE_OP); + + /* Go to SAFE_OP */ + transition_ec(EC_STATE_SAFE_OP); + + /* Go to INIT state */ + transition_ec(EC_STATE_INIT); + + + ecx_close(&ctx); } void ECManager::stop() { running_ = false; } @@ -259,38 +274,45 @@ bool ECManager::set_motor_values_apsa( rclcpp::Logger ECManager::logger = rclcpp::get_logger("ECManager"); -void ECManager::transition_ec(uint16 state) { +uint16 ECManager::transition_ec(uint16 state) { + // Get state_string std::string state_string = "Unknown"; - switch (state) { - 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; + { + switch (state) { + 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); - ecx_statecheck(&ctx, 0, state, EC_TIMEOUTSTATE * 4); - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - if (ctx.slavelist[0].state != state) { - RCLCPP_INFO(logger, "Not all nodes reached %d state", state); + int chk = 200; + uint16 reached_state; + do { + ecx_send_processdata(&ctx); + ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + reached_state = ecx_statecheck(&ctx, 0, state, EC_TIMEOUTSTATE * 4); + } 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_INFO(logger, "Node %d State=%2x StatusCode=%4x : %s", + 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)); } } - std::exit(EXIT_FAILURE); } + return reached_state; } From 2522f7904eb4861f779488f1dab258c65c49b77f Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 11:49:56 +0100 Subject: [PATCH 22/83] ec_manager: Add run method. --- .../src/rise_motion/include/rise_motion/ec_manager.hpp | 2 ++ rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 5 +++++ rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 3 +-- 3 files changed, 8 insertions(+), 2 deletions(-) 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 index dd8f3ec..d6960cb 100755 --- 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 @@ -13,6 +13,8 @@ class ECManager { public: ECManager(); ECManager(const std::string interface); + + void run(); void init_ec(); void cyclic_loop(); void stop(); 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 index 746df6f..18df696 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -20,6 +20,11 @@ struct { ECManager::ECManager(const std::string interface) : interface(interface) {} +void ECManager::run() { + init_ec(); + cyclic_loop(); +} + void ECManager::init_ec() { int ret; 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 index 8f8eb38..ed0523d 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -76,8 +76,7 @@ void EthercatNode::enableServiceCallback( { if (request->enable && !ethercat_enabled_) { RCLCPP_INFO(get_logger(), "Enabling EtherCAT communication"); - ec_manager_.init_ec(); - ec_thread_ = std::make_unique(&ECManager::cyclic_loop, &ec_manager_); + ec_thread_ = std::make_unique(&ECManager::run, &ec_manager_); ethercat_enabled_ = true; } else if (!request->enable && ethercat_enabled_) { From d1ad34f4aef62bb89158c48ce1679671d616ec4e Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 11:50:57 +0100 Subject: [PATCH 23/83] ec_manager: Fine tuning. --- .../src/rise_motion/src/ec_manager.cpp | 37 +++++++++++-------- 1 file changed, 21 insertions(+), 16 deletions(-) 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 index 18df696..67abdaf 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -100,25 +100,33 @@ void ECManager::cyclic_loop() { CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); } + // 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) { + std::exit(EXIT_FAILURE); + } + // Transitioning CiA402 State Machine to OPERATION_ENABLED { int flag = 1; RCLCPP_INFO(logger, "Going to operation_enabled"); while (flag) { + next += period; flag = 0; for (int i = 1; i <= ctx.slavecount; i++) { CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; CiA402Motor m{motor_inputs, motor_outputs}; - RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + RCLCPP_INFO(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); if (!m.get_state().has_value()) { flag = 1; - RCLCPP_INFO(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); + RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { flag = 1; if (m.get_state().value() == CiA402Motor::State::FAULT) { - RCLCPP_INFO(logger, "Motor %d in fault. Trying to recover...", i); + RCLCPP_WARN(logger, "Motor %d in fault. Trying to recover...", i); m.to_operation_enabled(); } else { m.to_operation_enabled(); @@ -127,15 +135,11 @@ void ECManager::cyclic_loop() { } ecx_send_processdata(&ctx); ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + std::this_thread::sleep_until(next); } RCLCPP_INFO(logger, "All motors in operation_enabled"); } - // Transition to OPERATIONAL - uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); - if (reached_state != EC_STATE_OPERATIONAL) { - std::exit(EXIT_FAILURE); - } RCLCPP_INFO(logger, "Entering Cyclic Loop"); while (running_) { next += period; @@ -236,11 +240,11 @@ void ECManager::cyclic_loop() { CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; CiA402Motor m{motor_inputs, motor_outputs}; - RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); + RCLCPP_INFO(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); if (!m.get_state().has_value()) { flag = 1; RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, motor_inputs->Statusword); - } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + } else if (m.get_state().value() != CiA402Motor::State::SWITCH_ON_DISABLED) { m.to_switch_on_disabled(); flag = 1; } @@ -251,16 +255,10 @@ void ECManager::cyclic_loop() { } } - /* Go to PRE_OP */ transition_ec(EC_STATE_PRE_OP); - - /* Go to SAFE_OP */ transition_ec(EC_STATE_SAFE_OP); - - /* Go to INIT state */ transition_ec(EC_STATE_INIT); - ecx_close(&ctx); } @@ -284,6 +282,9 @@ uint16 ECManager::transition_ec(uint16 state) { 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; @@ -302,10 +303,14 @@ uint16 ECManager::transition_ec(uint16 state) { ecx_writestate(&ctx, 0); int chk = 200; uint16 reached_state; + auto next = std::chrono::steady_clock::now(); + auto period = std::chrono::milliseconds(1); 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()); From adf31051eca3f2de3cfa80b10c79d0903ee7eeec Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 11:51:14 +0100 Subject: [PATCH 24/83] cia402.cpp: Complete switch_on_disabled logic. --- rise_motion_dev_ws/src/rise_motion/src/cia402.cpp | 8 ++++++-- 1 file changed, 6 insertions(+), 2 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp index e25306c..9470aa7 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -70,16 +70,20 @@ void CiA402Motor::to_switch_on_disabled() { 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::FAULT: next_op = Operation::FAULT_RESET; // Remove in Production break; + case State::READY_TO_SWITCH_ON: + next_op = Operation::DISABLE_VOLTAGE; + break; case State::FAULT_REACTION_ACTIVE: case State::SWITCH_ON_DISABLED: - case State::READY_TO_SWITCH_ON: - case State::SWITCHED_ON: default: return; } From f7c241e2b85a4a30aeb86f2478d11c0fc04216ea Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 11:51:43 +0100 Subject: [PATCH 25/83] ethercat_node.cpp: Remove unnecessary extensions. --- rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index ed0523d..314680e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -21,7 +21,7 @@ EthercatNode::EthercatNode(ECManager& ec_manager) enable_srv_ = create_service( "enable_ethercat", - std::bind(&EthercatNode::enableServiceCallback, this, std::placeholders::_1, std::placeholders::_2)); + std::bind(&EthercatNode::enableServiceCallback, this, _1, _2)); RCLCPP_INFO(get_logger(), "EtherCAT node initialized"); } From 249cd2fb5f95e107f36fc43f064fe7ecac88a790 Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 12:18:17 +0100 Subject: [PATCH 26/83] Add build script. The script must be run from the workspace folder. --- rise_motion_dev_ws/build.sh | 3 +++ 1 file changed, 3 insertions(+) create mode 100644 rise_motion_dev_ws/build.sh diff --git a/rise_motion_dev_ws/build.sh b/rise_motion_dev_ws/build.sh new file mode 100644 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 From 019c91c4c39f7aa0caae79f635901f7421c77ba3 Mon Sep 17 00:00:00 2001 From: simeon Date: Mon, 23 Feb 2026 12:19:25 +0100 Subject: [PATCH 27/83] build.sh: Update permissions. --- rise_motion_dev_ws/build.sh | 0 1 file changed, 0 insertions(+), 0 deletions(-) mode change 100644 => 100755 rise_motion_dev_ws/build.sh diff --git a/rise_motion_dev_ws/build.sh b/rise_motion_dev_ws/build.sh old mode 100644 new mode 100755 From 1a88f00850014b0f315a444f718fc9b566764a0d Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:36:30 +0100 Subject: [PATCH 28/83] Implement better ethercat startup logic. ROS Node calls init_ec and checks return code before starting cylic loop. --- .../include/rise_motion/ec_manager.hpp | 3 +-- .../src/rise_motion/src/ec_manager.cpp | 20 ++++++++----------- .../src/rise_motion/src/ethercat_node.cpp | 12 +++++++---- 3 files changed, 17 insertions(+), 18 deletions(-) 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 index d6960cb..c905d15 100755 --- 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 @@ -14,8 +14,7 @@ class ECManager { ECManager(); ECManager(const std::string interface); - void run(); - void init_ec(); + int init_ec(); void cyclic_loop(); void stop(); 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 index 67abdaf..c6184c5 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -20,12 +20,7 @@ struct { ECManager::ECManager(const std::string interface) : interface(interface) {} -void ECManager::run() { - init_ec(); - cyclic_loop(); -} - -void ECManager::init_ec() { +int ECManager::init_ec() { int ret; memset(&ctx, 0, sizeof(ctx)); @@ -35,42 +30,42 @@ void ECManager::init_ec() { ret = ecx_init(&ctx, interface.c_str()); if (ret <= 0) { RCLCPP_ERROR(logger, "Couldn't initialize SOEM context"); - std::exit(EXIT_FAILURE); + 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"); - std::exit(EXIT_FAILURE); + 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"); - std::exit(EXIT_FAILURE); + 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); - std::exit(EXIT_FAILURE); + 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"); - std::exit(EXIT_FAILURE); + 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"); - std::exit(EXIT_FAILURE); + return EXIT_FAILURE; } RCLCPP_INFO(logger, "Using %d of %lu bytes in IOMap", ret, sizeof(IOMap)); @@ -79,6 +74,7 @@ void ECManager::init_ec() { RCLCPP_INFO(logger, "Configuring distributed clock"); ecx_configdc(&ctx); + return EXIT_SUCCESS; } void ECManager::cyclic_loop() { 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 index 314680e..96a495e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -76,10 +76,14 @@ void EthercatNode::enableServiceCallback( { if (request->enable && !ethercat_enabled_) { RCLCPP_INFO(get_logger(), "Enabling EtherCAT communication"); - ec_thread_ = std::make_unique(&ECManager::run, &ec_manager_); - ethercat_enabled_ = true; - } - else if (!request->enable && ethercat_enabled_) { + if (ec_manager_.init_ec() == EXIT_FAILURE) { + RCLCPP_ERROR(get_logger(), "Couldn't init ethercat"); + } else { + ec_thread_ = + std::make_unique(&ECManager::cyclic_loop, &ec_manager_); + 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()) { From 23dadbd09017fb849141d13e1e8b56b15ec0dcc7 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:37:55 +0100 Subject: [PATCH 29/83] testing_node: Don't start publishing before valid pos. Also add a cmd line argument for controlling increment count. --- .../src/rise_motion/src/testing_node.cpp | 49 +++++++++++++------ 1 file changed, 33 insertions(+), 16 deletions(-) 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 index e99b669..d7c51e7 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/testing_node.cpp @@ -1,34 +1,40 @@ +#include #include #include +#include #include #include #include #include #include -const int increment = 20; + class TestNode : public rclcpp::Node { public: - TestNode() : Node("test_node") { + 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) { - auto const &motor_pos = msg.positions; - - // print_motor_positions(motor_pos, "Received"); - - 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; + if (!valid_positions) { + RCLCPP_INFO(get_logger(), "Got feedback"); + valid_positions = true; } - - output_pub->publish(response); - // print_motor_positions(response.positions, "Published"); + 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( @@ -37,7 +43,9 @@ class TestNode : public rclcpp::Node { 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"); @@ -61,10 +69,12 @@ class TestNode : public rclcpp::Node { 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; @@ -80,6 +90,8 @@ class TestNode : public rclcpp::Node { 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 @@ -87,15 +99,20 @@ class TestNode : public rclcpp::Node { 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(); + auto node = std::make_shared(incs); while (!node->request_enable_ethercat()) { } - rclcpp::spin(node); rclcpp::shutdown(); return 0; From 408bf886e802087b632fbbb4b9ec4637a497be40 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:40:43 +0100 Subject: [PATCH 30/83] ec_manager.cpp: Move configuration after operational EC. Also set the target position to the current position. This avoids sudden movement on startup. --- .../src/rise_motion/src/ec_manager.cpp | 24 ++++++++++++------- 1 file changed, 15 insertions(+), 9 deletions(-) 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 index c6184c5..5298c1b 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -87,15 +87,6 @@ void ECManager::cyclic_loop() { std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); - // Configuring Drives - // Setting ModeOfOperation to CyclicSyncPositionMode - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402Motor m{(CiA402_Inputs *)ctx.slavelist[i].inputs, - (CiA402_Outputs *)ctx.slavelist[i].outputs}; - m.set_mode_of_operation( - CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); - } - // Transition to OPERATIONAL // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); @@ -103,6 +94,21 @@ void ECManager::cyclic_loop() { std::exit(EXIT_FAILURE); } + // Configuring Drives + 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}; + // Setting ModeOfOperation to CyclicSyncPositionMode + m.set_mode_of_operation(CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + // Set Position to Current Position + motor_commands[i-1] = motor_inputs->PositionValue; + motor_outputs->TargetPosition = motor_inputs->PositionValue; + RCLCPP_INFO(logger, "Configured Motor %d: (%d)", i, motor_inputs->PositionValue); + } + // Transitioning CiA402 State Machine to OPERATION_ENABLED { int flag = 1; From 1fdcbfe490324628eae30de4d67ecb391b889edb Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:42:35 +0100 Subject: [PATCH 31/83] ec_manager.cpp: Disable auto fault recovery. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) 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 index 5298c1b..71bf3a7 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -128,8 +128,10 @@ void ECManager::cyclic_loop() { } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { flag = 1; if (m.get_state().value() == CiA402Motor::State::FAULT) { - RCLCPP_WARN(logger, "Motor %d in fault. Trying to recover...", i); - m.to_operation_enabled(); + RCLCPP_ERROR(logger, "Motor %d in fault. Exiting...", i); + exit(EXIT_FAILURE); +// RCLCPP_WARN(logger, "Motor %d in fault. Trying to recover...", i); +// m.to_operation_enabled(); } else { m.to_operation_enabled(); } From ab6ba2521052f681aa86e8dde4f9250b336c268a Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:43:32 +0100 Subject: [PATCH 32/83] ec_manager.cpp: Monitor CiA402 State during cyclic loop. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 7 +++++++ 1 file changed, 7 insertions(+) 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 index 71bf3a7..c1bd311 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -161,6 +161,13 @@ void ECManager::cyclic_loop() { (CiA402_Outputs *)ctx.slavelist[i].outputs; CiA402_Inputs *motor_inputs = (CiA402_Inputs *)ctx.slavelist[i].inputs; + CiA402Motor m{motor_inputs, motor_outputs}; + + if (!m.get_state().has_value()) { + RCLCPP_ERROR(logger, "Motor %d has no state", i); + } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { + RCLCPP_ERROR(logger, "Motor %d is not in OPERATION_ENABLED", i); + } if (cmd_apsa.perf_read(motor_commands)) { // New commands received! Apply them to EtherCAT nodes From a128608ec5007cbfdd322d07320d6aa7ea2a9699 Mon Sep 17 00:00:00 2001 From: simeon Date: Wed, 4 Mar 2026 16:44:35 +0100 Subject: [PATCH 33/83] ec_manager.cpp: Set TargetPosition even if no new data available. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index c1bd311..2b5888c 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -192,7 +192,7 @@ void ECManager::cyclic_loop() { motor_outputs->BitMask, motor_outputs->UserMOSI, motor_outputs->VelocityOffset); } - + motor_outputs->TargetPosition = motor_commands[i-1]; motor_feedback[i-1] = motor_inputs->PositionValue; RCLCPP_DEBUG(logger, "Motor Inputs:\n" From 67ace1b172a5d43e3c754deb2516cacd340e657e Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:41:34 +0100 Subject: [PATCH 34/83] Add SDOs --- .../include/rise_motion/ethercat_node.hpp | 15 +++++ .../src/rise_motion/src/ec_manager.cpp | 41 +++++++++++++ .../src/rise_motion/src/ethercat_node.cpp | 60 +++++++++++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 2 + .../rise_motion_messages/srv/SDOReadSrv.srv | 11 ++++ .../rise_motion_messages/srv/SDOWriteSrv.srv | 11 ++++ 6 files changed, 140 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/srv/SDOReadSrv.srv create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/srv/SDOWriteSrv.srv 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 index 2e41427..401e911 100755 --- 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 @@ -4,6 +4,8 @@ #include #include #include +#include +#include #include class EthercatNode : public rclcpp::Node { @@ -20,10 +22,23 @@ class EthercatNode : public rclcpp::Node { rclcpp::Publisher::SharedPtr feedback_pub_; rclcpp::TimerBase::SharedPtr feedback_timer_; rclcpp::Service::SharedPtr enable_srv_; + rclcpp::Service::SharedPtr sdo_read_srv_; + rclcpp::Service::SharedPtr sdo_write_srv_; + void commandCallback(const rise_motion_messages::msg::MotorPositions::SharedPtr msg); void publishFeedback(); 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); }; 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 index 2b5888c..2ed9c58 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -70,6 +70,14 @@ int ECManager::init_ec() { 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"); @@ -150,6 +158,7 @@ void ECManager::cyclic_loop() { ecx_send_processdata(&ctx); wkc = ecx_receive_processdata(&ctx, EC_TIMEOUTRET); + ecx_mbxhandler(&ctx, 0, 4); if (wkc != expectedWKC) { RCLCPP_ERROR(logger, "Not all nodes responded"); @@ -337,3 +346,35 @@ uint16 ECManager::transition_ec(uint16 state) { } return reached_state; } + +bool ECManager::sdo_read(uint16 device_id, uint16 index, uint8 subindex, + std::vector &value) { + int psize = 64; + uint8 *buf = new uint8[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 device_id, uint16 index, uint8 subindex, + std::vector &value) { + int psize = value.size(); + uint8 *buf = new uint8[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; +} + 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 index 96a495e..b09b038 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -22,6 +22,13 @@ EthercatNode::EthercatNode(ECManager& ec_manager) enable_srv_ = create_service( "enable_ethercat", std::bind(&EthercatNode::enableServiceCallback, 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"); } @@ -94,3 +101,56 @@ void EthercatNode::enableServiceCallback( 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::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_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index b4f5fa9..f27e9c1 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -14,6 +14,8 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateNoticeMsg.msg" "msg/MotorPositions.msg" "srv/EnableEthercatSrv.srv" + "srv/SDOReadSrv.srv" + "srv/SDOWriteSrv.srv" ) if(BUILD_TESTING) 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 From df521b214a400dcb9647ed7fbb1808936b59b595 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:43:02 +0100 Subject: [PATCH 35/83] rise_motion: cia402: Add transition_to. --- .../rise_motion/include/rise_motion/cia402.hpp | 2 ++ .../src/rise_motion/src/cia402.cpp | 18 ++++++++++++++++++ 2 files changed, 20 insertions(+) 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 index 8b25d53..8b54d5c 100755 --- 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 @@ -1,3 +1,4 @@ +#pragma once #include #include #include @@ -104,6 +105,7 @@ class CiA402Motor { 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); diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp index 9470aa7..f2acd3f 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -23,6 +23,24 @@ bool CiA402Motor::is_state(State s) const { } } +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); } From 51e596fbd67b0779e6015f091b9eb21c8b19ec3c Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:43:56 +0100 Subject: [PATCH 36/83] rise_motion: cia402: Remove auto fault reset. --- rise_motion_dev_ws/src/rise_motion/src/cia402.cpp | 6 +----- 1 file changed, 1 insertion(+), 5 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp index f2acd3f..cbf79a1 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/cia402.cpp @@ -64,8 +64,6 @@ void CiA402Motor::to_operation_enabled() { next_op = Operation::ENABLE_OPERATION; break; case State::FAULT: - next_op = Operation::FAULT_RESET; // Remove in Production - break; case State::OPERATION_ENABLED: case State::QUICK_STOP_ACTIVE: case State::FAULT_REACTION_ACTIVE: @@ -94,12 +92,10 @@ void CiA402Motor::to_switch_on_disabled() { case State::QUICK_STOP_ACTIVE: next_op = Operation::SHUTDOWN; break; - case State::FAULT: - next_op = Operation::FAULT_RESET; // Remove in Production - 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: From d71992b0d65d617628f3a2febfd5d85a521a693e Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:48:17 +0100 Subject: [PATCH 37/83] ec_manager: Move cycle_time + logger into initializer. Also make next and period properties. --- .../rise_motion/include/rise_motion/ec_manager.hpp | 7 +++++-- .../src/rise_motion/src/ec_manager.cpp | 13 ++++++------- 2 files changed, 11 insertions(+), 9 deletions(-) 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 index c905d15..29f5959 100755 --- 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 @@ -12,7 +12,7 @@ class ECManager { public: ECManager(); - ECManager(const std::string interface); + ECManager(const std::string interface, int cycle_period_ms); int init_ec(); void cyclic_loop(); @@ -36,7 +36,10 @@ class ECManager { uint8_t IOMap[IOMAP_SIZE]; std::atomic running_{false}; const std::string interface; - static rclcpp::Logger logger; + 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 (motor commands) 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 index 2ed9c58..f3bf6a7 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -18,7 +18,10 @@ struct { int slavecount = 1; } config; -ECManager::ECManager(const std::string interface) : interface(interface) {} +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; @@ -87,10 +90,9 @@ int ECManager::init_ec() { void ECManager::cyclic_loop() { // Still in SAFE_OP, PDO transmission is available + next = std::chrono::steady_clock::now(); running_ = true; int wkc; - auto next = std::chrono::steady_clock::now(); - auto period = std::chrono::milliseconds(1); std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); @@ -295,8 +297,6 @@ bool ECManager::set_motor_values_apsa( return cmd_apsa.comm_write(motor_values); } -rclcpp::Logger ECManager::logger = rclcpp::get_logger("ECManager"); - uint16 ECManager::transition_ec(uint16 state) { // Get state_string std::string state_string = "Unknown"; @@ -323,8 +323,7 @@ uint16 ECManager::transition_ec(uint16 state) { ecx_writestate(&ctx, 0); int chk = 200; uint16 reached_state; - auto next = std::chrono::steady_clock::now(); - auto period = std::chrono::milliseconds(1); + next = std::chrono::steady_clock::now(); do { next += period; ecx_send_processdata(&ctx); From 89273d52a969593e36548de10da03b5005a2f51e Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:50:04 +0100 Subject: [PATCH 38/83] ec_manager.hpp: Add missing declarations. Remove deprecated declarations. --- .../src/rise_motion/include/rise_motion/ec_manager.hpp | 6 +++--- 1 file changed, 3 insertions(+), 3 deletions(-) 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 index 29f5959..a1c588c 100755 --- 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 @@ -22,9 +22,9 @@ class ECManager { bool get_motor_values_apsa(std::vector& motor_values); bool set_motor_values_apsa(const std::vector& motor_values); - // Legacy mutex-based methods (deprecated) - void get_motor_values(std::vector& motor_values); - void set_motor_values(std::vector& motor_values); + // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread + bool sdo_read(uint16 device_id, uint16 index, uint8 subindex, std::vector& value); + bool sdo_write(uint16 device_id, uint16 index, uint8 subindex, std::vector& value); private: uint16 transition_ec(uint16 state); From 6196e50f6c14ffdde1088227f04239d6d690d67f Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:52:41 +0100 Subject: [PATCH 39/83] ec_manager.cpp: Add transition_motors_to. --- .../include/rise_motion/ec_manager.hpp | 2 +- .../src/rise_motion/src/ec_manager.cpp | 101 ++++++++---------- 2 files changed, 45 insertions(+), 58 deletions(-) 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 index a1c588c..74abc56 100755 --- 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 @@ -28,7 +28,7 @@ class ECManager { private: uint16 transition_ec(uint16 state); - void transition_to_operational(); + bool transition_motors_to(CiA402Motor::State state); // EtherCAT context and configuration int expectedWKC; 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 index f3bf6a7..99d1a9b 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -120,38 +120,9 @@ void ECManager::cyclic_loop() { } // Transitioning CiA402 State Machine to OPERATION_ENABLED - { - int flag = 1; - RCLCPP_INFO(logger, "Going to operation_enabled"); - while (flag) { - next += period; - flag = 0; - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; - CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; - CiA402Motor m{motor_inputs, motor_outputs}; - - RCLCPP_INFO(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); - if (!m.get_state().has_value()) { - flag = 1; - RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, ((CiA402_Inputs *)ctx.slavelist[i].inputs)->Statusword); - } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - flag = 1; - if (m.get_state().value() == CiA402Motor::State::FAULT) { - RCLCPP_ERROR(logger, "Motor %d in fault. Exiting...", i); - exit(EXIT_FAILURE); -// RCLCPP_WARN(logger, "Motor %d in fault. Trying to recover...", i); -// m.to_operation_enabled(); - } else { - m.to_operation_enabled(); - } - } - } - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - std::this_thread::sleep_until(next); - } - RCLCPP_INFO(logger, "All motors in operation_enabled"); + if (!transition_motors_to(CiA402Motor::State::OPERATION_ENABLED)) { + RCLCPP_ERROR(logger, "Couldn't transition all motors to OPERATION_ENABLED"); + goto shutdown; } RCLCPP_INFO(logger, "Entering Cyclic Loop"); @@ -249,32 +220,11 @@ void ECManager::cyclic_loop() { std::this_thread::sleep_until(next); } +shutdown: RCLCPP_INFO(logger, "Exiting cyclic loop"); - - // Transitioning Motors to SWITCH_ON_DISABLED - { - int flag = 1; - while (flag) { - next += period; - flag = 0; - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402_Inputs * motor_inputs = (CiA402_Inputs*)ctx.slavelist[i].inputs; - CiA402_Outputs * motor_outputs = (CiA402_Outputs*)ctx.slavelist[i].outputs; - CiA402Motor m{motor_inputs, motor_outputs}; - - RCLCPP_INFO(logger, "State of Motor %d: %s", i, m.state_as_string().c_str()); - if (!m.get_state().has_value()) { - flag = 1; - RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, motor_inputs->Statusword); - } else if (m.get_state().value() != CiA402Motor::State::SWITCH_ON_DISABLED) { - m.to_switch_on_disabled(); - flag = 1; - } - } - ecx_send_processdata(&ctx); - ecx_receive_processdata(&ctx, EC_TIMEOUTRET); - std::this_thread::sleep_until(next); - } + 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); @@ -377,3 +327,40 @@ bool ECManager::sdo_write(uint16 device_id, uint16 index, uint8 subindex, 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 (int i = 1; i <= ctx.slavecount; i++) { + CiA402_Inputs *motor_inputs = (CiA402_Inputs *)ctx.slavelist[i].inputs; + CiA402_Outputs *motor_outputs = + (CiA402_Outputs *)ctx.slavelist[i].outputs; + CiA402Motor m{motor_inputs, motor_outputs}; + RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, + m.state_as_string().c_str()); + if (!m.get_state().has_value()) { + continue_flag = 1; + RCLCPP_WARN(logger, "Motor %d has no decodable state: 0x%04X", i, + motor_inputs->Statusword); + } else if (m.get_state().value() == CiA402Motor::State::FAULT) { + RCLCPP_ERROR(logger, "Motor %d in fault", i); + return false; + } 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; +} From 5507795fc17b3a715776b6f37d6cac59cccc7036 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:54:46 +0100 Subject: [PATCH 40/83] ec_manager.cpp: Use goto instead of exit for graceful shutdown. --- .../src/rise_motion/src/ec_manager.cpp | 14 ++++++++++---- 1 file changed, 10 insertions(+), 4 deletions(-) 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 index 99d1a9b..26bb213 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -101,7 +101,7 @@ void ECManager::cyclic_loop() { // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); if (reached_state != EC_STATE_OPERATIONAL) { - std::exit(EXIT_FAILURE); + goto shutdown; } // Configuring Drives @@ -135,6 +135,7 @@ void ECManager::cyclic_loop() { if (wkc != expectedWKC) { RCLCPP_ERROR(logger, "Not all nodes responded"); + goto shutdown; } // Iterate over connected drives @@ -146,9 +147,12 @@ void ECManager::cyclic_loop() { CiA402Motor m{motor_inputs, motor_outputs}; if (!m.get_state().has_value()) { - RCLCPP_ERROR(logger, "Motor %d has no state", i); - } else if (m.get_state().value() != CiA402Motor::State::OPERATION_ENABLED) { - RCLCPP_ERROR(logger, "Motor %d is not in OPERATION_ENABLED", i); + RCLCPP_ERROR(logger, "Motor %d has no state", i); + goto shutdown; + } else if (m.get_state().value() != + CiA402Motor::State::OPERATION_ENABLED) { + RCLCPP_ERROR(logger, "Motor %d is not in OPERATION_ENABLED", i); + goto shutdown; } if (cmd_apsa.perf_read(motor_commands)) { @@ -232,6 +236,8 @@ void ECManager::cyclic_loop() { transition_ec(EC_STATE_INIT); ecx_close(&ctx); + RCLCPP_INFO(logger, "Exiting"); + stop(); } void ECManager::stop() { running_ = false; } From 644803963a7e677398f2f052347388e999aa57a5 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:55:51 +0100 Subject: [PATCH 41/83] ec_manager.cpp: Update next before entering loop. --- rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 1 + 1 file changed, 1 insertion(+) 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 index 26bb213..491da8e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -126,6 +126,7 @@ void ECManager::cyclic_loop() { } RCLCPP_INFO(logger, "Entering Cyclic Loop"); + next = std::chrono::steady_clock::now(); while (running_) { next += period; From 7c85fe138c1622969b6631af1f16729c82a07d8b Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:56:36 +0100 Subject: [PATCH 42/83] main.cpp: Call ec_manager with cycle_time. --- rise_motion_dev_ws/src/rise_motion/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/main.cpp b/rise_motion_dev_ws/src/rise_motion/src/main.cpp index 365d115..1d50077 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/main.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/main.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - ECManager ec_manager("enp1s0"); // TODO: Connect config from rise-os-core + ECManager ec_manager("enp1s0", 1); // TODO: Connect config from rise-os-core auto node = std::make_shared(ec_manager); rclcpp::spin(node); From 94e83b9d61590e099a059960142f5be6f5b64985 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:57:22 +0100 Subject: [PATCH 43/83] ec_manager: Add is_running. --- .../src/rise_motion/include/rise_motion/ec_manager.hpp | 1 + rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp | 2 ++ 2 files changed, 3 insertions(+) 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 index 74abc56..a81d985 100755 --- 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 @@ -16,6 +16,7 @@ class ECManager { int init_ec(); void cyclic_loop(); + bool is_running(); void stop(); // APSA-based motor value transfer (lock-free) 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 index 491da8e..d44f41e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -243,6 +243,8 @@ void ECManager::cyclic_loop() { 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); From e263c22cde235cba7c21f03f9eeb4270e75f24d3 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:57:46 +0100 Subject: [PATCH 44/83] ec_manager.hpp: Fix headers. --- .../src/rise_motion/include/rise_motion/ec_manager.hpp | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) 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 index a81d985..e011b96 100755 --- 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 @@ -4,8 +4,10 @@ #include #include #include +#include -#include "apsa.hpp" +#include +#include #define IOMAP_SIZE 4096 From ca6e8b960aa0215fc33473aebcaf73bc2e130ec7 Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 12:59:45 +0100 Subject: [PATCH 45/83] ethercat_node.cpp: Only publish if ethercat is enabled. --- rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) 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 index b09b038..d729f90 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -70,7 +70,7 @@ void EthercatNode::commandCallback( void EthercatNode::publishFeedback() { std::vector positions; - if (ec_manager_.get_motor_values_apsa(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); From 66ad5208a1a3aec17f18e550de663795410647cd Mon Sep 17 00:00:00 2001 From: simeon Date: Thu, 5 Mar 2026 13:01:28 +0100 Subject: [PATCH 46/83] Formatting. --- .../src/rise_motion/src/ec_manager.cpp | 134 +++++++++--------- .../src/rise_motion/src/ethercat_node.cpp | 34 +++-- 2 files changed, 84 insertions(+), 84 deletions(-) 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 index d44f41e..1d1ea23 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -111,12 +111,16 @@ void ECManager::cyclic_loop() { CiA402_Inputs *motor_inputs = (CiA402_Inputs *)ctx.slavelist[i].inputs; CiA402Motor m{motor_inputs, motor_outputs}; + // Setting ModeOfOperation to CyclicSyncPositionMode - m.set_mode_of_operation(CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + m.set_mode_of_operation( + CiA402Motor::ModeOfOperation::CyclicSyncPositionMode); + // Set Position to Current Position - motor_commands[i-1] = motor_inputs->PositionValue; + motor_commands[i - 1] = motor_inputs->PositionValue; motor_outputs->TargetPosition = motor_inputs->PositionValue; - RCLCPP_INFO(logger, "Configured Motor %d: (%d)", i, motor_inputs->PositionValue); + RCLCPP_INFO(logger, "Configured Motor %d: Init Position(%d)", i, + motor_inputs->PositionValue); } // Transitioning CiA402 State Machine to OPERATION_ENABLED @@ -125,6 +129,8 @@ void ECManager::cyclic_loop() { goto shutdown; } + RCLCPP_INFO(logger, "All motors in operation_enabled"); + RCLCPP_INFO(logger, "Entering Cyclic Loop"); next = std::chrono::steady_clock::now(); while (running_) { @@ -156,66 +162,57 @@ void ECManager::cyclic_loop() { goto shutdown; } - if (cmd_apsa.perf_read(motor_commands)) { - // New commands received! Apply them to EtherCAT nodes - motor_outputs->TargetPosition = motor_commands[i-1]; - 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", - motor_outputs->Controlword, motor_outputs->OpMode, - motor_outputs->TargetTorque, motor_outputs->TargetPosition, - motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, - motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, - motor_outputs->BitMask, motor_outputs->UserMOSI, - motor_outputs->VelocityOffset); - } - motor_outputs->TargetPosition = motor_commands[i-1]; - motor_feedback[i-1] = motor_inputs->PositionValue; + // Try to get new data + cmd_apsa.perf_read(motor_commands); + motor_outputs->TargetPosition = motor_commands[i - 1]; + motor_feedback[i - 1] = motor_inputs->PositionValue; + 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", - motor_inputs->Statusword, - motor_inputs->OpModeDisplay, - motor_inputs->PositionValue, - motor_inputs->VelocityValue, - motor_inputs->TorqueValue, - motor_inputs->AnalogInput1, - motor_inputs->AnalogInput2, - motor_inputs->AnalogInput3, - motor_inputs->AnalogInput4, - motor_inputs->TuningStatus, - motor_inputs->DigitalInputs, - motor_inputs->UserMISO, - motor_inputs->Timestamp, - motor_inputs->PositionDemandInternalValue, - motor_inputs->VelocityDemandValue, - motor_inputs->TorqueDemand - ); + "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", + motor_outputs->Controlword, motor_outputs->OpMode, + motor_outputs->TargetTorque, motor_outputs->TargetPosition, + motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, + motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, + motor_outputs->BitMask, motor_outputs->UserMOSI, + motor_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", + motor_inputs->Statusword, motor_inputs->OpModeDisplay, + motor_inputs->PositionValue, motor_inputs->VelocityValue, + motor_inputs->TorqueValue, motor_inputs->AnalogInput1, + motor_inputs->AnalogInput2, motor_inputs->AnalogInput3, + motor_inputs->AnalogInput4, motor_inputs->TuningStatus, + motor_inputs->DigitalInputs, motor_inputs->UserMISO, + motor_inputs->Timestamp, motor_inputs->PositionDemandInternalValue, + motor_inputs->VelocityDemandValue, motor_inputs->TorqueDemand); } // Make feedback available to ROS publisher (wait-free) @@ -293,14 +290,13 @@ uint16 ECManager::transition_ec(uint16 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)); - } + 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; } 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 index d729f90..89bad70 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -1,27 +1,30 @@ -#include +#include #include +#include +#include #include using std::placeholders::_1; using std::placeholders::_2; -EthercatNode::EthercatNode(ECManager& ec_manager) - : Node("ethercat_node"), ec_manager_(ec_manager) { +EthercatNode::EthercatNode(ECManager &ec_manager) + : Node("ethercat_node"), ec_manager_(ec_manager) { cmd_sub_ = create_subscription( - "motor_commands", 10, - std::bind(&EthercatNode::commandCallback, this, _1)); + "motor_commands", 10, + std::bind(&EthercatNode::commandCallback, this, _1)); feedback_pub_ = create_publisher( - "motor_feedback", 10); + "motor_feedback", 10); - feedback_timer_ = create_wall_timer( - std::chrono::milliseconds(10), - std::bind(&EthercatNode::publishFeedback, this)); + feedback_timer_ = + create_wall_timer(std::chrono::milliseconds(10), + std::bind(&EthercatNode::publishFeedback, this)); enable_srv_ = create_service( - "enable_ethercat", - std::bind(&EthercatNode::enableServiceCallback, this, _1, _2)); + "enable_ethercat", + std::bind(&EthercatNode::enableServiceCallback, this, _1, _2)); + sdo_read_srv_ = create_service( "sdo_read", std::bind(&EthercatNode::sdoReadServiceCallback, this, _1, _2)); @@ -78,16 +81,17 @@ void EthercatNode::publishFeedback() { } void EthercatNode::enableServiceCallback( - const std::shared_ptr request, - std::shared_ptr response) -{ + 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(&ECManager::cyclic_loop, &ec_manager_); + std::make_unique(&ECManager::cyclic_loop, &ec_manager_); ethercat_enabled_ = true; } } else if (!request->enable && ethercat_enabled_) { From 82bcc6787b285a3361e819cdf18a66a445e02cd5 Mon Sep 17 00:00:00 2001 From: Simeon Prause Date: Fri, 6 Mar 2026 15:48:18 +0100 Subject: [PATCH 47/83] ec_manager: Add shutdown method and motors property. --- .../include/rise_motion/cia402.hpp | 6 +- .../include/rise_motion/ec_manager.hpp | 3 + .../src/rise_motion/src/ec_manager.cpp | 154 +++++++++--------- 3 files changed, 85 insertions(+), 78 deletions(-) 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 index 8b54d5c..514190f 100755 --- 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 @@ -110,6 +110,9 @@ class CiA402Motor { 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; @@ -145,7 +148,4 @@ class CiA402Motor { {0b10001111, 0b00000111, Operation::DISABLE_OPERATION}, {0b10001111, 0b00001111, Operation::ENABLE_OPERATION}, {0b10000000, 0b10000000, Operation::FAULT_RESET}}; - - CiA402_Inputs *inputs; - CiA402_Outputs *outputs; }; 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 index e011b96..279b07b 100755 --- 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 @@ -32,11 +32,14 @@ class ECManager { 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; 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 index 1d1ea23..8b8cfd6 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -12,7 +13,6 @@ #include #include - // expected config, needs to be retrieved from config node struct { int slavecount = 1; @@ -53,7 +53,7 @@ int ECManager::init_ec() { // TODO: More extensive verification of network if (ctx.slavecount != config.slavecount) { RCLCPP_ERROR(logger, "Expected %d devices, but discovered %d", - config.slavecount, ctx.slavecount); + config.slavecount, ctx.slavecount); return EXIT_FAILURE; } @@ -85,6 +85,15 @@ int ECManager::init_ec() { 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; } @@ -92,7 +101,6 @@ void ECManager::cyclic_loop() { // Still in SAFE_OP, PDO transmission is available next = std::chrono::steady_clock::now(); running_ = true; - int wkc; std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); @@ -101,32 +109,29 @@ void ECManager::cyclic_loop() { // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED uint16 reached_state = transition_ec(EC_STATE_OPERATIONAL); if (reached_state != EC_STATE_OPERATIONAL) { - goto shutdown; + shutdown(); + return; } // Configuring Drives - 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}; - + 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 - 1] = motor_inputs->PositionValue; - motor_outputs->TargetPosition = motor_inputs->PositionValue; - RCLCPP_INFO(logger, "Configured Motor %d: Init Position(%d)", i, - motor_inputs->PositionValue); + 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"); - goto shutdown; + shutdown(); + return; } RCLCPP_INFO(logger, "All motors in operation_enabled"); @@ -134,6 +139,7 @@ void ECManager::cyclic_loop() { RCLCPP_INFO(logger, "Entering Cyclic Loop"); next = std::chrono::steady_clock::now(); while (running_) { + int wkc; next += period; ecx_send_processdata(&ctx); @@ -142,30 +148,29 @@ void ECManager::cyclic_loop() { if (wkc != expectedWKC) { RCLCPP_ERROR(logger, "Not all nodes responded"); - goto shutdown; + shutdown(); + return; } // Iterate over connected drives - 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}; + for (size_t i = 0; i < motors.size(); i++) { + CiA402Motor &m = motors[i]; if (!m.get_state().has_value()) { - RCLCPP_ERROR(logger, "Motor %d has no state", i); - goto shutdown; + 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 %d is not in OPERATION_ENABLED", i); - goto shutdown; + RCLCPP_ERROR(logger, "Motor %zu is not in OPERATION_ENABLED", i + 1); + shutdown(); + return; } // Try to get new data cmd_apsa.perf_read(motor_commands); - motor_outputs->TargetPosition = motor_commands[i - 1]; - motor_feedback[i - 1] = motor_inputs->PositionValue; + m.outputs->TargetPosition = motor_commands[i]; + motor_feedback[i] = m.inputs->PositionValue; RCLCPP_DEBUG(logger, "Motor Outputs:\n" @@ -180,39 +185,38 @@ void ECManager::cyclic_loop() { "\tBitMask: 0x%08X\n" "\tUserMOSI: 0x%08X\n" "\tVelocityOffset: %d\n", - motor_outputs->Controlword, motor_outputs->OpMode, - motor_outputs->TargetTorque, motor_outputs->TargetPosition, - motor_outputs->TargetVelocity, motor_outputs->TorqueOffset, - motor_outputs->TuningCommand, motor_outputs->PhysicalOutputs, - motor_outputs->BitMask, motor_outputs->UserMOSI, - motor_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", - motor_inputs->Statusword, motor_inputs->OpModeDisplay, - motor_inputs->PositionValue, motor_inputs->VelocityValue, - motor_inputs->TorqueValue, motor_inputs->AnalogInput1, - motor_inputs->AnalogInput2, motor_inputs->AnalogInput3, - motor_inputs->AnalogInput4, motor_inputs->TuningStatus, - motor_inputs->DigitalInputs, motor_inputs->UserMISO, - motor_inputs->Timestamp, motor_inputs->PositionDemandInternalValue, - motor_inputs->VelocityDemandValue, motor_inputs->TorqueDemand); + 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) @@ -222,8 +226,12 @@ void ECManager::cyclic_loop() { std::this_thread::sleep_until(next); } -shutdown: - RCLCPP_INFO(logger, "Exiting cyclic loop"); + 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"); @@ -234,8 +242,7 @@ void ECManager::cyclic_loop() { transition_ec(EC_STATE_INIT); ecx_close(&ctx); - RCLCPP_INFO(logger, "Exiting"); - stop(); + running_ = false; } void ECManager::stop() { running_ = false; } @@ -340,19 +347,16 @@ bool ECManager::transition_motors_to(CiA402Motor::State state) { next += period; tries_left--; continue_flag = 0; - for (int i = 1; i <= ctx.slavecount; i++) { - CiA402_Inputs *motor_inputs = (CiA402_Inputs *)ctx.slavelist[i].inputs; - CiA402_Outputs *motor_outputs = - (CiA402_Outputs *)ctx.slavelist[i].outputs; - CiA402Motor m{motor_inputs, motor_outputs}; - RCLCPP_DEBUG(logger, "State of Motor %d: %s", i, + 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 %d has no decodable state: 0x%04X", i, - motor_inputs->Statusword); + 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 %d in fault", i); + RCLCPP_ERROR(logger, "Motor %zu in fault", i + 1); return false; } else if (m.get_state().value() != state) { m.transition_to(state); From 2ea9f6fb35d4e7697dec239a1f688f6dbd3574a8 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 25 Mar 2026 16:14:23 +0100 Subject: [PATCH 48/83] Feat: Implemented Mock ECManager Closes #52 --- .gitignore | 4 +- docs/development.md | 385 ++++++++++++++++++ .../src/rise_motion/CMakeLists.txt | 15 + .../include/rise_motion/ec_manager.hpp | 19 +- .../include/rise_motion/ec_manager_mock.hpp | 43 ++ .../include/rise_motion/ethercat_node.hpp | 6 +- .../include/rise_motion/iec_manager.hpp | 21 + .../src/rise_motion/src/ec_manager.cpp | 12 +- .../src/rise_motion/src/ec_manager_mock.cpp | 71 ++++ .../src/rise_motion/src/ethercat_node.cpp | 6 +- .../src/rise_motion/src/main_mock.cpp | 16 + 11 files changed, 576 insertions(+), 22 deletions(-) create mode 100644 docs/development.md create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager_mock.hpp create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/iec_manager.hpp create mode 100644 rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp create mode 100644 rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp diff --git a/.gitignore b/.gitignore index 11ac0bf..352c1f9 100644 --- a/.gitignore +++ b/.gitignore @@ -2,4 +2,6 @@ **/log/ **/install/ **/*~ -.vscode \ No newline at end of file +.vscode +.claude +CLAUDE.md \ No newline at end of file diff --git a/docs/development.md b/docs/development.md new file mode 100644 index 0000000..93d28b7 --- /dev/null +++ b/docs/development.md @@ -0,0 +1,385 @@ +# RISE Motion – Entwicklungsdokumentation + +## Inhaltsverzeichnis +1. [Build & Ausführen](#1-build--ausführen) +2. [Projektstruktur](#2-projektstruktur) +3. [Architekturübersicht](#3-architekturübersicht) +4. [Komponenten](#4-komponenten) +5. [ROS2 Interface](#5-ros2-interface) +6. [Admittanzregelung](#6-admittanzregelung) +7. [Entwicklungsplan](#7-entwicklungsplan) + +--- + +## 1. Build & Ausführen + +### Voraussetzungen +- ROS2 Jazzy +- SOEM Submodul initialisiert: + ```bash + git submodule init + git submodule update + ``` + +### Bauen +```bash +cd rise_motion_dev_ws +colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +source install/setup.bash +``` + +Das Skript `rise_motion_dev_ws/build.sh` führt alle drei Schritte aus. + +```bash +cd ~/rise-motion/rise_motion_dev_ws +./build.sh +``` + +### Netzwerkrechte setzen (einmalig nach Build) +EtherCAT braucht Raw-Socket-Zugriff: +```bash +sudo setcap cap_net_admin,cap_net_raw+eip build/rise_motion/rise_motion_main +``` + +### Ausführen (mit Hardware) +Setzt eine EtherCAT-Verbindung auf Interface `enp1s0` voraus (Linux-PC, **nicht WSL2**): +```bash +# Terminal 1 – EtherCAT Node starten (benötigt Raw-Socket-Rechte) +sudo -E ros2 run rise_motion rise_motion_main + +# Terminal 2 – Test-Node starten +# Ruft /enable_ethercat automatisch auf, bevor Commands gesendet werden. +# Optionales Argument: Inkrement pro Zyklus (default: 10, besser: 1) +ros2 run rise_motion testing_node 1 +``` + +Der `testing_node` ruft `/enable_ethercat` selbst auf – kein manueller Service-Call nötig. Der Node wartet blockierend bis der Service verfügbar ist (`waiting for service to appear...`). + +Manueller Service-Call (z.B. zur Diagnose ohne testing_node): +```bash +ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" +``` + +Der `/enable_ethercat`-Call löst intern automatisch aus: +1. EtherCAT-Zustände `INIT → PRE_OP → SAFE_OP → OPERATIONAL` +2. Mode of Operation → `CyclicSyncPositionMode` +3. CiA402-State-Machine → `OPERATION_ENABLED` + +SDO-Calls (`/sdo_read`, `/sdo_write`) sind optional und dienen nur zur Diagnose einzelner Drive-Register. + +### Ausführen (ohne Hardware, WSL2) +```bash +# Immer: +source install/setup.bash + +# Terminal 1 +ros2 run rise_motion rise_motion_mock + +# (Terminal 2) Not needed! +ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" + +# Terminal 3 +ros2 run rise_motion testing_node + +# Terminal 4: Motor position validieren +ros2 topic echo /motor_feedback +``` + +Der Mock simuliert einen Motor im RAM: Kommandos werden sofort als Feedback zurückgegeben. Kein EtherCAT, kein SOEM, keine Netzwerkrechte erforderlich. + +--- + +## 2. State Machines + +Beim Start müssen zwei voneinander unabhängige State Machines hochgefahren werden. + +### EtherCAT Bus States (SOEM) + +Zustand des **Kommunikationsbusses** – unabhängig von den Motoren. + +``` +INIT → PRE_OP → SAFE_OP → OPERATIONAL +``` + +| State | Was passiert | +|---|---| +| `INIT` | Bus initialisiert, keine Kommunikation | +| `PRE_OP` | SDO-Kommunikation möglich (Konfiguration lesen/schreiben) | +| `SAFE_OP` | PDOs werden empfangen, aber noch nicht gesendet – Motoren kriegen noch keine Commands | +| `OPERATIONAL` | PDOs bidirektional aktiv – Motoren können gesteuert werden | + +`init_ec()` bringt den Bus bis `SAFE_OP`. `cyclic_loop()` bringt ihn auf `OPERATIONAL`. + +### CiA402 Drive States (pro Motor) + +Zustand **jedes einzelnen Servo-Drives** (Synapticon Circulo 9). Wird über das `Controlword`-PDO gesteuert, aktueller State steht im `Statusword`. + +``` +NOT_READY_TO_SWITCH_ON + ↓ +SWITCH_ON_DISABLED + ↓ +READY_TO_SWITCH_ON + ↓ +SWITCHED_ON + ↓ +OPERATION_ENABLED ← hier laufen die Motoren +``` + +Fehler-States: +- `QUICK_STOP_ACTIVE` – Notbremse aktiv +- `FAULT_REACTION_ACTIVE` → `FAULT` – Drive hat einen Fehler, muss manuell zurückgesetzt werden + +### Startreihenfolge + +``` +enable_ethercat Service → + init_ec(): + Bus: INIT → PRE_OP → SAFE_OP + + cyclic_loop(): + Bus: SAFE_OP → OPERATIONAL + Motoren: Mode = CyclicSyncPositionMode + Motoren: → OPERATION_ENABLED + → 1kHz Loop startet +``` + +EtherCAT muss `OPERATIONAL` sein, bevor die CiA402-State-Machine auf `OPERATION_ENABLED` geht. + +**Fehlerverhalten:** +- Motor im `FAULT`-State → `transition_motors_to()` bricht ab, Node fährt herunter (`Motor X in fault` im Log) +- Im cyclic loop wird **jeden Zyklus** geprüft ob alle Motoren `OPERATION_ENABLED` sind – wenn nicht, sofortiger Shutdown + +**FAULT zurücksetzen:** + +Tritt `Motor X in fault` auf, muss der FAULT-State zurückgesetzt werden bevor der Drive wieder in `OPERATION_ENABLED` gebracht werden kann. + +Einfachster Weg: **Power-Cycle des Synapticon Circulo** (Strom aus/ein). Faults werden bei Stromverlust zurückgesetzt. + +Alternativ per Controlword (Bit 7 = Fault Reset, CiA402 Index `0x6040`). Funktioniert nur wenn der Bus bereits `OPERATIONAL` ist: +```bash +ros2 service call /sdo_write rise_motion_messages/srv/SDOWriteSrv \ + "{device_id: 1, index: 0x6040, subindex: 0x00, value: [0x80]}" +``` + +Mögliche Ursachen für FAULT: unsauberer Shutdown beim letzten Lauf, Überstrom/Überspannung, oder Drive war beim Start nicht in Ruheposition. + +**Für die Admittanzregelung:** Wechsel von `CyclicSyncPositionMode` auf `CyclicSyncVelocityMode` (Mode 9) – an der Stelle `m.set_mode_of_operation(...)` in `cyclic_loop()`. + +--- + +## 3. Architekturübersicht + +``` +┌─────────────────────────────────────────────────────────────┐ +│ ROS2 Thread │ +│ │ +│ [AdmittanzNode] ──cmd──► [EthercatNode] │ +│ ▲ │ │ +│ │ feedback │ cmd │ +│ └──────────────────────────┘ │ +└──────────────────────────┬──────────────────────────────────┘ + │ APSA (lock-free) +┌──────────────────────────▼──────────────────────────────────┐ +│ EtherCAT Thread (1 kHz) │ +│ │ +│ [ECManager::cyclic_loop()] │ +│ │ │ +│ ┌─────────────▼─────────────┐ │ +│ │ SOEM / IgH │ │ +│ └─────────────┬─────────────┘ │ +└──────────────────────┼──────────────────────────────────────┘ + │ EtherCAT Bus + ┌────────────┼────────────┐ + [Motor 1] [Motor 2] ... [Motor 6] + Synapticon Circulo 9 (CiA402) +``` + +**Threads:** +- **ROS2-Thread:** `EthercatNode` empfängt Kommandos, veröffentlicht Feedback +- **EtherCAT-Thread:** `ECManager::cyclic_loop()` läuft mit 1 kHz, kommuniziert direkt mit den Drives + +**Datenaustausch:** APSA (Atomic Pointer Swap Algorithm) – Triple-Buffer, komplett lock-free, kein Mutex zwischen den Threads. + +--- + +## 4. Komponenten + +### ECManager +Verwaltet den EtherCAT-Bus und den Cyclic Loop. + +| Methode | Beschreibung | +|---|---| +| `init_ec()` | Bus initialisieren, Drives in OP-State bringen | +| `cyclic_loop()` | 1-kHz-Loop: PDO senden/empfangen, State-Machine | +| `is_running()` | Gibt zurück ob der Loop aktiv ist | +| `stop()` | Loop sauber beenden | +| `get_motor_values_apsa()` | Feedback aus EtherCAT-Thread lesen (ROS-Seite) | +| `set_motor_values_apsa()` | Kommando in EtherCAT-Thread schreiben (ROS-Seite) | +| `sdo_read/write()` | SDO-Zugriff für Drive-Konfiguration | + +### EthercatNode +ROS2-Node als Brücke zwischen ROS-Welt und ECManager. + +| Interface | Typ | Beschreibung | +|---|---|---| +| Sub: `motor_commands` | `MotorPositions` | Positionsbefehle empfangen | +| Pub: `motor_feedback` | `MotorPositions` | Motorpositionen publizieren | +| Srv: `enable_ethercat` | `EnableEthercatSrv` | EtherCAT starten/stoppen | +| Srv: `sdo_read` | `SDOReadSrv` | SDO-Register lesen | +| Srv: `sdo_write` | `SDOWriteSrv` | SDO-Register schreiben | + +### MockECManager +Implementiert `IECManager` ohne SOEM-Abhängigkeit. Läuft in WSL2 ohne Hardware. + +| Parameter | Default | Beschreibung | +|---|---|---| +| `cycle_period_ms` | 1 | Zykluszeit in ms | +| `num_motors` | 1 | Anzahl simulierter Motoren | +| `alpha` | 1.0 | Tracking-Faktor (1.0 = sofortiges Echo, <1.0 = Lag) | + +`init_ec()` gibt sofort `EXIT_SUCCESS` zurück. `cyclic_loop()` liest Kommandos aus `cmd_apsa`, berechnet `pos += alpha * (cmd - pos)` und schreibt das Ergebnis in `feedback_apsa`. + +### CiA402Motor +Implementiert das CiA402-Antriebsprofil (IEC 61800-7-201) für Synapticon Circulo 9. + +**State Machine:** `NOT_READY_TO_SWITCH_ON` → `SWITCH_ON_DISABLED` → `READY_TO_SWITCH_ON` → `SWITCHED_ON` → `OPERATION_ENABLED` + +**Betriebsmodi (ModeOfOperation):** +- `CyclicSyncPositionMode` (8) – aktuell verwendet +- `CyclicSyncVelocityMode` (9) – für Admittanzregelung geplant +- `CyclicSyncTorqueMode` (10) +- `ImpedanceMode` (-6), `JointTorqueMode` (-5) – Synapticon-spezifisch + +**PDO-Daten (Inputs von Drive):** Statusword, Position, Velocity, Torque, AnalogInput1–4, Timestamp +**PDO-Daten (Outputs an Drive):** Controlword, OpMode, TargetPosition, TargetVelocity, TargetTorque + +### APSA (Lock-free Buffer) +Triple-Buffer für sicheren, blockierungsfreien Datenaustausch zwischen ROS- und EtherCAT-Thread. + +| Methode | Seite | Beschreibung | +|---|---|---| +| `comm_write()` | ROS-Thread | Kommando schreiben | +| `comm_read()` | ROS-Thread | Feedback lesen | +| `perf_write()` | EtherCAT-Thread | Feedback schreiben | +| `perf_read()` | EtherCAT-Thread | Kommando lesen | + +**Verwendung im Projekt:** +- `cmd_apsa`: ROS → EtherCAT (Motorkommandos) +- `feedback_apsa`: EtherCAT → ROS (Motorpositionen) + +--- + +## 5. ROS2 Interface + +### Messages + +**`MotorPositions.msg`** +``` +int32[] positions # Motorpositionen (encoder-Inkremente), 6 Achsen +int8 target +``` + +**Achsen-Mapping** (Index 0–5): +| Index | Achse | +|---|---| +| 0 | Hüfte Abduktion/Adduktion Links | +| 1 | Hüfte Abduktion/Adduktion Rechts | +| 2 | Hüfte Flexion/Extension Links | +| 3 | Hüfte Flexion/Extension Rechts | +| 4 | Knie Flexion/Extension Links | +| 5 | Knie Flexion/Extension Rechts | + +### Services + +**`EnableEthercatSrv.srv`** +``` +bool enable +--- +int8 status_enable +``` + +**`SDOReadSrv.srv`** +``` +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 +``` + +--- + +## 6. Admittanzregelung + +### Regelungsgesetz +``` +M_v * v̇ + D_v * v + K_v * q = τ_int + +Euler-Integration (dt = 1 ms): + v̇ = (τ_int - D_v * v - K_v * q) / M_v + v += v̇ * dt + q += v * dt + +pos_target = pos_current + q +``` + +**Parameter:** +| Symbol | Bedeutung | Transparenter Modus | +|---|---|---| +| M_v | Virtuelle Masse | > 0 | +| D_v | Virtuelle Dämpfung | > 0 | +| K_v | Virtuelle Steifigkeit | **0** (transparent) | + +### Kraftsensor +- Wägezelle zwischen Manschette und Exoskelett +- Moment: `τ = F * l` (konstanter Hebelarm `l`) +- ADC-Wert in `AnalogInput1` (und ggf. `AnalogInput2`) der CiA402-PDOs +- Kalibrierung: ADC-Ticks → Newton → Nm (Woche 3) + +### Geplante ROS2-Nodes + +| Node | Datei | Status | +|---|---|---| +| `EthercatNode` | `ethercat_node.cpp` | vorhanden | +| `AdmittanzNode` | *geplant* | Woche 1 | +| `TestNode` | `testing_node.cpp` | vorhanden (Positions-Inkrement) | + +### Offene Punkte +- Velocity-Commands an Servo: restlicher RISE-OS Stack arbeitet mit Positionscommands → Integration noch offen +- Geplantes Topic `motor_feedback_full` mit `WrenchMsg` (Woche 2) + +### Safety (minimal) +- `v_max`: maximale Gelenkgeschwindigkeit +- `dv_max`: maximale Beschleunigung +- Watchdog: bei ausbleibendem Kraftsensor-Signal stoppen + +--- + +## 7. Entwicklungsplan + +### Woche 1 – Mock & Grundstruktur +- [x] Mock `ECManager` (lokale Tests ohne Hardware) +- [ ] `AdmittanzNode` mit Fake-Kraft-Input (`F = 10 * sin(2π * 0.5 * t)`) +- [ ] Mathematik validieren (Plots) + +### Woche 2 – Kraftsensor-Integration (Software) +- [ ] `FeedbackData` Struct erweitern (AnalogInput1/2 aus CiA402 PDO) +- [ ] ECManager: Analog Inputs in `feedback_apsa` durchleiten +- [ ] Neue Message `WrenchMsg.msg` definieren + +### Woche 3 – Hardware-Tests +- [ ] Echten Kraftsensor testen (Linux-PC, Interface `enp1s0`) +- [ ] Kalibrierung (ADC-Ticks → Newton → Nm) +- [ ] Erste Admittanz-Tests am Exoskelett + +### Woche 4+ – Integration & Tuning +- [ ] Parameter M, D, K über ROS2 Parameter Server konfigurierbar +- [ ] Integration in RISE-OS Stack (`motor_command_safe`) +- [ ] Safety-Logik: v_max, pos_limits, Watchdog, Notaus diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index ce8a2e3..1dd2d48 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -37,6 +37,21 @@ install(TARGETS 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 ) 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 index 279b07b..0a0764d 100755 --- 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 @@ -6,28 +6,29 @@ #include #include +#include #include #include #define IOMAP_SIZE 4096 -class ECManager { +class ECManager : public IECManager { public: ECManager(); ECManager(const std::string interface, int cycle_period_ms); - int init_ec(); - void cyclic_loop(); - bool is_running(); - void stop(); + 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); - bool set_motor_values_apsa(const std::vector& motor_values); + bool get_motor_values_apsa(std::vector& motor_values) override; + bool set_motor_values_apsa(const std::vector& motor_values) override; // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread - bool sdo_read(uint16 device_id, uint16 index, uint8 subindex, std::vector& value); - bool sdo_write(uint16 device_id, uint16 index, uint8 subindex, std::vector& value); + 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); 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..dd36365 --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/ec_manager_mock.hpp @@ -0,0 +1,43 @@ +#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 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 motor_commands_; + + std::chrono::time_point next_; + const std::chrono::duration> period_; + + APSA> cmd_apsa_; + APSA> 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 index 401e911..f77df11 100755 --- 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 @@ -1,7 +1,7 @@ #pragma once #include #include -#include +#include #include #include #include @@ -10,11 +10,11 @@ class EthercatNode : public rclcpp::Node { public: - explicit EthercatNode(ECManager& ec_manager); + explicit EthercatNode(IECManager& ec_manager); ~EthercatNode(); private: - ECManager& ec_manager_; + IECManager& ec_manager_; std::unique_ptr ec_thread_; bool ethercat_enabled_{false}; 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..449062f --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/include/rise_motion/iec_manager.hpp @@ -0,0 +1,21 @@ +#pragma once +#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 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/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp index 8b8cfd6..8042d5c 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -308,10 +308,10 @@ uint16 ECManager::transition_ec(uint16 state) { return reached_state; } -bool ECManager::sdo_read(uint16 device_id, uint16 index, uint8 subindex, - std::vector &value) { +bool ECManager::sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, + std::vector &value) { int psize = 64; - uint8 *buf = new uint8[psize]; + uint8_t *buf = new uint8_t[psize]; boolean CA = FALSE; int wkc = ecx_SDOread(&ctx, device_id, index, subindex, CA, &psize, @@ -326,10 +326,10 @@ bool ECManager::sdo_read(uint16 device_id, uint16 index, uint8 subindex, return true; } -bool ECManager::sdo_write(uint16 device_id, uint16 index, uint8 subindex, - std::vector &value) { +bool ECManager::sdo_write(uint16_t device_id, uint16_t index, uint8_t subindex, + std::vector &value) { int psize = value.size(); - uint8 *buf = new uint8[psize]; + uint8_t *buf = new uint8_t[psize]; boolean CA = FALSE; int wkc = ecx_SDOwrite(&ctx, device_id, index, subindex, CA, psize, 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..3eee8dc --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp @@ -0,0 +1,71 @@ +#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), + motor_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 motor commands from ROS thread (wait-free) + cmd_apsa_.perf_read(motor_commands_); + + // Simulate motor: track commanded position with configurable lag + for (int i = 0; i < num_motors_; ++i) { + mock_positions_[i] += static_cast( + alpha_ * static_cast(motor_commands_[i] - mock_positions_[i])); + } + + // Publish feedback to ROS thread (wait-free) + feedback_apsa_.perf_write(mock_positions_); + + 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::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 index 89bad70..3b6e956 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -7,7 +7,7 @@ using std::placeholders::_1; using std::placeholders::_2; -EthercatNode::EthercatNode(ECManager &ec_manager) +EthercatNode::EthercatNode(IECManager &ec_manager) : Node("ethercat_node"), ec_manager_(ec_manager) { cmd_sub_ = create_subscription( @@ -91,7 +91,7 @@ void EthercatNode::enableServiceCallback( RCLCPP_ERROR(get_logger(), "Couldn't init ethercat"); } else { ec_thread_ = - std::make_unique(&ECManager::cyclic_loop, &ec_manager_); + std::make_unique([this]() { ec_manager_.cyclic_loop(); }); ethercat_enabled_ = true; } } else if (!request->enable && ethercat_enabled_) { @@ -116,7 +116,7 @@ void EthercatNode::sdoReadServiceCallback( return; } - std::vector value; + std::vector value; bool success = ec_manager_.sdo_read(request->device_id, request->index, request->subindex, value); 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..15c1a8b --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp @@ -0,0 +1,16 @@ +#include +#include +#include +#include + +int main(int argc, char *argv[]) { + rclcpp::init(argc, argv); + + MockECManager ec_manager(/*cycle_period_ms=*/1, /*num_motors=*/1); + auto node = std::make_shared(ec_manager); + + rclcpp::spin(node); + + rclcpp::shutdown(); + return 0; +} From 0b6fa4335c38a8aab27c544a015c97a234e9465f Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 25 Mar 2026 16:24:24 +0100 Subject: [PATCH 49/83] Improve docs --- docs/development.md | 25 ++++++++++++++++++------- 1 file changed, 18 insertions(+), 7 deletions(-) diff --git a/docs/development.md b/docs/development.md index 93d28b7..afc765d 100644 --- a/docs/development.md +++ b/docs/development.md @@ -22,18 +22,18 @@ ``` ### Bauen -```bash -cd rise_motion_dev_ws -colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -source install/setup.bash -``` - Das Skript `rise_motion_dev_ws/build.sh` führt alle drei Schritte aus. ```bash cd ~/rise-motion/rise_motion_dev_ws ./build.sh ``` +Intern macht das: +```bash +cd rise_motion_dev_ws +colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON +source install/setup.bash +``` ### Netzwerkrechte setzen (einmalig nach Build) EtherCAT braucht Raw-Socket-Zugriff: @@ -45,7 +45,7 @@ sudo setcap cap_net_admin,cap_net_raw+eip build/rise_motion/rise_motion_main Setzt eine EtherCAT-Verbindung auf Interface `enp1s0` voraus (Linux-PC, **nicht WSL2**): ```bash # Terminal 1 – EtherCAT Node starten (benötigt Raw-Socket-Rechte) -sudo -E ros2 run rise_motion rise_motion_main +ros2 run rise_motion rise_motion_main # Terminal 2 – Test-Node starten # Ruft /enable_ethercat automatisch auf, bevor Commands gesendet werden. @@ -67,6 +67,17 @@ Der `/enable_ethercat`-Call löst intern automatisch aus: SDO-Calls (`/sdo_read`, `/sdo_write`) sind optional und dienen nur zur Diagnose einzelner Drive-Register. +Manuelle Commands an einen Motor schicken (ohne `testing_node`): +```bash +# Einmalig: +ros2 topic pub /motor_commands rise_motion_messages/msg/MotorPositions \ + "{positions: [1000, 0, 0, 0, 0, 0], target: 0}" + +# Kontinuierlich (10 Hz): +ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ + "{positions: [1000, 0, 0, 0, 0, 0], target: 0}" +``` + ### Ausführen (ohne Hardware, WSL2) ```bash # Immer: From 3fd9759c04b872b4a92e5c7abaaf030b317e65ab Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 25 Mar 2026 16:35:38 +0100 Subject: [PATCH 50/83] Fix docs --- docs/development.md | 12 +++++++----- 1 file changed, 7 insertions(+), 5 deletions(-) diff --git a/docs/development.md b/docs/development.md index afc765d..3727c53 100644 --- a/docs/development.md +++ b/docs/development.md @@ -69,13 +69,15 @@ SDO-Calls (`/sdo_read`, `/sdo_write`) sind optional und dienen nur zur Diagnose Manuelle Commands an einen Motor schicken (ohne `testing_node`): ```bash -# Einmalig: -ros2 topic pub /motor_commands rise_motion_messages/msg/MotorPositions \ - "{positions: [1000, 0, 0, 0, 0, 0], target: 0}" +# Terminal 1 +ros2 run rise_motion rise_motion_main + +# Terminal 2 – EtherCAT manuell enablen +ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" -# Kontinuierlich (10 Hz): +# Terminal 3 – Commands schicken (kontinuierlich, 10 Hz) ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ - "{positions: [1000, 0, 0, 0, 0, 0], target: 0}" + "{positions: [1000, 0, 0, 0, 0, 0]}" ``` ### Ausführen (ohne Hardware, WSL2) From ea29f9205aa347a3959873f4fa9c4d0dceafd86c Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 19:21:35 +0100 Subject: [PATCH 51/83] Add .gitignore --- .gitignore | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/.gitignore b/.gitignore index 352c1f9..893fda8 100644 --- a/.gitignore +++ b/.gitignore @@ -1,7 +1,10 @@ **/build/ **/log/ **/install/ +rosbag2_*/ **/*~ .vscode .claude -CLAUDE.md \ No newline at end of file +CLAUDE.md +plans/ +SOMANET Circulo Safe Motion.pdf \ No newline at end of file From 7678cd1d0040e8dbd4436fa3cd30dad299c72b2b Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 19:25:05 +0100 Subject: [PATCH 52/83] dont know what im doing --- .gitignore | 2 +- docs/development.md | 13 ++++--- .../include/rise_motion/ec_manager.hpp | 6 +++- .../include/rise_motion/ec_manager_mock.hpp | 3 ++ .../include/rise_motion/ethercat_node.hpp | 4 +++ .../include/rise_motion/iec_manager.hpp | 3 ++ .../rise_motion/motor_feedback_data.hpp | 21 +++++++++++ .../src/rise_motion/src/ec_manager.cpp | 23 ++++++++++++ .../src/rise_motion/src/ec_manager_mock.cpp | 19 ++++++++++ .../src/rise_motion/src/ethercat_node.cpp | 35 +++++++++++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../msg/MotorFeedbackFull.msg | 17 +++++++++ 12 files changed, 138 insertions(+), 9 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/motor_feedback_data.hpp create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg diff --git a/.gitignore b/.gitignore index 893fda8..29b3824 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,4 @@ rosbag2_*/ .claude CLAUDE.md plans/ -SOMANET Circulo Safe Motion.pdf \ No newline at end of file +SOMANET Circulo Safe Motion.pdf diff --git a/docs/development.md b/docs/development.md index 3727c53..19b0a81 100644 --- a/docs/development.md +++ b/docs/development.md @@ -82,20 +82,19 @@ ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ ### Ausführen (ohne Hardware, WSL2) ```bash -# Immer: source install/setup.bash # Terminal 1 ros2 run rise_motion rise_motion_mock -# (Terminal 2) Not needed! -ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" - -# Terminal 3 -ros2 run rise_motion testing_node +# Terminal 2 – testing_node ruft /enable_ethercat automatisch auf +ros2 run rise_motion testing_node 1 -# Terminal 4: Motor position validieren +# Terminal 3 – Motor positions validieren ros2 topic echo /motor_feedback + +# Terminal 4 – Alle PDO-Felder inkl. Kraftsensor (analog_input1 = 1Hz-Sinus im Mock) +ros2 topic echo /motor_feedback_full ``` Der Mock simuliert einen Motor im RAM: Kommandos werden sofort als Feedback zurückgegeben. Kein EtherCAT, kein SOEM, keine Netzwerkrechte erforderlich. 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 index 0a0764d..4576802 100755 --- 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 @@ -25,6 +25,7 @@ class ECManager : public IECManager { // 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; // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread bool sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; @@ -52,6 +53,9 @@ class ECManager : public IECManager { // cmd_apsa: ROS → EtherCAT (motor commands) APSA> cmd_apsa; - // feedback_apsa: EtherCAT → ROS (motor feedback) + // 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; }; 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 index dd36365..3e0ba31 100644 --- 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 @@ -20,6 +20,7 @@ class MockECManager : public IECManager { 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 sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; @@ -34,10 +35,12 @@ class MockECManager : public IECManager { std::vector mock_positions_; std::vector motor_commands_; + uint64_t tick_count_{0}; std::chrono::time_point next_; const std::chrono::duration> period_; APSA> 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 index f77df11..b915adf 100755 --- 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 @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,8 @@ class EthercatNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr 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_; @@ -28,6 +31,7 @@ class EthercatNode : public rclcpp::Node { void commandCallback(const rise_motion_messages::msg::MotorPositions::SharedPtr msg); void publishFeedback(); + void publishFullFeedback(); void enableServiceCallback( const 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 index 449062f..b0293aa 100644 --- 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 @@ -2,6 +2,8 @@ #include #include +#include + class IECManager { public: virtual ~IECManager() = default; @@ -13,6 +15,7 @@ class IECManager { 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 sdo_read(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/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp index 8042d5c..c74e753 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -104,6 +104,7 @@ void ECManager::cyclic_loop() { std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); + std::vector full_feedback(ctx.slavecount); // Transition to OPERATIONAL // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED @@ -172,6 +173,23 @@ void ECManager::cyclic_loop() { m.outputs->TargetPosition = motor_commands[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" @@ -221,6 +239,7 @@ void ECManager::cyclic_loop() { // 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); @@ -260,6 +279,10 @@ bool ECManager::set_motor_values_apsa( return cmd_apsa.comm_write(motor_values); } +bool ECManager::get_full_feedback_apsa(std::vector &feedback) { + return full_feedback_apsa.comm_read(feedback); +} + uint16 ECManager::transition_ec(uint16 state) { // Get state_string std::string state_string = "Unknown"; 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 index 3eee8dc..017052b 100644 --- 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 @@ -1,3 +1,4 @@ +#include #include #include #include @@ -39,6 +40,20 @@ void MockECManager::cyclic_loop() { // Publish feedback to ROS thread (wait-free) feedback_apsa_.perf_write(mock_positions_); + // Simulate full PDO feedback: analog_input1 = 1Hz sinus (±2.5V range) + // ADC range: 0=Umin(-5V), 65535=Umax(+5V), midpoint=32768(0V) + double t = tick_count_ * period_.count() * 1e-3; + uint16_t analog_sim = static_cast( + 32768.0 + 16384.0 * std::sin(2.0 * M_PI * 1.0 * t)); + + std::vector full_feedback(num_motors_); + for (int i = 0; i < num_motors_; ++i) { + full_feedback[i].position = mock_positions_[i]; + full_feedback[i].analog_input1 = analog_sim; + } + full_feedback_apsa_.perf_write(full_feedback); + + tick_count_++; std::this_thread::sleep_until(next_); } } @@ -59,6 +74,10 @@ bool MockECManager::set_motor_values_apsa(const std::vector& motor_valu 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::sdo_read(uint16_t /*device_id*/, uint16_t /*index*/, uint8_t /*subindex*/, std::vector& value) { value = {0}; 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 index 3b6e956..4aa21bf 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -21,6 +21,14 @@ EthercatNode::EthercatNode(IECManager &ec_manager) 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)); @@ -133,6 +141,33 @@ void EthercatNode::sdoReadServiceCallback( response->value = value; response->value_type = 0; } +void EthercatNode::publishFullFeedback() { + std::vector feedback; + + if (ethercat_enabled_ && ec_manager_.get_full_feedback_apsa(feedback)) { + auto msg = rise_motion_messages::msg::MotorFeedbackFull(); + 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, diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index f27e9c1..996177e 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -13,6 +13,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateCurrentMsg.msg" "msg/StateNoticeMsg.msg" "msg/MotorPositions.msg" + "msg/MotorFeedbackFull.msg" "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" 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..a058e2b --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg @@ -0,0 +1,17 @@ +# All CiA402_Inputs PDO fields as parallel arrays, index = motor +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 From 1ab4135a226749dcd44910f5baf7e71efbb57621 Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 17:58:42 +0100 Subject: [PATCH 53/83] Edit documentation --- docs/development.md | 92 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 83 insertions(+), 9 deletions(-) diff --git a/docs/development.md b/docs/development.md index 19b0a81..9b78640 100644 --- a/docs/development.md +++ b/docs/development.md @@ -6,8 +6,9 @@ 3. [Architekturübersicht](#3-architekturübersicht) 4. [Komponenten](#4-komponenten) 5. [ROS2 Interface](#5-ros2-interface) -6. [Admittanzregelung](#6-admittanzregelung) -7. [Entwicklungsplan](#7-entwicklungsplan) +6. [Datenaufzeichnung](#6-datenaufzeichnung) +7. [Admittanzregelung](#7-admittanzregelung) +8. [Entwicklungsplan](#8-entwicklungsplan) --- @@ -81,7 +82,15 @@ ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ ``` ### Ausführen (ohne Hardware, WSL2) +Zuerst muss gebaut werden: ```bash +cd ~/rise-motion/rise_motion_dev_ws +./build.sh +``` +Dann: +```bash +# In jedem Terminal +cd ~/rise-motion/rise_motion_dev_ws source install/setup.bash # Terminal 1 @@ -328,7 +337,69 @@ uint8[] value --- -## 6. Admittanzregelung +## 6. Datenaufzeichnung + +### ROS2 Bag + +Alle Topics können als `.mcap`-Datei aufgezeichnet und später offline ausgewertet werden – ohne laufende Hardware. + +**Aufzeichnen:** +```bash +cd ~/rise-motion/rise_motion_dev_ws +source install/setup.bash +ros2 bag record /motor_feedback_full /motor_feedback +# Stoppen mit Ctrl+C +# Erstellt automatisch einen Ordner mit Zeitstempel, z.B. rosbag2_2026_03_27_12_00_00/ +``` + +**Abspielen** (simuliert die Topics als wären sie live): +```bash +ros2 bag play rosbag2_2026_03_27_12_00_00/ +# In anderem Terminal: ros2 topic echo /motor_feedback_full +``` + +**Metadaten anzeigen:** +```bash +ros2 bag info rosbag2_2026_03_27_12_00_00/ +# Zeigt: Dauer, Anzahl Messages, Topics, Frequenzen +``` + +### Nützliche Diagnose-Tools + +**Publishrate messen:** +```bash +ros2 topic hz /motor_feedback_full # sollte ~100 Hz zeigen (10ms Timer) +``` + +**Node-Graph anzeigen** (welche Nodes welche Topics nutzen): +```bash +ros2 run rqt_graph rqt_graph +``` + +**PlotJuggler** – GUI zum Plotten von Bag-Dateien und Live-Topics (empfohlen für Admittanz-Tuning): + +Installation (einmalig): +```bash +sudo apt install ros-jazzy-plotjuggler-ros +``` + +Starten (im selben Terminal wie `source install/setup.bash`, sonst werden Message-Typen nicht erkannt): +```bash +ros2 run plotjuggler plotjuggler +``` + +Live-Topics plotten: +1. **Streaming** → `ROS2 Topic Subscriber` → **Start** +2. Links in der Liste erscheinen alle aktiven Topics – z.B. `/motor_feedback_full` aufklappen +3. Felder per Drag&Drop ins Plot-Fenster ziehen (z.B. `analog_input1[0]`) + +Bag-Datei auswerten: +1. **File** → `Load Data` → Bag-Ordner auswählen +2. Felder per Drag&Drop plotten + +--- + +## 7. Admittanzregelung ### Regelungsgesetz ``` @@ -365,7 +436,7 @@ pos_target = pos_current + q ### Offene Punkte - Velocity-Commands an Servo: restlicher RISE-OS Stack arbeitet mit Positionscommands → Integration noch offen -- Geplantes Topic `motor_feedback_full` mit `WrenchMsg` (Woche 2) +- Kalibrierung Kraftsensor: ADC-Ticks → Newton → Nm (Woche 3) ### Safety (minimal) - `v_max`: maximale Gelenkgeschwindigkeit @@ -374,7 +445,7 @@ pos_target = pos_current + q --- -## 7. Entwicklungsplan +## 8. Entwicklungsplan ### Woche 1 – Mock & Grundstruktur - [x] Mock `ECManager` (lokale Tests ohne Hardware) @@ -382,16 +453,19 @@ pos_target = pos_current + q - [ ] Mathematik validieren (Plots) ### Woche 2 – Kraftsensor-Integration (Software) -- [ ] `FeedbackData` Struct erweitern (AnalogInput1/2 aus CiA402 PDO) -- [ ] ECManager: Analog Inputs in `feedback_apsa` durchleiten -- [ ] Neue Message `WrenchMsg.msg` definieren +- [x] `MotorFeedbackData` Struct (alle CiA402_Inputs Felder) +- [x] Zweites APSA `full_feedback_apsa` in ECManager und MockECManager +- [x] Neue Message `MotorFeedbackFull.msg` (alle PDO-Felder als parallele Arrays) +- [x] Neues Topic `/motor_feedback_full` in EthercatNode +- [x] Mock simuliert `analog_input1` als 1Hz-Sinus (ADC-Ticks, ±2.5V) ### Woche 3 – Hardware-Tests - [ ] Echten Kraftsensor testen (Linux-PC, Interface `enp1s0`) -- [ ] Kalibrierung (ADC-Ticks → Newton → Nm) +- [ ] Kalibrierung (ADC-Ticks → Newton → Nm, Offset) - [ ] Erste Admittanz-Tests am Exoskelett ### Woche 4+ – Integration & Tuning +- [ ] `AdmittanzNode` implementieren (subscribed auf `/motor_feedback_full`) - [ ] Parameter M, D, K über ROS2 Parameter Server konfigurierbar - [ ] Integration in RISE-OS Stack (`motor_command_safe`) - [ ] Safety-Logik: v_max, pos_limits, Watchdog, Notaus From 8d1db3f5553f18eb154340430027e40afb055c39 Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 18:49:36 +0100 Subject: [PATCH 54/83] feat: implemented basic admittance --- .../src/rise_motion/CMakeLists.txt | 12 ++ .../src/rise_motion/src/admittance_node.cpp | 177 ++++++++++++++++++ 2 files changed, 189 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 1dd2d48..a778149 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -52,6 +52,18 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) +add_executable(admittance_node + src/admittance_node.cpp +) +target_link_libraries(admittance_node PUBLIC + ${rise_motion_messages_TARGETS} + rclcpp::rclcpp +) +install(TARGETS + admittance_node + DESTINATION lib/${PROJECT_NAME} +) + add_executable(testing_node src/testing_node.cpp ) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp new file mode 100644 index 0000000..7bd180a --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -0,0 +1,177 @@ +#include +#include +#include +#include +#include +#include +#include +#include + +// AdmittanzNode: Implements admittance control law +// +// Input: τ_int (interaction torque via load cell, from analog_input1) +// Output: velocity commands on /motor_commands_vel (MotorPositions as container) +// +// Control law (transparent mode, K_v = 0): +// M_v * v̇ + D_v * v = τ_int +// Euler, dt = 1ms: +// v̇ = (τ_int - D_v * v) / M_v +// v += v̇ * dt +// +// NOTE: Currently publishes to /motor_commands_vel (not wired to motor). +// Wiring to actual motor commands requires enabling CyclicSyncVelocityMode +// on the drive first (TODO Woche 3/4). + +class AdmittanzNode : public rclcpp::Node { +public: + AdmittanzNode() : Node("admittanz_node") { + // Admittance parameters + declare_parameter("M_v", 1.0); + declare_parameter("D_v", 10.0); + declare_parameter("K_v", 0.0); + + // Force sensor calibration: ADC → Voltage → Force → Torque + // V = (adc - offset) / 65536.0 * 10.0 (10V range: -5V..+5V) + // F = V * sensitivity_inv [N] + // τ = F * lever_arm [Nm] + declare_parameter("offset", 32768.0); // ADC ticks at 0V + declare_parameter("sensitivity_inv", 100.0); // N/V (placeholder, calibrate Woche 3) + declare_parameter("lever_arm", 0.1); // m (placeholder) + + // Safety limits + declare_parameter("tau_max", 50.0); // Nm - input clipping + declare_parameter("vel_max", 1000.0); // inc/s - output clipping + declare_parameter("dvel_max", 500.0); // inc/s per step - jerk limit + + feedback_sub_ = + create_subscription( + "motor_feedback_full", 10, + [this](rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { + feedbackCallback(msg); + }); + + cmd_pub_ = create_publisher( + "motor_commands_vel", 10); + + compute_timer_ = create_wall_timer( + std::chrono::milliseconds(1), + std::bind(&AdmittanzNode::computeAdmittance, this)); + + client_ = create_client( + "enable_ethercat"); + + RCLCPP_INFO(get_logger(), "AdmittanzNode initialized (publishing to /motor_commands_vel)"); + } + + ~AdmittanzNode() { RCLCPP_INFO(get_logger(), "AdmittanzNode shutting down"); } + + int request_enable_ethercat() { + RCLCPP_INFO(get_logger(), "Requesting Enable Ethercat"); + while (!client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_logger(), + "client interrupted while waiting for service to appear."); + return 1; + } + RCLCPP_INFO(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(shared_from_this(), result_future) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "service call failed"); + client_->remove_pending_request(result_future); + return 1; + } + return result_future.get()->status_enable; + } + +private: + rclcpp::Subscription::SharedPtr + feedback_sub_; + rclcpp::Publisher::SharedPtr + cmd_pub_; + rclcpp::Client::SharedPtr + client_; + rclcpp::TimerBase::SharedPtr compute_timer_; + + // State per motor + std::vector pos_current_; + std::vector velocity_; + std::vector displacement_; + std::vector analog_input1_; + bool has_feedback_{false}; + + static constexpr double dt_ = 0.001; // 1ms + + void feedbackCallback( + rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { + if (!has_feedback_) { + size_t n = msg->positions.size(); + pos_current_.resize(n, 0); + velocity_.resize(n, 0.0); + displacement_.resize(n, 0.0); + analog_input1_.resize(n, 32768); // init to 0V (midpoint) + has_feedback_ = true; + RCLCPP_INFO(get_logger(), "Got first feedback (%zu motors)", n); + } + pos_current_ = msg->positions; + analog_input1_ = msg->analog_input1; + } + + void computeAdmittance() { + if (!has_feedback_) return; + + // Read parameters every cycle (allows live tuning via ros2 param set) + const double M_v = get_parameter("M_v").as_double(); + const double D_v = get_parameter("D_v").as_double(); + const double K_v = get_parameter("K_v").as_double(); + const double offset = get_parameter("offset").as_double(); + const double sensitivity_inv = get_parameter("sensitivity_inv").as_double(); + const double lever_arm = get_parameter("lever_arm").as_double(); + const double tau_max = get_parameter("tau_max").as_double(); + const double vel_max = get_parameter("vel_max").as_double(); + const double dvel_max = get_parameter("dvel_max").as_double(); + + auto msg = rise_motion_messages::msg::MotorPositions(); + msg.positions.resize(pos_current_.size()); + + for (size_t i = 0; i < pos_current_.size(); ++i) { + // ADC → Voltage → Force → Torque + double V = (static_cast(analog_input1_[i]) - offset) / 65536.0 * 10.0; + double F = V * sensitivity_inv; + double tau = F * lever_arm; + + // Input clipping + tau = std::clamp(tau, -tau_max, tau_max); + + // Admittance Euler integration + double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; + double v_prev = velocity_[i]; + velocity_[i] += vdot * dt_; + displacement_[i] += velocity_[i] * dt_; + + // Output: jerk limit then velocity clamp + velocity_[i] = std::clamp(velocity_[i], + v_prev - dvel_max * dt_, + v_prev + dvel_max * dt_); + velocity_[i] = std::clamp(velocity_[i], -vel_max, vel_max); + + msg.positions[i] = static_cast(velocity_[i]); + } + + cmd_pub_->publish(msg); + } +}; + +int main(int argc, char **argv) { + rclcpp::init(argc, argv); + auto node = std::make_shared(); + while (!node->request_enable_ethercat()) { + } + rclcpp::spin(node); + rclcpp::shutdown(); + return 0; +} From 877f96657eff4362a31fc76025314f6acbe61fed Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 16:15:38 +0100 Subject: [PATCH 55/83] Feat: Implemented forwarding all motor data --- .gitignore | 2 +- docs/development.md | 13 ++++--- .../include/rise_motion/ec_manager.hpp | 6 +++- .../include/rise_motion/ec_manager_mock.hpp | 3 ++ .../include/rise_motion/ethercat_node.hpp | 4 +++ .../include/rise_motion/iec_manager.hpp | 3 ++ .../rise_motion/motor_feedback_data.hpp | 21 +++++++++++ .../src/rise_motion/src/ec_manager.cpp | 23 ++++++++++++ .../src/rise_motion/src/ec_manager_mock.cpp | 19 ++++++++++ .../src/rise_motion/src/ethercat_node.cpp | 35 +++++++++++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../msg/MotorFeedbackFull.msg | 17 +++++++++ 12 files changed, 138 insertions(+), 9 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion/include/rise_motion/motor_feedback_data.hpp create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg diff --git a/.gitignore b/.gitignore index 893fda8..29b3824 100644 --- a/.gitignore +++ b/.gitignore @@ -7,4 +7,4 @@ rosbag2_*/ .claude CLAUDE.md plans/ -SOMANET Circulo Safe Motion.pdf \ No newline at end of file +SOMANET Circulo Safe Motion.pdf diff --git a/docs/development.md b/docs/development.md index 3727c53..19b0a81 100644 --- a/docs/development.md +++ b/docs/development.md @@ -82,20 +82,19 @@ ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ ### Ausführen (ohne Hardware, WSL2) ```bash -# Immer: source install/setup.bash # Terminal 1 ros2 run rise_motion rise_motion_mock -# (Terminal 2) Not needed! -ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" - -# Terminal 3 -ros2 run rise_motion testing_node +# Terminal 2 – testing_node ruft /enable_ethercat automatisch auf +ros2 run rise_motion testing_node 1 -# Terminal 4: Motor position validieren +# Terminal 3 – Motor positions validieren ros2 topic echo /motor_feedback + +# Terminal 4 – Alle PDO-Felder inkl. Kraftsensor (analog_input1 = 1Hz-Sinus im Mock) +ros2 topic echo /motor_feedback_full ``` Der Mock simuliert einen Motor im RAM: Kommandos werden sofort als Feedback zurückgegeben. Kein EtherCAT, kein SOEM, keine Netzwerkrechte erforderlich. 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 index 0a0764d..4576802 100755 --- 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 @@ -25,6 +25,7 @@ class ECManager : public IECManager { // 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; // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread bool sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; @@ -52,6 +53,9 @@ class ECManager : public IECManager { // cmd_apsa: ROS → EtherCAT (motor commands) APSA> cmd_apsa; - // feedback_apsa: EtherCAT → ROS (motor feedback) + // 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; }; 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 index dd36365..3e0ba31 100644 --- 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 @@ -20,6 +20,7 @@ class MockECManager : public IECManager { 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 sdo_read(uint16_t device_id, uint16_t index, uint8_t subindex, std::vector& value) override; @@ -34,10 +35,12 @@ class MockECManager : public IECManager { std::vector mock_positions_; std::vector motor_commands_; + uint64_t tick_count_{0}; std::chrono::time_point next_; const std::chrono::duration> period_; APSA> 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 index f77df11..b915adf 100755 --- 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 @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -21,6 +22,8 @@ class EthercatNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr 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_; @@ -28,6 +31,7 @@ class EthercatNode : public rclcpp::Node { void commandCallback(const rise_motion_messages::msg::MotorPositions::SharedPtr msg); void publishFeedback(); + void publishFullFeedback(); void enableServiceCallback( const 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 index 449062f..b0293aa 100644 --- 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 @@ -2,6 +2,8 @@ #include #include +#include + class IECManager { public: virtual ~IECManager() = default; @@ -13,6 +15,7 @@ class IECManager { 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 sdo_read(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/src/ec_manager.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp index 8042d5c..c74e753 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -104,6 +104,7 @@ void ECManager::cyclic_loop() { std::vector motor_commands(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); + std::vector full_feedback(ctx.slavecount); // Transition to OPERATIONAL // Ethercat needs to be operational before CiA402 is OPERATION_ENABLED @@ -172,6 +173,23 @@ void ECManager::cyclic_loop() { m.outputs->TargetPosition = motor_commands[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" @@ -221,6 +239,7 @@ void ECManager::cyclic_loop() { // 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); @@ -260,6 +279,10 @@ bool ECManager::set_motor_values_apsa( return cmd_apsa.comm_write(motor_values); } +bool ECManager::get_full_feedback_apsa(std::vector &feedback) { + return full_feedback_apsa.comm_read(feedback); +} + uint16 ECManager::transition_ec(uint16 state) { // Get state_string std::string state_string = "Unknown"; 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 index 3eee8dc..017052b 100644 --- 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 @@ -1,3 +1,4 @@ +#include #include #include #include @@ -39,6 +40,20 @@ void MockECManager::cyclic_loop() { // Publish feedback to ROS thread (wait-free) feedback_apsa_.perf_write(mock_positions_); + // Simulate full PDO feedback: analog_input1 = 1Hz sinus (±2.5V range) + // ADC range: 0=Umin(-5V), 65535=Umax(+5V), midpoint=32768(0V) + double t = tick_count_ * period_.count() * 1e-3; + uint16_t analog_sim = static_cast( + 32768.0 + 16384.0 * std::sin(2.0 * M_PI * 1.0 * t)); + + std::vector full_feedback(num_motors_); + for (int i = 0; i < num_motors_; ++i) { + full_feedback[i].position = mock_positions_[i]; + full_feedback[i].analog_input1 = analog_sim; + } + full_feedback_apsa_.perf_write(full_feedback); + + tick_count_++; std::this_thread::sleep_until(next_); } } @@ -59,6 +74,10 @@ bool MockECManager::set_motor_values_apsa(const std::vector& motor_valu 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::sdo_read(uint16_t /*device_id*/, uint16_t /*index*/, uint8_t /*subindex*/, std::vector& value) { value = {0}; 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 index 3b6e956..4aa21bf 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -21,6 +21,14 @@ EthercatNode::EthercatNode(IECManager &ec_manager) 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)); @@ -133,6 +141,33 @@ void EthercatNode::sdoReadServiceCallback( response->value = value; response->value_type = 0; } +void EthercatNode::publishFullFeedback() { + std::vector feedback; + + if (ethercat_enabled_ && ec_manager_.get_full_feedback_apsa(feedback)) { + auto msg = rise_motion_messages::msg::MotorFeedbackFull(); + 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, diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index f27e9c1..996177e 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -13,6 +13,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateCurrentMsg.msg" "msg/StateNoticeMsg.msg" "msg/MotorPositions.msg" + "msg/MotorFeedbackFull.msg" "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" 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..a058e2b --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg @@ -0,0 +1,17 @@ +# All CiA402_Inputs PDO fields as parallel arrays, index = motor +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 From 6aeeb2fd7007a3b52ec03a01b2196ec2a8cb4fd8 Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 17:58:42 +0100 Subject: [PATCH 56/83] Edit documentation --- docs/development.md | 92 ++++++++++++++++++++++++++++++++++++++++----- 1 file changed, 83 insertions(+), 9 deletions(-) diff --git a/docs/development.md b/docs/development.md index 19b0a81..9b78640 100644 --- a/docs/development.md +++ b/docs/development.md @@ -6,8 +6,9 @@ 3. [Architekturübersicht](#3-architekturübersicht) 4. [Komponenten](#4-komponenten) 5. [ROS2 Interface](#5-ros2-interface) -6. [Admittanzregelung](#6-admittanzregelung) -7. [Entwicklungsplan](#7-entwicklungsplan) +6. [Datenaufzeichnung](#6-datenaufzeichnung) +7. [Admittanzregelung](#7-admittanzregelung) +8. [Entwicklungsplan](#8-entwicklungsplan) --- @@ -81,7 +82,15 @@ ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ ``` ### Ausführen (ohne Hardware, WSL2) +Zuerst muss gebaut werden: ```bash +cd ~/rise-motion/rise_motion_dev_ws +./build.sh +``` +Dann: +```bash +# In jedem Terminal +cd ~/rise-motion/rise_motion_dev_ws source install/setup.bash # Terminal 1 @@ -328,7 +337,69 @@ uint8[] value --- -## 6. Admittanzregelung +## 6. Datenaufzeichnung + +### ROS2 Bag + +Alle Topics können als `.mcap`-Datei aufgezeichnet und später offline ausgewertet werden – ohne laufende Hardware. + +**Aufzeichnen:** +```bash +cd ~/rise-motion/rise_motion_dev_ws +source install/setup.bash +ros2 bag record /motor_feedback_full /motor_feedback +# Stoppen mit Ctrl+C +# Erstellt automatisch einen Ordner mit Zeitstempel, z.B. rosbag2_2026_03_27_12_00_00/ +``` + +**Abspielen** (simuliert die Topics als wären sie live): +```bash +ros2 bag play rosbag2_2026_03_27_12_00_00/ +# In anderem Terminal: ros2 topic echo /motor_feedback_full +``` + +**Metadaten anzeigen:** +```bash +ros2 bag info rosbag2_2026_03_27_12_00_00/ +# Zeigt: Dauer, Anzahl Messages, Topics, Frequenzen +``` + +### Nützliche Diagnose-Tools + +**Publishrate messen:** +```bash +ros2 topic hz /motor_feedback_full # sollte ~100 Hz zeigen (10ms Timer) +``` + +**Node-Graph anzeigen** (welche Nodes welche Topics nutzen): +```bash +ros2 run rqt_graph rqt_graph +``` + +**PlotJuggler** – GUI zum Plotten von Bag-Dateien und Live-Topics (empfohlen für Admittanz-Tuning): + +Installation (einmalig): +```bash +sudo apt install ros-jazzy-plotjuggler-ros +``` + +Starten (im selben Terminal wie `source install/setup.bash`, sonst werden Message-Typen nicht erkannt): +```bash +ros2 run plotjuggler plotjuggler +``` + +Live-Topics plotten: +1. **Streaming** → `ROS2 Topic Subscriber` → **Start** +2. Links in der Liste erscheinen alle aktiven Topics – z.B. `/motor_feedback_full` aufklappen +3. Felder per Drag&Drop ins Plot-Fenster ziehen (z.B. `analog_input1[0]`) + +Bag-Datei auswerten: +1. **File** → `Load Data` → Bag-Ordner auswählen +2. Felder per Drag&Drop plotten + +--- + +## 7. Admittanzregelung ### Regelungsgesetz ``` @@ -365,7 +436,7 @@ pos_target = pos_current + q ### Offene Punkte - Velocity-Commands an Servo: restlicher RISE-OS Stack arbeitet mit Positionscommands → Integration noch offen -- Geplantes Topic `motor_feedback_full` mit `WrenchMsg` (Woche 2) +- Kalibrierung Kraftsensor: ADC-Ticks → Newton → Nm (Woche 3) ### Safety (minimal) - `v_max`: maximale Gelenkgeschwindigkeit @@ -374,7 +445,7 @@ pos_target = pos_current + q --- -## 7. Entwicklungsplan +## 8. Entwicklungsplan ### Woche 1 – Mock & Grundstruktur - [x] Mock `ECManager` (lokale Tests ohne Hardware) @@ -382,16 +453,19 @@ pos_target = pos_current + q - [ ] Mathematik validieren (Plots) ### Woche 2 – Kraftsensor-Integration (Software) -- [ ] `FeedbackData` Struct erweitern (AnalogInput1/2 aus CiA402 PDO) -- [ ] ECManager: Analog Inputs in `feedback_apsa` durchleiten -- [ ] Neue Message `WrenchMsg.msg` definieren +- [x] `MotorFeedbackData` Struct (alle CiA402_Inputs Felder) +- [x] Zweites APSA `full_feedback_apsa` in ECManager und MockECManager +- [x] Neue Message `MotorFeedbackFull.msg` (alle PDO-Felder als parallele Arrays) +- [x] Neues Topic `/motor_feedback_full` in EthercatNode +- [x] Mock simuliert `analog_input1` als 1Hz-Sinus (ADC-Ticks, ±2.5V) ### Woche 3 – Hardware-Tests - [ ] Echten Kraftsensor testen (Linux-PC, Interface `enp1s0`) -- [ ] Kalibrierung (ADC-Ticks → Newton → Nm) +- [ ] Kalibrierung (ADC-Ticks → Newton → Nm, Offset) - [ ] Erste Admittanz-Tests am Exoskelett ### Woche 4+ – Integration & Tuning +- [ ] `AdmittanzNode` implementieren (subscribed auf `/motor_feedback_full`) - [ ] Parameter M, D, K über ROS2 Parameter Server konfigurierbar - [ ] Integration in RISE-OS Stack (`motor_command_safe`) - [ ] Safety-Logik: v_max, pos_limits, Watchdog, Notaus From e00238d47875b60c7bb720d0d551b283d9ae6367 Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 20:11:14 +0100 Subject: [PATCH 57/83] added debug message for admittance ctrl --- .../src/rise_motion/src/admittance_node.cpp | 57 +++++++++++++------ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../msg/AdmittanceDebug.msg | 14 +++++ 3 files changed, 56 insertions(+), 16 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index 7bd180a..9a6b913 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -3,6 +3,7 @@ #include #include #include +#include #include #include #include @@ -53,6 +54,9 @@ class AdmittanzNode : public rclcpp::Node { cmd_pub_ = create_publisher( "motor_commands_vel", 10); + debug_pub_ = create_publisher( + "admittance_debug", 10); + compute_timer_ = create_wall_timer( std::chrono::milliseconds(1), std::bind(&AdmittanzNode::computeAdmittance, this)); @@ -93,6 +97,8 @@ class AdmittanzNode : public rclcpp::Node { feedback_sub_; rclcpp::Publisher::SharedPtr cmd_pub_; + rclcpp::Publisher::SharedPtr + debug_pub_; rclcpp::Client::SharedPtr client_; rclcpp::TimerBase::SharedPtr compute_timer_; @@ -135,34 +141,53 @@ class AdmittanzNode : public rclcpp::Node { const double vel_max = get_parameter("vel_max").as_double(); const double dvel_max = get_parameter("dvel_max").as_double(); + const size_t n = pos_current_.size(); auto msg = rise_motion_messages::msg::MotorPositions(); - msg.positions.resize(pos_current_.size()); - - for (size_t i = 0; i < pos_current_.size(); ++i) { + msg.positions.resize(n); + + auto dbg = rise_motion_messages::msg::AdmittanceDebug(); + dbg.adc_voltage.resize(n); + dbg.force.resize(n); + dbg.torque_raw.resize(n); + dbg.torque_clipped.resize(n); + dbg.vdot.resize(n); + dbg.velocity_raw.resize(n); + dbg.velocity_output.resize(n); + dbg.displacement.resize(n); + + for (size_t i = 0; i < n; ++i) { // ADC → Voltage → Force → Torque - double V = (static_cast(analog_input1_[i]) - offset) / 65536.0 * 10.0; - double F = V * sensitivity_inv; - double tau = F * lever_arm; - - // Input clipping - tau = std::clamp(tau, -tau_max, tau_max); + double V = (static_cast(analog_input1_[i]) - offset) / 65536.0 * 10.0; + double F = V * sensitivity_inv; + double tau_raw = F * lever_arm; + double tau = std::clamp(tau_raw, -tau_max, tau_max); // Admittance Euler integration - double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; - double v_prev = velocity_[i]; - velocity_[i] += vdot * dt_; + double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; + double v_prev = velocity_[i]; + velocity_[i] += vdot * dt_; displacement_[i] += velocity_[i] * dt_; // Output: jerk limit then velocity clamp - velocity_[i] = std::clamp(velocity_[i], - v_prev - dvel_max * dt_, - v_prev + dvel_max * dt_); - velocity_[i] = std::clamp(velocity_[i], -vel_max, vel_max); + double v_after_jerk = std::clamp(velocity_[i], + v_prev - dvel_max * dt_, + v_prev + dvel_max * dt_); + velocity_[i] = std::clamp(v_after_jerk, -vel_max, vel_max); msg.positions[i] = static_cast(velocity_[i]); + + dbg.adc_voltage[i] = V; + dbg.force[i] = F; + dbg.torque_raw[i] = tau_raw; + dbg.torque_clipped[i] = tau; + dbg.vdot[i] = vdot; + dbg.velocity_raw[i] = v_prev + vdot * dt_; + dbg.velocity_output[i] = velocity_[i]; + dbg.displacement[i] = displacement_[i]; } cmd_pub_->publish(msg); + debug_pub_->publish(dbg); } }; diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index 996177e..0fa3494 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -14,6 +14,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/StateNoticeMsg.msg" "msg/MotorPositions.msg" "msg/MotorFeedbackFull.msg" + "msg/AdmittanceDebug.msg" "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" 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..f54e28c --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -0,0 +1,14 @@ +# Per-motor debug data for admittance controller +# Arrays are indexed by motor number + +# Force sensor calibration chain +float64[] adc_voltage # [V] ADC ticks -> voltage +float64[] force # [N] voltage -> force +float64[] torque_raw # [Nm] force * lever_arm (before clipping) +float64[] torque_clipped # [Nm] after tau_max clipping + +# Admittance integration +float64[] vdot # [enc-inc/s^2] acceleration +float64[] velocity_raw # [enc-inc/s] after Euler integration (before safety) +float64[] velocity_output # [enc-inc/s] after jerk limit + vel clamp (published value) +float64[] displacement # [enc-inc] integrated displacement (for K_v > 0) From 40deca4fc7c90e4dd8adc037c1c89daf66fd1c07 Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 27 Mar 2026 22:05:06 +0100 Subject: [PATCH 58/83] Improvements to admittance node MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit * admittance_node.cpp: * Publisher-Typ: MotorPositions → MotorVelocity * ADC-Formel: (adc / 65535.0 * 5.0) - 2.5 (0–5V, 2.5V=0N) * Input-Clipping: f_max [N] vor Hebelarm, statt tau_max [Nm] danach * Positionslimits: pos_min/pos_max Parameter, Velocity=0 bei Grenzüberschreitung * offset Parameter entfernt * AdmittanceDebug.msg: torque_clipped → force_clipped, force = raw (vor Clamp) * CMakeLists.txt: MotorVelocity.msg eingetragen --- .../src/rise_motion/src/admittance_node.cpp | 99 +++++++++++-------- .../src/rise_motion_messages/CMakeLists.txt | 1 + .../msg/AdmittanceDebug.msg | 4 +- .../msg/MotorVelocity.msg | 1 + 4 files changed, 63 insertions(+), 42 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index 9a6b913..336b044 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -1,17 +1,18 @@ #include #include #include +#include #include #include #include #include -#include +#include #include // AdmittanzNode: Implements admittance control law // // Input: τ_int (interaction torque via load cell, from analog_input1) -// Output: velocity commands on /motor_commands_vel (MotorPositions as container) +// Output: velocity commands on /motor_commands_vel (MotorVelocity) // // Control law (transparent mode, K_v = 0): // M_v * v̇ + D_v * v = τ_int @@ -19,9 +20,15 @@ // v̇ = (τ_int - D_v * v) / M_v // v += v̇ * dt // +// Force sensor calibration (ADC 0..65535, 0V..5V, 2.5V = 0N): +// V = (adc / 65535.0 * 5.0) - 2.5 [V] +// F = V * sensitivity_inv [N] +// F = clamp(F, -f_max, f_max) [N] - sensor range limit +// τ = F * lever_arm [Nm] +// // NOTE: Currently publishes to /motor_commands_vel (not wired to motor). // Wiring to actual motor commands requires enabling CyclicSyncVelocityMode -// on the drive first (TODO Woche 3/4). +// on the drive first (TODO Woche 4). class AdmittanzNode : public rclcpp::Node { public: @@ -32,18 +39,22 @@ class AdmittanzNode : public rclcpp::Node { declare_parameter("K_v", 0.0); // Force sensor calibration: ADC → Voltage → Force → Torque - // V = (adc - offset) / 65536.0 * 10.0 (10V range: -5V..+5V) - // F = V * sensitivity_inv [N] - // τ = F * lever_arm [Nm] - declare_parameter("offset", 32768.0); // ADC ticks at 0V + // ADC range: 0..65535 → 0V..5V, midpoint 32767.5 = 2.5V = 0N declare_parameter("sensitivity_inv", 100.0); // N/V (placeholder, calibrate Woche 3) declare_parameter("lever_arm", 0.1); // m (placeholder) // Safety limits - declare_parameter("tau_max", 50.0); // Nm - input clipping + declare_parameter("f_max", 500.0); // N - input clipping (sensor range) declare_parameter("vel_max", 1000.0); // inc/s - output clipping declare_parameter("dvel_max", 500.0); // inc/s per step - jerk limit + // Position limits (enc-inc): motor stops if position exceeds these bounds + // Defaults: no effect (full int32 range) + declare_parameter("pos_min", + static_cast(std::numeric_limits::min())); + declare_parameter("pos_max", + static_cast(std::numeric_limits::max())); + feedback_sub_ = create_subscription( "motor_feedback_full", 10, @@ -51,7 +62,7 @@ class AdmittanzNode : public rclcpp::Node { feedbackCallback(msg); }); - cmd_pub_ = create_publisher( + cmd_pub_ = create_publisher( "motor_commands_vel", 10); debug_pub_ = create_publisher( @@ -95,7 +106,7 @@ class AdmittanzNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr feedback_sub_; - rclcpp::Publisher::SharedPtr + rclcpp::Publisher::SharedPtr cmd_pub_; rclcpp::Publisher::SharedPtr debug_pub_; @@ -119,7 +130,7 @@ class AdmittanzNode : public rclcpp::Node { pos_current_.resize(n, 0); velocity_.resize(n, 0.0); displacement_.resize(n, 0.0); - analog_input1_.resize(n, 32768); // init to 0V (midpoint) + analog_input1_.resize(n, 32768); // init to midpoint (0N) has_feedback_ = true; RCLCPP_INFO(get_logger(), "Got first feedback (%zu motors)", n); } @@ -131,25 +142,26 @@ class AdmittanzNode : public rclcpp::Node { if (!has_feedback_) return; // Read parameters every cycle (allows live tuning via ros2 param set) - const double M_v = get_parameter("M_v").as_double(); - const double D_v = get_parameter("D_v").as_double(); - const double K_v = get_parameter("K_v").as_double(); - const double offset = get_parameter("offset").as_double(); + const double M_v = get_parameter("M_v").as_double(); + const double D_v = get_parameter("D_v").as_double(); + const double K_v = get_parameter("K_v").as_double(); const double sensitivity_inv = get_parameter("sensitivity_inv").as_double(); - const double lever_arm = get_parameter("lever_arm").as_double(); - const double tau_max = get_parameter("tau_max").as_double(); - const double vel_max = get_parameter("vel_max").as_double(); - const double dvel_max = get_parameter("dvel_max").as_double(); + const double lever_arm = get_parameter("lever_arm").as_double(); + const double f_max = get_parameter("f_max").as_double(); + const double vel_max = get_parameter("vel_max").as_double(); + const double dvel_max = get_parameter("dvel_max").as_double(); + const auto pos_min = static_cast(get_parameter("pos_min").as_int()); + const auto pos_max = static_cast(get_parameter("pos_max").as_int()); const size_t n = pos_current_.size(); - auto msg = rise_motion_messages::msg::MotorPositions(); - msg.positions.resize(n); + auto msg = rise_motion_messages::msg::MotorVelocity(); + msg.velocities.resize(n); auto dbg = rise_motion_messages::msg::AdmittanceDebug(); dbg.adc_voltage.resize(n); dbg.force.resize(n); + dbg.force_clipped.resize(n); dbg.torque_raw.resize(n); - dbg.torque_clipped.resize(n); dbg.vdot.resize(n); dbg.velocity_raw.resize(n); dbg.velocity_output.resize(n); @@ -157,10 +169,11 @@ class AdmittanzNode : public rclcpp::Node { for (size_t i = 0; i < n; ++i) { // ADC → Voltage → Force → Torque - double V = (static_cast(analog_input1_[i]) - offset) / 65536.0 * 10.0; - double F = V * sensitivity_inv; - double tau_raw = F * lever_arm; - double tau = std::clamp(tau_raw, -tau_max, tau_max); + // ADC: 0..65535 → 0V..5V, 2.5V = 0N (bipolar sensor) + double V = (static_cast(analog_input1_[i]) / 65535.0 * 5.0) - 2.5; + double F_raw = V * sensitivity_inv; + double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] + double tau = F * lever_arm; // [Nm] // Admittance Euler integration double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; @@ -169,21 +182,27 @@ class AdmittanzNode : public rclcpp::Node { displacement_[i] += velocity_[i] * dt_; // Output: jerk limit then velocity clamp - double v_after_jerk = std::clamp(velocity_[i], - v_prev - dvel_max * dt_, - v_prev + dvel_max * dt_); - velocity_[i] = std::clamp(v_after_jerk, -vel_max, vel_max); - - msg.positions[i] = static_cast(velocity_[i]); - - dbg.adc_voltage[i] = V; - dbg.force[i] = F; - dbg.torque_raw[i] = tau_raw; - dbg.torque_clipped[i] = tau; - dbg.vdot[i] = vdot; - dbg.velocity_raw[i] = v_prev + vdot * dt_; + velocity_[i] = std::clamp(velocity_[i], + v_prev - dvel_max * dt_, + v_prev + dvel_max * dt_); + velocity_[i] = std::clamp(velocity_[i], -vel_max, vel_max); + + // Position limits: stop motion toward a breached limit + if ((pos_current_[i] <= pos_min && velocity_[i] < 0.0) || + (pos_current_[i] >= pos_max && velocity_[i] > 0.0)) { + velocity_[i] = 0.0; + } + + msg.velocities[i] = static_cast(velocity_[i]); + + dbg.adc_voltage[i] = V; + dbg.force[i] = F_raw; + dbg.force_clipped[i] = F; + dbg.torque_raw[i] = tau; + dbg.vdot[i] = vdot; + dbg.velocity_raw[i] = v_prev + vdot * dt_; dbg.velocity_output[i] = velocity_[i]; - dbg.displacement[i] = displacement_[i]; + dbg.displacement[i] = displacement_[i]; } cmd_pub_->publish(msg); diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index 0fa3494..a7a7c23 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -15,6 +15,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorPositions.msg" "msg/MotorFeedbackFull.msg" "msg/AdmittanceDebug.msg" + "msg/MotorVelocity.msg" "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" 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 index f54e28c..23bdf52 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -4,8 +4,8 @@ # Force sensor calibration chain float64[] adc_voltage # [V] ADC ticks -> voltage float64[] force # [N] voltage -> force -float64[] torque_raw # [Nm] force * lever_arm (before clipping) -float64[] torque_clipped # [Nm] after tau_max clipping +float64[] force_clipped # [N] after f_max clipping +float64[] torque_raw # [Nm] force_clipped * lever_arm # Admittance integration float64[] vdot # [enc-inc/s^2] acceleration 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..ba3df3c --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg @@ -0,0 +1 @@ +int32[] velocities # velocity commands in enc-inc/s, one per motor From 4cb6466706102963603a8910885c24c24d109436 Mon Sep 17 00:00:00 2001 From: Florian Date: Sun, 29 Mar 2026 15:32:25 +0200 Subject: [PATCH 59/83] Basic admittance working Parameter tuned for example --- .../src/rise_motion/src/admittance_node.cpp | 115 ++++++++++++------ .../src/rise_motion/src/ec_manager_mock.cpp | 6 +- .../msg/AdmittanceDebug.msg | 24 ++-- 3 files changed, 97 insertions(+), 48 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index 336b044..00d3ecc 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -12,13 +12,17 @@ // AdmittanzNode: Implements admittance control law // // Input: τ_int (interaction torque via load cell, from analog_input1) -// Output: velocity commands on /motor_commands_vel (MotorVelocity) +// Output: velocity commands on /motor_commands_vel (MotorVelocity, enc-inc/s) // // Control law (transparent mode, K_v = 0): // M_v * v̇ + D_v * v = τ_int // Euler, dt = 1ms: -// v̇ = (τ_int - D_v * v) / M_v -// v += v̇ * dt +// v̇ = (τ_int - D_v * v) / M_v [rad/s^2] +// v += v̇ * dt [rad/s] +// q += v * dt [rad] +// +// All internal state is in physical units (rad, rad/s, rad/s^2). +// Conversion to enc-inc/s happens only on publish via enc_per_rad. // // Force sensor calibration (ADC 0..65535, 0V..5V, 2.5V = 0N): // V = (adc / 65535.0 * 5.0) - 2.5 [V] @@ -33,22 +37,31 @@ class AdmittanzNode : public rclcpp::Node { public: AdmittanzNode() : Node("admittanz_node") { - // Admittance parameters - declare_parameter("M_v", 1.0); - declare_parameter("D_v", 10.0); - declare_parameter("K_v", 0.0); + // Admittance parameters (SI units) + // M_v: virtual inertia – higher = slower response, more "mass-like" feel + // D_v: virtual damping – higher = smaller velocity at same torque, more resistance + // steady-state: v_eq = tau / D_v + // K_v: virtual stiffness – 0 for transparent mode, >0 adds restoring force to neutral + declare_parameter("M_v", 3.0); // [kg*m^2] realistic knee joint inertia: 0.3..0.8 + declare_parameter("D_v", 10.0); // [Nm*s/rad] v_eq = 25Nm / 5.0 = 5 rad/s (286 deg/s) + declare_parameter("K_v", 0.0); // [Nm/rad] 0 = transparent mode // Force sensor calibration: ADC → Voltage → Force → Torque - // ADC range: 0..65535 → 0V..5V, midpoint 32767.5 = 2.5V = 0N - declare_parameter("sensitivity_inv", 100.0); // N/V (placeholder, calibrate Woche 3) - declare_parameter("lever_arm", 0.1); // m (placeholder) + // ADC range: 0..65535 → 0V..5V, midpoint 2.5V = 0N + declare_parameter("sensitivity_inv", 10.0); // [N/V] placeholder, calibrate Woche 3 + declare_parameter("lever_arm", 0.1); // [m] placeholder + + // Safety limits (SI units) + declare_parameter("f_max", 200.0); // [N] typical cuff load cell range + declare_parameter("vel_max", 3.0); // [rad/s] ~172 deg/s, reasonable for transparent mode + declare_parameter("dvel_max", 10.0); // [rad/s^2] gentle jerk limit, avoids abrupt steps - // Safety limits - declare_parameter("f_max", 500.0); // N - input clipping (sensor range) - declare_parameter("vel_max", 1000.0); // inc/s - output clipping - declare_parameter("dvel_max", 500.0); // inc/s per step - jerk limit + // Encoder conversion: enc-inc per radian (motor-specific, depends on resolution + gear ratio) + // enc_per_rad = ENCODER_RESOLUTION * GEAR_RATIO / (2*pi) + // Default: Hüfte AA (2560 * 160 / 2pi ≈ 65306) + declare_parameter("enc_per_rad", 65306.0); - // Position limits (enc-inc): motor stops if position exceeds these bounds + // Position limits (enc-inc, kept in motor units since pos_current_ comes from PDO) // Defaults: no effect (full int32 range) declare_parameter("pos_min", static_cast(std::numeric_limits::min())); @@ -114,14 +127,19 @@ class AdmittanzNode : public rclcpp::Node { client_; rclcpp::TimerBase::SharedPtr compute_timer_; - // State per motor + // State per motor (all in physical units: rad/s, rad) std::vector pos_current_; - std::vector velocity_; - std::vector displacement_; + std::vector velocity_; // [rad/s] + std::vector displacement_; // [rad] std::vector analog_input1_; bool has_feedback_{false}; - static constexpr double dt_ = 0.001; // 1ms + // Timing: measure actual dt each cycle instead of assuming 1ms + // WSL2 timer jitter can cause dt to vary between 0.5ms and 13ms + std::chrono::steady_clock::time_point last_compute_time_; + bool first_compute_{true}; + static constexpr double dtmin_ = 0.0005; // 0.5ms - ignore spuriously short steps + static constexpr double dtmax_ = 0.005; // 5ms - clamp runaway steps void feedbackCallback( rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { @@ -141,6 +159,17 @@ class AdmittanzNode : public rclcpp::Node { void computeAdmittance() { if (!has_feedback_) return; + // Measure actual dt since last call + auto now = std::chrono::steady_clock::now(); + if (first_compute_) { + last_compute_time_ = now; + first_compute_ = false; + return; + } + double dt = std::chrono::duration(now - last_compute_time_).count(); + last_compute_time_ = now; + dt = std::clamp(dt, dtmin_, dtmax_); + // Read parameters every cycle (allows live tuning via ros2 param set) const double M_v = get_parameter("M_v").as_double(); const double D_v = get_parameter("D_v").as_double(); @@ -150,6 +179,7 @@ class AdmittanzNode : public rclcpp::Node { const double f_max = get_parameter("f_max").as_double(); const double vel_max = get_parameter("vel_max").as_double(); const double dvel_max = get_parameter("dvel_max").as_double(); + const double enc_per_rad = get_parameter("enc_per_rad").as_double(); const auto pos_min = static_cast(get_parameter("pos_min").as_int()); const auto pos_max = static_cast(get_parameter("pos_max").as_int()); @@ -157,6 +187,8 @@ class AdmittanzNode : public rclcpp::Node { auto msg = rise_motion_messages::msg::MotorVelocity(); msg.velocities.resize(n); + constexpr double rad_to_deg = 180.0 / M_PI; + auto dbg = rise_motion_messages::msg::AdmittanceDebug(); dbg.adc_voltage.resize(n); dbg.force.resize(n); @@ -166,43 +198,54 @@ class AdmittanzNode : public rclcpp::Node { dbg.velocity_raw.resize(n); dbg.velocity_output.resize(n); dbg.displacement.resize(n); + dbg.vdot_deg.resize(n); + dbg.velocity_raw_deg.resize(n); + dbg.velocity_output_deg.resize(n); + dbg.displacement_deg.resize(n); for (size_t i = 0; i < n; ++i) { // ADC → Voltage → Force → Torque - // ADC: 0..65535 → 0V..5V, 2.5V = 0N (bipolar sensor) + // ADC: 0..65535 → 0V..5V, midpoint 2.5V = 0N (bipolar sensor) + // Subtract 2.5V so that V=0 means no force. Without this, the controller + // would see F=250N at rest and drive the motor even with no interaction. double V = (static_cast(analog_input1_[i]) / 65535.0 * 5.0) - 2.5; double F_raw = V * sensitivity_inv; double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] double tau = F * lever_arm; // [Nm] - // Admittance Euler integration + // Admittance Euler integration (all in rad/s, rad) double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; double v_prev = velocity_[i]; - velocity_[i] += vdot * dt_; - displacement_[i] += velocity_[i] * dt_; + velocity_[i] += vdot * dt; + displacement_[i] += velocity_[i] * dt; - // Output: jerk limit then velocity clamp + // Output safety: jerk limit then velocity clamp velocity_[i] = std::clamp(velocity_[i], - v_prev - dvel_max * dt_, - v_prev + dvel_max * dt_); + v_prev - dvel_max * dt, + v_prev + dvel_max * dt); velocity_[i] = std::clamp(velocity_[i], -vel_max, vel_max); - // Position limits: stop motion toward a breached limit + // Position limits (enc-inc): stop motion toward a breached limit if ((pos_current_[i] <= pos_min && velocity_[i] < 0.0) || (pos_current_[i] >= pos_max && velocity_[i] > 0.0)) { velocity_[i] = 0.0; } - msg.velocities[i] = static_cast(velocity_[i]); + // Convert rad/s → enc-inc/s for motor command + msg.velocities[i] = static_cast(velocity_[i] * enc_per_rad); - dbg.adc_voltage[i] = V; - dbg.force[i] = F_raw; - dbg.force_clipped[i] = F; - dbg.torque_raw[i] = tau; - dbg.vdot[i] = vdot; - dbg.velocity_raw[i] = v_prev + vdot * dt_; - dbg.velocity_output[i] = velocity_[i]; - dbg.displacement[i] = displacement_[i]; + dbg.adc_voltage[i] = V; + dbg.force[i] = F_raw; + dbg.force_clipped[i] = F; + dbg.torque_raw[i] = tau; + dbg.vdot[i] = vdot; + dbg.velocity_raw[i] = v_prev + vdot * dt; + dbg.velocity_output[i] = velocity_[i]; + dbg.displacement[i] = displacement_[i]; + dbg.vdot_deg[i] = vdot * rad_to_deg; + dbg.velocity_raw_deg[i] = (v_prev + vdot * dt) * rad_to_deg; + dbg.velocity_output_deg[i] = velocity_[i] * rad_to_deg; + dbg.displacement_deg[i] = displacement_[i] * rad_to_deg; } cmd_pub_->publish(msg); 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 index 017052b..98a1282 100644 --- 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 @@ -40,11 +40,11 @@ void MockECManager::cyclic_loop() { // Publish feedback to ROS thread (wait-free) feedback_apsa_.perf_write(mock_positions_); - // Simulate full PDO feedback: analog_input1 = 1Hz sinus (±2.5V range) - // ADC range: 0=Umin(-5V), 65535=Umax(+5V), midpoint=32768(0V) + // Simulate full PDO feedback: analog_input1 = 1Hz sinus (full ADC range) + // ADC range: 0=0V, 65535=5V, midpoint=32767.5=2.5V=0N (bipolar sensor) double t = tick_count_ * period_.count() * 1e-3; uint16_t analog_sim = static_cast( - 32768.0 + 16384.0 * std::sin(2.0 * M_PI * 1.0 * t)); + 32767.5 + 32767.5 * std::sin(2.0 * M_PI * 1.0 * t)); std::vector full_feedback(num_motors_); for (int i = 0; i < num_motors_; ++i) { 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 index 23bdf52..0522cc1 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -2,13 +2,19 @@ # Arrays are indexed by motor number # Force sensor calibration chain -float64[] adc_voltage # [V] ADC ticks -> voltage -float64[] force # [N] voltage -> force -float64[] force_clipped # [N] after f_max clipping -float64[] torque_raw # [Nm] force_clipped * lever_arm +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[] torque_raw # [Nm] force_clipped * lever_arm -> input to controller -# Admittance integration -float64[] vdot # [enc-inc/s^2] acceleration -float64[] velocity_raw # [enc-inc/s] after Euler integration (before safety) -float64[] velocity_output # [enc-inc/s] after jerk limit + vel clamp (published value) -float64[] displacement # [enc-inc] integrated displacement (for K_v > 0) +# 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] From a2f46839b3e74e0762478d407d6a37663f4f01b3 Mon Sep 17 00:00:00 2001 From: Florian Date: Sun, 29 Mar 2026 15:54:11 +0200 Subject: [PATCH 60/83] Reduce control loop to only run when new data is available --- .../src/rise_motion/src/admittance_node.cpp | 31 +++++++++++++------ 1 file changed, 21 insertions(+), 10 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index 00d3ecc..c469f95 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -68,6 +68,14 @@ class AdmittanzNode : public rclcpp::Node { declare_parameter("pos_max", static_cast(std::numeric_limits::max())); + // Control loop timing + // compute_period_ms defines the timer rate (independent of feedback publish rate). + // dt clamps guard against jitter (dtmin) and missed feedback packets (dtmax). + // dtmax=20ms covers one missed 10ms feedback packet. + declare_parameter("compute_period_ms", 1); // [ms] timer period; 1ms = 1kHz + declare_parameter("dt_min_ms", 0.5); // [ms] ignore spuriously short steps + declare_parameter("dt_max_ms", 20.0); // [ms] clamp missed-packet steps + feedback_sub_ = create_subscription( "motor_feedback_full", 10, @@ -81,8 +89,9 @@ class AdmittanzNode : public rclcpp::Node { debug_pub_ = create_publisher( "admittance_debug", 10); + const int period_ms = get_parameter("compute_period_ms").as_int(); compute_timer_ = create_wall_timer( - std::chrono::milliseconds(1), + std::chrono::milliseconds(period_ms), std::bind(&AdmittanzNode::computeAdmittance, this)); client_ = create_client( @@ -133,13 +142,11 @@ class AdmittanzNode : public rclcpp::Node { std::vector displacement_; // [rad] std::vector analog_input1_; bool has_feedback_{false}; + bool new_feedback_available_{false}; // true after each feedbackCallback, cleared by computeAdmittance - // Timing: measure actual dt each cycle instead of assuming 1ms - // WSL2 timer jitter can cause dt to vary between 0.5ms and 13ms + // Timing: measure actual dt each cycle instead of assuming fixed period std::chrono::steady_clock::time_point last_compute_time_; bool first_compute_{true}; - static constexpr double dtmin_ = 0.0005; // 0.5ms - ignore spuriously short steps - static constexpr double dtmax_ = 0.005; // 5ms - clamp runaway steps void feedbackCallback( rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { @@ -152,14 +159,16 @@ class AdmittanzNode : public rclcpp::Node { has_feedback_ = true; RCLCPP_INFO(get_logger(), "Got first feedback (%zu motors)", n); } - pos_current_ = msg->positions; - analog_input1_ = msg->analog_input1; + pos_current_ = msg->positions; + analog_input1_ = msg->analog_input1; + new_feedback_available_ = true; } void computeAdmittance() { - if (!has_feedback_) return; + if (!has_feedback_ || !new_feedback_available_) return; + new_feedback_available_ = false; - // Measure actual dt since last call + // Measure actual dt since last compute auto now = std::chrono::steady_clock::now(); if (first_compute_) { last_compute_time_ = now; @@ -168,7 +177,9 @@ class AdmittanzNode : public rclcpp::Node { } double dt = std::chrono::duration(now - last_compute_time_).count(); last_compute_time_ = now; - dt = std::clamp(dt, dtmin_, dtmax_); + const double dtmin = get_parameter("dt_min_ms").as_double() * 1e-3; + const double dtmax = get_parameter("dt_max_ms").as_double() * 1e-3; + dt = std::clamp(dt, dtmin, dtmax); // Read parameters every cycle (allows live tuning via ros2 param set) const double M_v = get_parameter("M_v").as_double(); From ba5bad2208c066817d05252071dee22dcc1b0303 Mon Sep 17 00:00:00 2001 From: Florian Date: Sun, 29 Mar 2026 16:04:23 +0200 Subject: [PATCH 61/83] fix: safety guards and timestamps for admittance node - Guard against M_v=0 division by zero (RCLCPP_ERROR_THROTTLE) - Reinitialize state vectors on feedback size change - Fix main() loop to respect rclcpp::ok() on Ctrl-C - Add std_msgs/Header to MotorFeedbackFull, MotorVelocity, AdmittanceDebug --- .../src/rise_motion/src/admittance_node.cpp | 27 ++++++++++++++++--- .../src/rise_motion/src/ethercat_node.cpp | 1 + .../src/rise_motion_messages/CMakeLists.txt | 2 ++ .../msg/AdmittanceDebug.msg | 2 ++ .../msg/MotorFeedbackFull.msg | 2 ++ .../msg/MotorVelocity.msg | 1 + 6 files changed, 31 insertions(+), 4 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index c469f95..dac27b8 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -150,17 +150,25 @@ class AdmittanzNode : public rclcpp::Node { void feedbackCallback( rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { + const size_t n = msg->positions.size(); if (!has_feedback_) { - size_t n = msg->positions.size(); pos_current_.resize(n, 0); velocity_.resize(n, 0.0); displacement_.resize(n, 0.0); analog_input1_.resize(n, 32768); // init to midpoint (0N) has_feedback_ = true; RCLCPP_INFO(get_logger(), "Got first feedback (%zu motors)", n); + } else if (n != pos_current_.size()) { + // Motor count changed (e.g. EtherCAT reconnect) — reinitialize state + RCLCPP_WARN(get_logger(), "Feedback size changed %zu → %zu, reinitializing state", + pos_current_.size(), n); + pos_current_.assign(n, 0); + velocity_.assign(n, 0.0); + displacement_.assign(n, 0.0); + analog_input1_.assign(n, 32768); } - pos_current_ = msg->positions; - analog_input1_ = msg->analog_input1; + pos_current_ = msg->positions; + analog_input1_ = msg->analog_input1; new_feedback_available_ = true; } @@ -183,6 +191,11 @@ class AdmittanzNode : public rclcpp::Node { // Read parameters every cycle (allows live tuning via ros2 param set) const double M_v = get_parameter("M_v").as_double(); + if (M_v <= 0.0) { + RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, + "Invalid M_v=%.4f (must be > 0), skipping cycle", M_v); + return; + } const double D_v = get_parameter("D_v").as_double(); const double K_v = get_parameter("K_v").as_double(); const double sensitivity_inv = get_parameter("sensitivity_inv").as_double(); @@ -196,11 +209,13 @@ class AdmittanzNode : public rclcpp::Node { const size_t n = pos_current_.size(); auto msg = rise_motion_messages::msg::MotorVelocity(); + msg.header.stamp = now(); msg.velocities.resize(n); constexpr double rad_to_deg = 180.0 / M_PI; auto dbg = rise_motion_messages::msg::AdmittanceDebug(); + dbg.header.stamp = now(); dbg.adc_voltage.resize(n); dbg.force.resize(n); dbg.force_clipped.resize(n); @@ -267,7 +282,11 @@ class AdmittanzNode : public rclcpp::Node { int main(int argc, char **argv) { rclcpp::init(argc, argv); auto node = std::make_shared(); - while (!node->request_enable_ethercat()) { + while (rclcpp::ok() && !node->request_enable_ethercat()) { + } + if (!rclcpp::ok()) { + rclcpp::shutdown(); + return 1; } rclcpp::spin(node); rclcpp::shutdown(); 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 index 4aa21bf..0fea786 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -146,6 +146,7 @@ void EthercatNode::publishFullFeedback() { 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); diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index a7a7c23..1830272 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -8,6 +8,7 @@ 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" @@ -19,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.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 index 0522cc1..a9306d1 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -1,5 +1,7 @@ # 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 # [V] ADC ticks -> voltage (centered: 0V = no force) 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 index a058e2b..a3583e0 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorFeedbackFull.msg @@ -1,4 +1,6 @@ # 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 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 index ba3df3c..8f873a5 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg @@ -1 +1,2 @@ +std_msgs/Header header # ROS timestamp when command was computed int32[] velocities # velocity commands in enc-inc/s, one per motor From 31742c8f5c9ead79df86944c39d6010a94c8b3fd Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 13:45:35 +0200 Subject: [PATCH 62/83] fix: update timestamp retrieval for motor velocity and debug messages --- rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index dac27b8..868b214 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -209,13 +209,13 @@ class AdmittanzNode : public rclcpp::Node { const size_t n = pos_current_.size(); auto msg = rise_motion_messages::msg::MotorVelocity(); - msg.header.stamp = now(); + msg.header.stamp = get_clock()->now(); msg.velocities.resize(n); constexpr double rad_to_deg = 180.0 / M_PI; auto dbg = rise_motion_messages::msg::AdmittanceDebug(); - dbg.header.stamp = now(); + dbg.header.stamp = get_clock()->now(); dbg.adc_voltage.resize(n); dbg.force.resize(n); dbg.force_clipped.resize(n); From 0f70f2ab3a135d57bd6c9024f1885e0e2b3588f4 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 15:14:52 +0200 Subject: [PATCH 63/83] feat: velocity mode switching via ROS2 service --- .../include/rise_motion/ec_manager.hpp | 11 ++++++- .../include/rise_motion/ec_manager_mock.hpp | 2 ++ .../include/rise_motion/ethercat_node.hpp | 8 +++++ .../include/rise_motion/iec_manager.hpp | 3 ++ .../src/rise_motion/src/ec_manager.cpp | 28 +++++++++++++++-- .../src/rise_motion/src/ec_manager_mock.cpp | 8 +++++ .../src/rise_motion/src/ethercat_node.cpp | 30 +++++++++++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../srv/SetOperationModeSrv.srv | 4 +++ 9 files changed, 92 insertions(+), 3 deletions(-) create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/srv/SetOperationModeSrv.srv 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 index 4576802..9042692 100755 --- 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 @@ -26,6 +26,8 @@ class ECManager : public IECManager { 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; + 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; @@ -50,12 +52,19 @@ class ECManager : public IECManager { const std::chrono::duration> period; // period in ms // APSA instances for lock-free communication - // cmd_apsa: ROS → EtherCAT (motor commands) + // cmd_apsa: ROS → EtherCAT (position commands, Mode 8) APSA> cmd_apsa; + // vel_cmd_apsa: ROS → EtherCAT (velocity commands, Mode 9) + APSA> vel_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 + // 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 index 3e0ba31..01a1ed0 100644 --- 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 @@ -21,6 +21,8 @@ class MockECManager : public IECManager { 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; + 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; 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 index b915adf..6f7c059 100755 --- 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 @@ -4,9 +4,11 @@ #include #include #include +#include #include #include #include +#include #include class EthercatNode : public rclcpp::Node { @@ -20,6 +22,7 @@ class EthercatNode : public rclcpp::Node { bool ethercat_enabled_{false}; rclcpp::Subscription::SharedPtr cmd_sub_; + rclcpp::Subscription::SharedPtr cmd_vel_sub_; rclcpp::Publisher::SharedPtr feedback_pub_; rclcpp::TimerBase::SharedPtr feedback_timer_; rclcpp::Publisher::SharedPtr full_feedback_pub_; @@ -27,9 +30,11 @@ class EthercatNode : public rclcpp::Node { 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 publishFeedback(); void publishFullFeedback(); void enableServiceCallback( @@ -45,4 +50,7 @@ class EthercatNode : public rclcpp::Node { 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 index b0293aa..cfe7300 100644 --- 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 @@ -17,6 +17,9 @@ class IECManager { 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 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, 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 index c74e753..2c5962c 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -103,6 +103,7 @@ void ECManager::cyclic_loop() { running_ = true; std::vector motor_commands(ctx.slavecount, 0); + std::vector motor_velocities(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); std::vector full_feedback(ctx.slavecount); @@ -168,9 +169,23 @@ void ECManager::cyclic_loop() { return; } - // Try to get new data + // 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); - m.outputs->TargetPosition = motor_commands[i]; + vel_cmd_apsa.perf_read(motor_velocities); + 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->TargetPosition = m.inputs->PositionValue; + } else if (target == 9) { // CyclicSyncVelocityMode + m.outputs->TargetVelocity = motor_velocities[i]; + } else { // CyclicSyncPositionMode (default) + m.outputs->TargetPosition = motor_commands[i]; + } motor_feedback[i] = m.inputs->PositionValue; full_feedback[i].statusword = m.inputs->Statusword; @@ -283,6 +298,15 @@ 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); +} + +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"; 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 index 98a1282..f580fc4 100644 --- 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 @@ -78,6 +78,14 @@ bool MockECManager::get_full_feedback_apsa(std::vector& feedb return full_feedback_apsa_.comm_read(feedback); } +bool MockECManager::set_motor_velocity_apsa(const std::vector& /*velocities*/) { + 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}; 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 index 0fea786..d04fc89 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -14,6 +14,10 @@ EthercatNode::EthercatNode(IECManager &ec_manager) "motor_commands", 10, std::bind(&EthercatNode::commandCallback, this, _1)); + cmd_vel_sub_ = create_subscription( + "motor_commands_vel", 10, + std::bind(&EthercatNode::velocityCommandCallback, this, _1)); + feedback_pub_ = create_publisher( "motor_feedback", 10); @@ -33,6 +37,10 @@ EthercatNode::EthercatNode(IECManager &ec_manager) "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)); @@ -141,6 +149,28 @@ void EthercatNode::sdoReadServiceCallback( 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::setOperationModeCallback( + std::shared_ptr request, + std::shared_ptr response) { + if (!ethercat_enabled_) { + response->success = false; + response->message = "EtherCAT not enabled"; + return; + } + ec_manager_.set_operation_mode(request->mode); + response->success = true; + response->message = request->mode == 9 ? "Velocity mode active" : "Position mode active"; + RCLCPP_INFO(get_logger(), "Operation mode set to %d", request->mode); +} + void EthercatNode::publishFullFeedback() { std::vector feedback; diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index 1830272..a6b1dcc 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -20,6 +20,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" + "srv/SetOperationModeSrv.srv" DEPENDENCIES std_msgs ) 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 From 316e3a871a0416463e7d91473dcfa835d5d3925c Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 15:29:55 +0200 Subject: [PATCH 64/83] feat: add mode validation, only accept postition and velocities --- rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp | 8 +++++++- 1 file changed, 7 insertions(+), 1 deletion(-) 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 index d04fc89..d92bdb5 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -165,9 +165,15 @@ void EthercatNode::setOperationModeCallback( response->message = "EtherCAT not enabled"; return; } + if (request->mode != 8 && request->mode != 9) { + response->success = false; + response->message = "Invalid mode " + std::to_string(request->mode) + ". Only mode 8 (CyclicSyncPosition) and 9 (CyclicSyncVelocity) are supported."; + RCLCPP_WARN(get_logger(), "Rejected unsupported operation mode %d", request->mode); + return; + } ec_manager_.set_operation_mode(request->mode); response->success = true; - response->message = request->mode == 9 ? "Velocity mode active" : "Position mode active"; + response->message = request->mode == 9 ? "Velocity mode active (mode 9)" : "Position mode active (mode 8)"; RCLCPP_INFO(get_logger(), "Operation mode set to %d", request->mode); } From 1c79d09657d540905e68e7fe510a28f1533361a6 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 17:37:27 +0200 Subject: [PATCH 65/83] Feat: Add fake sensor input for admittance node --- .../src/rise_motion/CMakeLists.txt | 2 ++ .../src/rise_motion/src/admittance_node.cpp | 29 ++++++++++++++++--- 2 files changed, 27 insertions(+), 4 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index a778149..736f937 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -9,6 +9,7 @@ endif() find_package(ament_cmake REQUIRED) find_package(rclcpp REQUIRED) find_package(rise_motion_messages REQUIRED) +find_package(std_msgs REQUIRED) include_directories(include) @@ -57,6 +58,7 @@ add_executable(admittance_node ) target_link_libraries(admittance_node PUBLIC ${rise_motion_messages_TARGETS} + ${std_msgs_TARGETS} rclcpp::rclcpp ) install(TARGETS diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index 868b214..d027a13 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -1,4 +1,5 @@ #include +#include #include #include #include @@ -8,6 +9,7 @@ #include #include #include +#include // AdmittanzNode: Implements admittance control law // @@ -76,6 +78,16 @@ class AdmittanzNode : public rclcpp::Node { declare_parameter("dt_min_ms", 0.5); // [ms] ignore spuriously short steps declare_parameter("dt_max_ms", 20.0); // [ms] clamp missed-packet steps + // Fake sensor override: if true, /force_override topic (std_msgs/Float32, [N]) + // replaces analog_input1 for all motors. Use for testing without a real load cell. + declare_parameter("use_force_override", false); + + force_override_sub_ = create_subscription( + "force_override", 10, + [this](std_msgs::msg::Float32::SharedPtr msg) { + force_override_value_ = msg->data; + }); + feedback_sub_ = create_subscription( "motor_feedback_full", 10, @@ -128,6 +140,8 @@ class AdmittanzNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr feedback_sub_; + rclcpp::Subscription::SharedPtr force_override_sub_; + std::atomic force_override_value_{0.0f}; rclcpp::Publisher::SharedPtr cmd_pub_; rclcpp::Publisher::SharedPtr @@ -234,10 +248,17 @@ class AdmittanzNode : public rclcpp::Node { // ADC: 0..65535 → 0V..5V, midpoint 2.5V = 0N (bipolar sensor) // Subtract 2.5V so that V=0 means no force. Without this, the controller // would see F=250N at rest and drive the motor even with no interaction. - double V = (static_cast(analog_input1_[i]) / 65535.0 * 5.0) - 2.5; - double F_raw = V * sensitivity_inv; - double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] - double tau = F * lever_arm; // [Nm] + double V; + double F_raw; + if (get_parameter("use_force_override").as_bool()) { + F_raw = static_cast(force_override_value_.load()); + V = F_raw / sensitivity_inv; // back-calculate for debug display + } else { + V = (static_cast(analog_input1_[i]) / 65535.0 * 5.0) - 2.5; + F_raw = V * sensitivity_inv; + } + double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] + double tau = F * lever_arm; // [Nm] // Admittance Euler integration (all in rad/s, rad) double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; From 67c9ede6ccf1153c49fa2eebcbbefaf521c91d97 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 17:55:00 +0200 Subject: [PATCH 66/83] Change override for sensor data from force to adc ticks to test the whole thing --- .../src/rise_motion/src/admittance_node.cpp | 35 +++++++++---------- 1 file changed, 17 insertions(+), 18 deletions(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index d027a13..df0aba3 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -9,7 +9,7 @@ #include #include #include -#include +#include // AdmittanzNode: Implements admittance control law // @@ -78,14 +78,17 @@ class AdmittanzNode : public rclcpp::Node { declare_parameter("dt_min_ms", 0.5); // [ms] ignore spuriously short steps declare_parameter("dt_max_ms", 20.0); // [ms] clamp missed-packet steps - // Fake sensor override: if true, /force_override topic (std_msgs/Float32, [N]) - // replaces analog_input1 for all motors. Use for testing without a real load cell. + // Fake sensor override: if true, /analog_input_override topic (std_msgs/UInt16, ADC ticks 0..65535) + // replaces analog_input1 for all motors. Same format as the real sensor — full calibration pipeline active. + // 0 = 0V = -2.5V after offset = max negative force + // 32768 = 2.5V = 0N (neutral) + // 65535 = 5V = max positive force declare_parameter("use_force_override", false); - force_override_sub_ = create_subscription( - "force_override", 10, - [this](std_msgs::msg::Float32::SharedPtr msg) { - force_override_value_ = msg->data; + analog_override_sub_ = create_subscription( + "analog_input_override", 10, + [this](std_msgs::msg::UInt16::SharedPtr msg) { + analog_override_value_ = msg->data; }); feedback_sub_ = @@ -140,8 +143,8 @@ class AdmittanzNode : public rclcpp::Node { private: rclcpp::Subscription::SharedPtr feedback_sub_; - rclcpp::Subscription::SharedPtr force_override_sub_; - std::atomic force_override_value_{0.0f}; + rclcpp::Subscription::SharedPtr analog_override_sub_; + std::atomic analog_override_value_{32768}; // default: 2.5V = 0N rclcpp::Publisher::SharedPtr cmd_pub_; rclcpp::Publisher::SharedPtr @@ -248,15 +251,11 @@ class AdmittanzNode : public rclcpp::Node { // ADC: 0..65535 → 0V..5V, midpoint 2.5V = 0N (bipolar sensor) // Subtract 2.5V so that V=0 means no force. Without this, the controller // would see F=250N at rest and drive the motor even with no interaction. - double V; - double F_raw; - if (get_parameter("use_force_override").as_bool()) { - F_raw = static_cast(force_override_value_.load()); - V = F_raw / sensitivity_inv; // back-calculate for debug display - } else { - V = (static_cast(analog_input1_[i]) / 65535.0 * 5.0) - 2.5; - F_raw = V * sensitivity_inv; - } + const uint16_t adc = get_parameter("use_force_override").as_bool() + ? analog_override_value_.load() + : analog_input1_[i]; + double V = (static_cast(adc) / 65535.0 * 5.0) - 2.5; + double F_raw = V * sensitivity_inv; double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] double tau = F * lever_arm; // [Nm] From a536f4bd09a4b4bb0c75a516fc3ef497c0b0d73d Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 17:56:23 +0200 Subject: [PATCH 67/83] Feat: Admttanz node setzt operation mode beim start auf velocity --- .../src/rise_motion/src/admittance_node.cpp | 35 +++++++++++++++++++ 1 file changed, 35 insertions(+) diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp index df0aba3..d1f874b 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp @@ -9,6 +9,7 @@ #include #include #include +#include #include // AdmittanzNode: Implements admittance control law @@ -111,6 +112,8 @@ class AdmittanzNode : public rclcpp::Node { client_ = create_client( "enable_ethercat"); + mode_client_ = create_client( + "set_operation_mode"); RCLCPP_INFO(get_logger(), "AdmittanzNode initialized (publishing to /motor_commands_vel)"); } @@ -140,6 +143,32 @@ class AdmittanzNode : public rclcpp::Node { return result_future.get()->status_enable; } + bool request_set_velocity_mode() { + while (!mode_client_->wait_for_service(std::chrono::seconds(1))) { + if (!rclcpp::ok()) { + RCLCPP_ERROR(get_logger(), "Interrupted waiting for set_operation_mode service."); + return false; + } + RCLCPP_INFO(get_logger(), "Waiting for set_operation_mode service..."); + } + auto request = std::make_shared(); + request->mode = 9; + auto result_future = mode_client_->async_send_request(request); + if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != + rclcpp::FutureReturnCode::SUCCESS) { + RCLCPP_ERROR(get_logger(), "set_operation_mode service call failed"); + mode_client_->remove_pending_request(result_future); + return false; + } + auto result = result_future.get(); + if (!result->success) { + RCLCPP_ERROR(get_logger(), "set_operation_mode rejected: %s", result->message.c_str()); + return false; + } + RCLCPP_INFO(get_logger(), "Velocity mode (mode 9) active"); + return true; + } + private: rclcpp::Subscription::SharedPtr feedback_sub_; @@ -151,6 +180,8 @@ class AdmittanzNode : public rclcpp::Node { debug_pub_; rclcpp::Client::SharedPtr client_; + rclcpp::Client::SharedPtr + mode_client_; rclcpp::TimerBase::SharedPtr compute_timer_; // State per motor (all in physical units: rad/s, rad) @@ -308,6 +339,10 @@ int main(int argc, char **argv) { rclcpp::shutdown(); return 1; } + if (!node->request_set_velocity_mode()) { + rclcpp::shutdown(); + return 1; + } rclcpp::spin(node); rclcpp::shutdown(); return 0; From 65fe72e7434f055c248d95183fb927b7036e8bf8 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 8 Apr 2026 18:23:04 +0200 Subject: [PATCH 68/83] Add python script to simulate adc reading --- .../scripts/sine_force_publisher.py | 74 +++++++++++++++++++ 1 file changed, 74 insertions(+) create mode 100755 rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py diff --git a/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py b/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py new file mode 100755 index 0000000..bb6410c --- /dev/null +++ b/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py @@ -0,0 +1,74 @@ +#!/usr/bin/env python3 +""" +Publishes a sine wave on /analog_input_override (std_msgs/UInt16, ADC ticks). + +ADC range: 0..65535 + 0 = 0V = max negative force + 32768 = 2.5V = 0N (neutral) + 65535 = 5V = max positive force + +Usage: + python3 sine_force_publisher.py [amplitude_n] [frequency_hz] + + amplitude_n: Force amplitude in N (default: 25.0) + frequency_hz: Sine frequency in Hz (default: 0.5) + +Examples: + python3 sine_force_publisher.py # 25N @ 0.5Hz + python3 sine_force_publisher.py 10 1.0 # 10N @ 1Hz +""" + +import sys +import math +import time +import rclpy +from rclpy.node import Node +from std_msgs.msg import UInt16 + +# ADC calibration (must match admittance_node parameters) +SENSITIVITY_INV = 10.0 # [N/V] — same default as admittance_node +ADC_MAX = 65535 +V_RANGE = 5.0 +V_OFFSET = 2.5 # 2.5V = 0N + + +def force_to_adc(force_n: float) -> int: + """Convert force [N] to ADC ticks.""" + voltage = force_n / SENSITIVITY_INV + V_OFFSET + adc = int(voltage / V_RANGE * ADC_MAX) + return max(0, min(ADC_MAX, adc)) + + +def main(): + amplitude_n = float(sys.argv[1]) if len(sys.argv) > 1 else 25.0 + frequency_hz = float(sys.argv[2]) if len(sys.argv) > 2 else 0.5 + + rclpy.init() + node = Node('sine_force_publisher') + pub = node.create_publisher(UInt16, 'analog_input_override', 10) + + node.get_logger().info( + f"Publishing sine: amplitude={amplitude_n}N, frequency={frequency_hz}Hz" + ) + node.get_logger().info( + f"ADC range: {force_to_adc(-amplitude_n)} .. {force_to_adc(amplitude_n)} " + f"(neutral=32768)" + ) + + dt = 0.01 # 100Hz publish rate + t = 0.0 + msg = UInt16() + + while rclpy.ok(): + force = amplitude_n * math.sin(2.0 * math.pi * frequency_hz * t) + msg.data = force_to_adc(force) + pub.publish(msg) + rclpy.spin_once(node, timeout_sec=0) + time.sleep(dt) + t += dt + + rclpy.shutdown() + + +if __name__ == '__main__': + main() From 7ddfe4d8a8e20c37ca0a2beec129eb4aab57a860 Mon Sep 17 00:00:00 2001 From: Florian Date: Thu, 9 Apr 2026 17:33:20 +0200 Subject: [PATCH 69/83] Remove admittance node to keep rise-motion clean --- .../src/rise_motion/CMakeLists.txt | 12 - .../scripts/sine_force_publisher.py | 74 ---- .../src/rise_motion/src/admittance_node.cpp | 349 ------------------ 3 files changed, 435 deletions(-) delete mode 100755 rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py delete mode 100644 rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt index 736f937..693ddb1 100644 --- a/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion/CMakeLists.txt @@ -53,18 +53,6 @@ install(TARGETS DESTINATION lib/${PROJECT_NAME} ) -add_executable(admittance_node - src/admittance_node.cpp -) -target_link_libraries(admittance_node PUBLIC - ${rise_motion_messages_TARGETS} - ${std_msgs_TARGETS} - rclcpp::rclcpp -) -install(TARGETS - admittance_node - DESTINATION lib/${PROJECT_NAME} -) add_executable(testing_node src/testing_node.cpp diff --git a/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py b/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py deleted file mode 100755 index bb6410c..0000000 --- a/rise_motion_dev_ws/src/rise_motion/scripts/sine_force_publisher.py +++ /dev/null @@ -1,74 +0,0 @@ -#!/usr/bin/env python3 -""" -Publishes a sine wave on /analog_input_override (std_msgs/UInt16, ADC ticks). - -ADC range: 0..65535 - 0 = 0V = max negative force - 32768 = 2.5V = 0N (neutral) - 65535 = 5V = max positive force - -Usage: - python3 sine_force_publisher.py [amplitude_n] [frequency_hz] - - amplitude_n: Force amplitude in N (default: 25.0) - frequency_hz: Sine frequency in Hz (default: 0.5) - -Examples: - python3 sine_force_publisher.py # 25N @ 0.5Hz - python3 sine_force_publisher.py 10 1.0 # 10N @ 1Hz -""" - -import sys -import math -import time -import rclpy -from rclpy.node import Node -from std_msgs.msg import UInt16 - -# ADC calibration (must match admittance_node parameters) -SENSITIVITY_INV = 10.0 # [N/V] — same default as admittance_node -ADC_MAX = 65535 -V_RANGE = 5.0 -V_OFFSET = 2.5 # 2.5V = 0N - - -def force_to_adc(force_n: float) -> int: - """Convert force [N] to ADC ticks.""" - voltage = force_n / SENSITIVITY_INV + V_OFFSET - adc = int(voltage / V_RANGE * ADC_MAX) - return max(0, min(ADC_MAX, adc)) - - -def main(): - amplitude_n = float(sys.argv[1]) if len(sys.argv) > 1 else 25.0 - frequency_hz = float(sys.argv[2]) if len(sys.argv) > 2 else 0.5 - - rclpy.init() - node = Node('sine_force_publisher') - pub = node.create_publisher(UInt16, 'analog_input_override', 10) - - node.get_logger().info( - f"Publishing sine: amplitude={amplitude_n}N, frequency={frequency_hz}Hz" - ) - node.get_logger().info( - f"ADC range: {force_to_adc(-amplitude_n)} .. {force_to_adc(amplitude_n)} " - f"(neutral=32768)" - ) - - dt = 0.01 # 100Hz publish rate - t = 0.0 - msg = UInt16() - - while rclpy.ok(): - force = amplitude_n * math.sin(2.0 * math.pi * frequency_hz * t) - msg.data = force_to_adc(force) - pub.publish(msg) - rclpy.spin_once(node, timeout_sec=0) - time.sleep(dt) - t += dt - - rclpy.shutdown() - - -if __name__ == '__main__': - main() diff --git a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp b/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp deleted file mode 100644 index d1f874b..0000000 --- a/rise_motion_dev_ws/src/rise_motion/src/admittance_node.cpp +++ /dev/null @@ -1,349 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -// AdmittanzNode: Implements admittance control law -// -// Input: τ_int (interaction torque via load cell, from analog_input1) -// Output: velocity commands on /motor_commands_vel (MotorVelocity, enc-inc/s) -// -// Control law (transparent mode, K_v = 0): -// M_v * v̇ + D_v * v = τ_int -// Euler, dt = 1ms: -// v̇ = (τ_int - D_v * v) / M_v [rad/s^2] -// v += v̇ * dt [rad/s] -// q += v * dt [rad] -// -// All internal state is in physical units (rad, rad/s, rad/s^2). -// Conversion to enc-inc/s happens only on publish via enc_per_rad. -// -// Force sensor calibration (ADC 0..65535, 0V..5V, 2.5V = 0N): -// V = (adc / 65535.0 * 5.0) - 2.5 [V] -// F = V * sensitivity_inv [N] -// F = clamp(F, -f_max, f_max) [N] - sensor range limit -// τ = F * lever_arm [Nm] -// -// NOTE: Currently publishes to /motor_commands_vel (not wired to motor). -// Wiring to actual motor commands requires enabling CyclicSyncVelocityMode -// on the drive first (TODO Woche 4). - -class AdmittanzNode : public rclcpp::Node { -public: - AdmittanzNode() : Node("admittanz_node") { - // Admittance parameters (SI units) - // M_v: virtual inertia – higher = slower response, more "mass-like" feel - // D_v: virtual damping – higher = smaller velocity at same torque, more resistance - // steady-state: v_eq = tau / D_v - // K_v: virtual stiffness – 0 for transparent mode, >0 adds restoring force to neutral - declare_parameter("M_v", 3.0); // [kg*m^2] realistic knee joint inertia: 0.3..0.8 - declare_parameter("D_v", 10.0); // [Nm*s/rad] v_eq = 25Nm / 5.0 = 5 rad/s (286 deg/s) - declare_parameter("K_v", 0.0); // [Nm/rad] 0 = transparent mode - - // Force sensor calibration: ADC → Voltage → Force → Torque - // ADC range: 0..65535 → 0V..5V, midpoint 2.5V = 0N - declare_parameter("sensitivity_inv", 10.0); // [N/V] placeholder, calibrate Woche 3 - declare_parameter("lever_arm", 0.1); // [m] placeholder - - // Safety limits (SI units) - declare_parameter("f_max", 200.0); // [N] typical cuff load cell range - declare_parameter("vel_max", 3.0); // [rad/s] ~172 deg/s, reasonable for transparent mode - declare_parameter("dvel_max", 10.0); // [rad/s^2] gentle jerk limit, avoids abrupt steps - - // Encoder conversion: enc-inc per radian (motor-specific, depends on resolution + gear ratio) - // enc_per_rad = ENCODER_RESOLUTION * GEAR_RATIO / (2*pi) - // Default: Hüfte AA (2560 * 160 / 2pi ≈ 65306) - declare_parameter("enc_per_rad", 65306.0); - - // Position limits (enc-inc, kept in motor units since pos_current_ comes from PDO) - // Defaults: no effect (full int32 range) - declare_parameter("pos_min", - static_cast(std::numeric_limits::min())); - declare_parameter("pos_max", - static_cast(std::numeric_limits::max())); - - // Control loop timing - // compute_period_ms defines the timer rate (independent of feedback publish rate). - // dt clamps guard against jitter (dtmin) and missed feedback packets (dtmax). - // dtmax=20ms covers one missed 10ms feedback packet. - declare_parameter("compute_period_ms", 1); // [ms] timer period; 1ms = 1kHz - declare_parameter("dt_min_ms", 0.5); // [ms] ignore spuriously short steps - declare_parameter("dt_max_ms", 20.0); // [ms] clamp missed-packet steps - - // Fake sensor override: if true, /analog_input_override topic (std_msgs/UInt16, ADC ticks 0..65535) - // replaces analog_input1 for all motors. Same format as the real sensor — full calibration pipeline active. - // 0 = 0V = -2.5V after offset = max negative force - // 32768 = 2.5V = 0N (neutral) - // 65535 = 5V = max positive force - declare_parameter("use_force_override", false); - - analog_override_sub_ = create_subscription( - "analog_input_override", 10, - [this](std_msgs::msg::UInt16::SharedPtr msg) { - analog_override_value_ = msg->data; - }); - - feedback_sub_ = - create_subscription( - "motor_feedback_full", 10, - [this](rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { - feedbackCallback(msg); - }); - - cmd_pub_ = create_publisher( - "motor_commands_vel", 10); - - debug_pub_ = create_publisher( - "admittance_debug", 10); - - const int period_ms = get_parameter("compute_period_ms").as_int(); - compute_timer_ = create_wall_timer( - std::chrono::milliseconds(period_ms), - std::bind(&AdmittanzNode::computeAdmittance, this)); - - client_ = create_client( - "enable_ethercat"); - mode_client_ = create_client( - "set_operation_mode"); - - RCLCPP_INFO(get_logger(), "AdmittanzNode initialized (publishing to /motor_commands_vel)"); - } - - ~AdmittanzNode() { RCLCPP_INFO(get_logger(), "AdmittanzNode shutting down"); } - - int request_enable_ethercat() { - RCLCPP_INFO(get_logger(), "Requesting Enable Ethercat"); - while (!client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(get_logger(), - "client interrupted while waiting for service to appear."); - return 1; - } - RCLCPP_INFO(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(shared_from_this(), result_future) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(get_logger(), "service call failed"); - client_->remove_pending_request(result_future); - return 1; - } - return result_future.get()->status_enable; - } - - bool request_set_velocity_mode() { - while (!mode_client_->wait_for_service(std::chrono::seconds(1))) { - if (!rclcpp::ok()) { - RCLCPP_ERROR(get_logger(), "Interrupted waiting for set_operation_mode service."); - return false; - } - RCLCPP_INFO(get_logger(), "Waiting for set_operation_mode service..."); - } - auto request = std::make_shared(); - request->mode = 9; - auto result_future = mode_client_->async_send_request(request); - if (rclcpp::spin_until_future_complete(shared_from_this(), result_future) != - rclcpp::FutureReturnCode::SUCCESS) { - RCLCPP_ERROR(get_logger(), "set_operation_mode service call failed"); - mode_client_->remove_pending_request(result_future); - return false; - } - auto result = result_future.get(); - if (!result->success) { - RCLCPP_ERROR(get_logger(), "set_operation_mode rejected: %s", result->message.c_str()); - return false; - } - RCLCPP_INFO(get_logger(), "Velocity mode (mode 9) active"); - return true; - } - -private: - rclcpp::Subscription::SharedPtr - feedback_sub_; - rclcpp::Subscription::SharedPtr analog_override_sub_; - std::atomic analog_override_value_{32768}; // default: 2.5V = 0N - rclcpp::Publisher::SharedPtr - cmd_pub_; - rclcpp::Publisher::SharedPtr - debug_pub_; - rclcpp::Client::SharedPtr - client_; - rclcpp::Client::SharedPtr - mode_client_; - rclcpp::TimerBase::SharedPtr compute_timer_; - - // State per motor (all in physical units: rad/s, rad) - std::vector pos_current_; - std::vector velocity_; // [rad/s] - std::vector displacement_; // [rad] - std::vector analog_input1_; - bool has_feedback_{false}; - bool new_feedback_available_{false}; // true after each feedbackCallback, cleared by computeAdmittance - - // Timing: measure actual dt each cycle instead of assuming fixed period - std::chrono::steady_clock::time_point last_compute_time_; - bool first_compute_{true}; - - void feedbackCallback( - rise_motion_messages::msg::MotorFeedbackFull::SharedPtr msg) { - const size_t n = msg->positions.size(); - if (!has_feedback_) { - pos_current_.resize(n, 0); - velocity_.resize(n, 0.0); - displacement_.resize(n, 0.0); - analog_input1_.resize(n, 32768); // init to midpoint (0N) - has_feedback_ = true; - RCLCPP_INFO(get_logger(), "Got first feedback (%zu motors)", n); - } else if (n != pos_current_.size()) { - // Motor count changed (e.g. EtherCAT reconnect) — reinitialize state - RCLCPP_WARN(get_logger(), "Feedback size changed %zu → %zu, reinitializing state", - pos_current_.size(), n); - pos_current_.assign(n, 0); - velocity_.assign(n, 0.0); - displacement_.assign(n, 0.0); - analog_input1_.assign(n, 32768); - } - pos_current_ = msg->positions; - analog_input1_ = msg->analog_input1; - new_feedback_available_ = true; - } - - void computeAdmittance() { - if (!has_feedback_ || !new_feedback_available_) return; - new_feedback_available_ = false; - - // Measure actual dt since last compute - auto now = std::chrono::steady_clock::now(); - if (first_compute_) { - last_compute_time_ = now; - first_compute_ = false; - return; - } - double dt = std::chrono::duration(now - last_compute_time_).count(); - last_compute_time_ = now; - const double dtmin = get_parameter("dt_min_ms").as_double() * 1e-3; - const double dtmax = get_parameter("dt_max_ms").as_double() * 1e-3; - dt = std::clamp(dt, dtmin, dtmax); - - // Read parameters every cycle (allows live tuning via ros2 param set) - const double M_v = get_parameter("M_v").as_double(); - if (M_v <= 0.0) { - RCLCPP_ERROR_THROTTLE(get_logger(), *get_clock(), 1000, - "Invalid M_v=%.4f (must be > 0), skipping cycle", M_v); - return; - } - const double D_v = get_parameter("D_v").as_double(); - const double K_v = get_parameter("K_v").as_double(); - const double sensitivity_inv = get_parameter("sensitivity_inv").as_double(); - const double lever_arm = get_parameter("lever_arm").as_double(); - const double f_max = get_parameter("f_max").as_double(); - const double vel_max = get_parameter("vel_max").as_double(); - const double dvel_max = get_parameter("dvel_max").as_double(); - const double enc_per_rad = get_parameter("enc_per_rad").as_double(); - const auto pos_min = static_cast(get_parameter("pos_min").as_int()); - const auto pos_max = static_cast(get_parameter("pos_max").as_int()); - - const size_t n = pos_current_.size(); - auto msg = rise_motion_messages::msg::MotorVelocity(); - msg.header.stamp = get_clock()->now(); - msg.velocities.resize(n); - - constexpr double rad_to_deg = 180.0 / M_PI; - - auto dbg = rise_motion_messages::msg::AdmittanceDebug(); - dbg.header.stamp = get_clock()->now(); - dbg.adc_voltage.resize(n); - dbg.force.resize(n); - dbg.force_clipped.resize(n); - dbg.torque_raw.resize(n); - dbg.vdot.resize(n); - dbg.velocity_raw.resize(n); - dbg.velocity_output.resize(n); - dbg.displacement.resize(n); - dbg.vdot_deg.resize(n); - dbg.velocity_raw_deg.resize(n); - dbg.velocity_output_deg.resize(n); - dbg.displacement_deg.resize(n); - - for (size_t i = 0; i < n; ++i) { - // ADC → Voltage → Force → Torque - // ADC: 0..65535 → 0V..5V, midpoint 2.5V = 0N (bipolar sensor) - // Subtract 2.5V so that V=0 means no force. Without this, the controller - // would see F=250N at rest and drive the motor even with no interaction. - const uint16_t adc = get_parameter("use_force_override").as_bool() - ? analog_override_value_.load() - : analog_input1_[i]; - double V = (static_cast(adc) / 65535.0 * 5.0) - 2.5; - double F_raw = V * sensitivity_inv; - double F = std::clamp(F_raw, -f_max, f_max); // sensor range limit [N] - double tau = F * lever_arm; // [Nm] - - // Admittance Euler integration (all in rad/s, rad) - double vdot = (tau - D_v * velocity_[i] - K_v * displacement_[i]) / M_v; - double v_prev = velocity_[i]; - velocity_[i] += vdot * dt; - displacement_[i] += velocity_[i] * dt; - - // Output safety: jerk limit then velocity clamp - velocity_[i] = std::clamp(velocity_[i], - v_prev - dvel_max * dt, - v_prev + dvel_max * dt); - velocity_[i] = std::clamp(velocity_[i], -vel_max, vel_max); - - // Position limits (enc-inc): stop motion toward a breached limit - if ((pos_current_[i] <= pos_min && velocity_[i] < 0.0) || - (pos_current_[i] >= pos_max && velocity_[i] > 0.0)) { - velocity_[i] = 0.0; - } - - // Convert rad/s → enc-inc/s for motor command - msg.velocities[i] = static_cast(velocity_[i] * enc_per_rad); - - dbg.adc_voltage[i] = V; - dbg.force[i] = F_raw; - dbg.force_clipped[i] = F; - dbg.torque_raw[i] = tau; - dbg.vdot[i] = vdot; - dbg.velocity_raw[i] = v_prev + vdot * dt; - dbg.velocity_output[i] = velocity_[i]; - dbg.displacement[i] = displacement_[i]; - dbg.vdot_deg[i] = vdot * rad_to_deg; - dbg.velocity_raw_deg[i] = (v_prev + vdot * dt) * rad_to_deg; - dbg.velocity_output_deg[i] = velocity_[i] * rad_to_deg; - dbg.displacement_deg[i] = displacement_[i] * rad_to_deg; - } - - cmd_pub_->publish(msg); - debug_pub_->publish(dbg); - } -}; - -int main(int argc, char **argv) { - rclcpp::init(argc, argv); - auto node = std::make_shared(); - while (rclcpp::ok() && !node->request_enable_ethercat()) { - } - if (!rclcpp::ok()) { - rclcpp::shutdown(); - return 1; - } - if (!node->request_set_velocity_mode()) { - rclcpp::shutdown(); - return 1; - } - rclcpp::spin(node); - rclcpp::shutdown(); - return 0; -} From a43234138ad3b58db5978d85cea2916f32e77b60 Mon Sep 17 00:00:00 2001 From: Florian Date: Thu, 9 Apr 2026 19:24:45 +0200 Subject: [PATCH 70/83] remove sinewave in mock manager --- .../src/rise_motion/src/ec_manager_mock.cpp | 10 +--------- 1 file changed, 1 insertion(+), 9 deletions(-) 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 index f580fc4..8e03a18 100644 --- 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 @@ -1,4 +1,3 @@ -#include #include #include #include @@ -40,16 +39,9 @@ void MockECManager::cyclic_loop() { // Publish feedback to ROS thread (wait-free) feedback_apsa_.perf_write(mock_positions_); - // Simulate full PDO feedback: analog_input1 = 1Hz sinus (full ADC range) - // ADC range: 0=0V, 65535=5V, midpoint=32767.5=2.5V=0N (bipolar sensor) - double t = tick_count_ * period_.count() * 1e-3; - uint16_t analog_sim = static_cast( - 32767.5 + 32767.5 * std::sin(2.0 * M_PI * 1.0 * t)); - std::vector full_feedback(num_motors_); for (int i = 0; i < num_motors_; ++i) { - full_feedback[i].position = mock_positions_[i]; - full_feedback[i].analog_input1 = analog_sim; + full_feedback[i].position = mock_positions_[i]; } full_feedback_apsa_.perf_write(full_feedback); From 31e075bdffb62dfdacbd7c0cad388bd038fd003e Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 15 Apr 2026 15:09:44 +0200 Subject: [PATCH 71/83] remove docs from tracking --- docs/development.md | 471 -------------------------------------------- 1 file changed, 471 deletions(-) delete mode 100644 docs/development.md diff --git a/docs/development.md b/docs/development.md deleted file mode 100644 index 9b78640..0000000 --- a/docs/development.md +++ /dev/null @@ -1,471 +0,0 @@ -# RISE Motion – Entwicklungsdokumentation - -## Inhaltsverzeichnis -1. [Build & Ausführen](#1-build--ausführen) -2. [Projektstruktur](#2-projektstruktur) -3. [Architekturübersicht](#3-architekturübersicht) -4. [Komponenten](#4-komponenten) -5. [ROS2 Interface](#5-ros2-interface) -6. [Datenaufzeichnung](#6-datenaufzeichnung) -7. [Admittanzregelung](#7-admittanzregelung) -8. [Entwicklungsplan](#8-entwicklungsplan) - ---- - -## 1. Build & Ausführen - -### Voraussetzungen -- ROS2 Jazzy -- SOEM Submodul initialisiert: - ```bash - git submodule init - git submodule update - ``` - -### Bauen -Das Skript `rise_motion_dev_ws/build.sh` führt alle drei Schritte aus. - -```bash -cd ~/rise-motion/rise_motion_dev_ws -./build.sh -``` -Intern macht das: -```bash -cd rise_motion_dev_ws -colcon build --symlink-install --cmake-args -DCMAKE_EXPORT_COMPILE_COMMANDS=ON -source install/setup.bash -``` - -### Netzwerkrechte setzen (einmalig nach Build) -EtherCAT braucht Raw-Socket-Zugriff: -```bash -sudo setcap cap_net_admin,cap_net_raw+eip build/rise_motion/rise_motion_main -``` - -### Ausführen (mit Hardware) -Setzt eine EtherCAT-Verbindung auf Interface `enp1s0` voraus (Linux-PC, **nicht WSL2**): -```bash -# Terminal 1 – EtherCAT Node starten (benötigt Raw-Socket-Rechte) -ros2 run rise_motion rise_motion_main - -# Terminal 2 – Test-Node starten -# Ruft /enable_ethercat automatisch auf, bevor Commands gesendet werden. -# Optionales Argument: Inkrement pro Zyklus (default: 10, besser: 1) -ros2 run rise_motion testing_node 1 -``` - -Der `testing_node` ruft `/enable_ethercat` selbst auf – kein manueller Service-Call nötig. Der Node wartet blockierend bis der Service verfügbar ist (`waiting for service to appear...`). - -Manueller Service-Call (z.B. zur Diagnose ohne testing_node): -```bash -ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" -``` - -Der `/enable_ethercat`-Call löst intern automatisch aus: -1. EtherCAT-Zustände `INIT → PRE_OP → SAFE_OP → OPERATIONAL` -2. Mode of Operation → `CyclicSyncPositionMode` -3. CiA402-State-Machine → `OPERATION_ENABLED` - -SDO-Calls (`/sdo_read`, `/sdo_write`) sind optional und dienen nur zur Diagnose einzelner Drive-Register. - -Manuelle Commands an einen Motor schicken (ohne `testing_node`): -```bash -# Terminal 1 -ros2 run rise_motion rise_motion_main - -# Terminal 2 – EtherCAT manuell enablen -ros2 service call /enable_ethercat rise_motion_messages/srv/EnableEthercatSrv "{enable: true}" - -# Terminal 3 – Commands schicken (kontinuierlich, 10 Hz) -ros2 topic pub -r 10 /motor_commands rise_motion_messages/msg/MotorPositions \ - "{positions: [1000, 0, 0, 0, 0, 0]}" -``` - -### Ausführen (ohne Hardware, WSL2) -Zuerst muss gebaut werden: -```bash -cd ~/rise-motion/rise_motion_dev_ws -./build.sh -``` -Dann: -```bash -# In jedem Terminal -cd ~/rise-motion/rise_motion_dev_ws -source install/setup.bash - -# Terminal 1 -ros2 run rise_motion rise_motion_mock - -# Terminal 2 – testing_node ruft /enable_ethercat automatisch auf -ros2 run rise_motion testing_node 1 - -# Terminal 3 – Motor positions validieren -ros2 topic echo /motor_feedback - -# Terminal 4 – Alle PDO-Felder inkl. Kraftsensor (analog_input1 = 1Hz-Sinus im Mock) -ros2 topic echo /motor_feedback_full -``` - -Der Mock simuliert einen Motor im RAM: Kommandos werden sofort als Feedback zurückgegeben. Kein EtherCAT, kein SOEM, keine Netzwerkrechte erforderlich. - ---- - -## 2. State Machines - -Beim Start müssen zwei voneinander unabhängige State Machines hochgefahren werden. - -### EtherCAT Bus States (SOEM) - -Zustand des **Kommunikationsbusses** – unabhängig von den Motoren. - -``` -INIT → PRE_OP → SAFE_OP → OPERATIONAL -``` - -| State | Was passiert | -|---|---| -| `INIT` | Bus initialisiert, keine Kommunikation | -| `PRE_OP` | SDO-Kommunikation möglich (Konfiguration lesen/schreiben) | -| `SAFE_OP` | PDOs werden empfangen, aber noch nicht gesendet – Motoren kriegen noch keine Commands | -| `OPERATIONAL` | PDOs bidirektional aktiv – Motoren können gesteuert werden | - -`init_ec()` bringt den Bus bis `SAFE_OP`. `cyclic_loop()` bringt ihn auf `OPERATIONAL`. - -### CiA402 Drive States (pro Motor) - -Zustand **jedes einzelnen Servo-Drives** (Synapticon Circulo 9). Wird über das `Controlword`-PDO gesteuert, aktueller State steht im `Statusword`. - -``` -NOT_READY_TO_SWITCH_ON - ↓ -SWITCH_ON_DISABLED - ↓ -READY_TO_SWITCH_ON - ↓ -SWITCHED_ON - ↓ -OPERATION_ENABLED ← hier laufen die Motoren -``` - -Fehler-States: -- `QUICK_STOP_ACTIVE` – Notbremse aktiv -- `FAULT_REACTION_ACTIVE` → `FAULT` – Drive hat einen Fehler, muss manuell zurückgesetzt werden - -### Startreihenfolge - -``` -enable_ethercat Service → - init_ec(): - Bus: INIT → PRE_OP → SAFE_OP - - cyclic_loop(): - Bus: SAFE_OP → OPERATIONAL - Motoren: Mode = CyclicSyncPositionMode - Motoren: → OPERATION_ENABLED - → 1kHz Loop startet -``` - -EtherCAT muss `OPERATIONAL` sein, bevor die CiA402-State-Machine auf `OPERATION_ENABLED` geht. - -**Fehlerverhalten:** -- Motor im `FAULT`-State → `transition_motors_to()` bricht ab, Node fährt herunter (`Motor X in fault` im Log) -- Im cyclic loop wird **jeden Zyklus** geprüft ob alle Motoren `OPERATION_ENABLED` sind – wenn nicht, sofortiger Shutdown - -**FAULT zurücksetzen:** - -Tritt `Motor X in fault` auf, muss der FAULT-State zurückgesetzt werden bevor der Drive wieder in `OPERATION_ENABLED` gebracht werden kann. - -Einfachster Weg: **Power-Cycle des Synapticon Circulo** (Strom aus/ein). Faults werden bei Stromverlust zurückgesetzt. - -Alternativ per Controlword (Bit 7 = Fault Reset, CiA402 Index `0x6040`). Funktioniert nur wenn der Bus bereits `OPERATIONAL` ist: -```bash -ros2 service call /sdo_write rise_motion_messages/srv/SDOWriteSrv \ - "{device_id: 1, index: 0x6040, subindex: 0x00, value: [0x80]}" -``` - -Mögliche Ursachen für FAULT: unsauberer Shutdown beim letzten Lauf, Überstrom/Überspannung, oder Drive war beim Start nicht in Ruheposition. - -**Für die Admittanzregelung:** Wechsel von `CyclicSyncPositionMode` auf `CyclicSyncVelocityMode` (Mode 9) – an der Stelle `m.set_mode_of_operation(...)` in `cyclic_loop()`. - ---- - -## 3. Architekturübersicht - -``` -┌─────────────────────────────────────────────────────────────┐ -│ ROS2 Thread │ -│ │ -│ [AdmittanzNode] ──cmd──► [EthercatNode] │ -│ ▲ │ │ -│ │ feedback │ cmd │ -│ └──────────────────────────┘ │ -└──────────────────────────┬──────────────────────────────────┘ - │ APSA (lock-free) -┌──────────────────────────▼──────────────────────────────────┐ -│ EtherCAT Thread (1 kHz) │ -│ │ -│ [ECManager::cyclic_loop()] │ -│ │ │ -│ ┌─────────────▼─────────────┐ │ -│ │ SOEM / IgH │ │ -│ └─────────────┬─────────────┘ │ -└──────────────────────┼──────────────────────────────────────┘ - │ EtherCAT Bus - ┌────────────┼────────────┐ - [Motor 1] [Motor 2] ... [Motor 6] - Synapticon Circulo 9 (CiA402) -``` - -**Threads:** -- **ROS2-Thread:** `EthercatNode` empfängt Kommandos, veröffentlicht Feedback -- **EtherCAT-Thread:** `ECManager::cyclic_loop()` läuft mit 1 kHz, kommuniziert direkt mit den Drives - -**Datenaustausch:** APSA (Atomic Pointer Swap Algorithm) – Triple-Buffer, komplett lock-free, kein Mutex zwischen den Threads. - ---- - -## 4. Komponenten - -### ECManager -Verwaltet den EtherCAT-Bus und den Cyclic Loop. - -| Methode | Beschreibung | -|---|---| -| `init_ec()` | Bus initialisieren, Drives in OP-State bringen | -| `cyclic_loop()` | 1-kHz-Loop: PDO senden/empfangen, State-Machine | -| `is_running()` | Gibt zurück ob der Loop aktiv ist | -| `stop()` | Loop sauber beenden | -| `get_motor_values_apsa()` | Feedback aus EtherCAT-Thread lesen (ROS-Seite) | -| `set_motor_values_apsa()` | Kommando in EtherCAT-Thread schreiben (ROS-Seite) | -| `sdo_read/write()` | SDO-Zugriff für Drive-Konfiguration | - -### EthercatNode -ROS2-Node als Brücke zwischen ROS-Welt und ECManager. - -| Interface | Typ | Beschreibung | -|---|---|---| -| Sub: `motor_commands` | `MotorPositions` | Positionsbefehle empfangen | -| Pub: `motor_feedback` | `MotorPositions` | Motorpositionen publizieren | -| Srv: `enable_ethercat` | `EnableEthercatSrv` | EtherCAT starten/stoppen | -| Srv: `sdo_read` | `SDOReadSrv` | SDO-Register lesen | -| Srv: `sdo_write` | `SDOWriteSrv` | SDO-Register schreiben | - -### MockECManager -Implementiert `IECManager` ohne SOEM-Abhängigkeit. Läuft in WSL2 ohne Hardware. - -| Parameter | Default | Beschreibung | -|---|---|---| -| `cycle_period_ms` | 1 | Zykluszeit in ms | -| `num_motors` | 1 | Anzahl simulierter Motoren | -| `alpha` | 1.0 | Tracking-Faktor (1.0 = sofortiges Echo, <1.0 = Lag) | - -`init_ec()` gibt sofort `EXIT_SUCCESS` zurück. `cyclic_loop()` liest Kommandos aus `cmd_apsa`, berechnet `pos += alpha * (cmd - pos)` und schreibt das Ergebnis in `feedback_apsa`. - -### CiA402Motor -Implementiert das CiA402-Antriebsprofil (IEC 61800-7-201) für Synapticon Circulo 9. - -**State Machine:** `NOT_READY_TO_SWITCH_ON` → `SWITCH_ON_DISABLED` → `READY_TO_SWITCH_ON` → `SWITCHED_ON` → `OPERATION_ENABLED` - -**Betriebsmodi (ModeOfOperation):** -- `CyclicSyncPositionMode` (8) – aktuell verwendet -- `CyclicSyncVelocityMode` (9) – für Admittanzregelung geplant -- `CyclicSyncTorqueMode` (10) -- `ImpedanceMode` (-6), `JointTorqueMode` (-5) – Synapticon-spezifisch - -**PDO-Daten (Inputs von Drive):** Statusword, Position, Velocity, Torque, AnalogInput1–4, Timestamp -**PDO-Daten (Outputs an Drive):** Controlword, OpMode, TargetPosition, TargetVelocity, TargetTorque - -### APSA (Lock-free Buffer) -Triple-Buffer für sicheren, blockierungsfreien Datenaustausch zwischen ROS- und EtherCAT-Thread. - -| Methode | Seite | Beschreibung | -|---|---|---| -| `comm_write()` | ROS-Thread | Kommando schreiben | -| `comm_read()` | ROS-Thread | Feedback lesen | -| `perf_write()` | EtherCAT-Thread | Feedback schreiben | -| `perf_read()` | EtherCAT-Thread | Kommando lesen | - -**Verwendung im Projekt:** -- `cmd_apsa`: ROS → EtherCAT (Motorkommandos) -- `feedback_apsa`: EtherCAT → ROS (Motorpositionen) - ---- - -## 5. ROS2 Interface - -### Messages - -**`MotorPositions.msg`** -``` -int32[] positions # Motorpositionen (encoder-Inkremente), 6 Achsen -int8 target -``` - -**Achsen-Mapping** (Index 0–5): -| Index | Achse | -|---|---| -| 0 | Hüfte Abduktion/Adduktion Links | -| 1 | Hüfte Abduktion/Adduktion Rechts | -| 2 | Hüfte Flexion/Extension Links | -| 3 | Hüfte Flexion/Extension Rechts | -| 4 | Knie Flexion/Extension Links | -| 5 | Knie Flexion/Extension Rechts | - -### Services - -**`EnableEthercatSrv.srv`** -``` -bool enable ---- -int8 status_enable -``` - -**`SDOReadSrv.srv`** -``` -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 -``` - ---- - -## 6. Datenaufzeichnung - -### ROS2 Bag - -Alle Topics können als `.mcap`-Datei aufgezeichnet und später offline ausgewertet werden – ohne laufende Hardware. - -**Aufzeichnen:** -```bash -cd ~/rise-motion/rise_motion_dev_ws -source install/setup.bash -ros2 bag record /motor_feedback_full /motor_feedback -# Stoppen mit Ctrl+C -# Erstellt automatisch einen Ordner mit Zeitstempel, z.B. rosbag2_2026_03_27_12_00_00/ -``` - -**Abspielen** (simuliert die Topics als wären sie live): -```bash -ros2 bag play rosbag2_2026_03_27_12_00_00/ -# In anderem Terminal: ros2 topic echo /motor_feedback_full -``` - -**Metadaten anzeigen:** -```bash -ros2 bag info rosbag2_2026_03_27_12_00_00/ -# Zeigt: Dauer, Anzahl Messages, Topics, Frequenzen -``` - -### Nützliche Diagnose-Tools - -**Publishrate messen:** -```bash -ros2 topic hz /motor_feedback_full # sollte ~100 Hz zeigen (10ms Timer) -``` - -**Node-Graph anzeigen** (welche Nodes welche Topics nutzen): -```bash -ros2 run rqt_graph rqt_graph -``` - -**PlotJuggler** – GUI zum Plotten von Bag-Dateien und Live-Topics (empfohlen für Admittanz-Tuning): - -Installation (einmalig): -```bash -sudo apt install ros-jazzy-plotjuggler-ros -``` - -Starten (im selben Terminal wie `source install/setup.bash`, sonst werden Message-Typen nicht erkannt): -```bash -ros2 run plotjuggler plotjuggler -``` - -Live-Topics plotten: -1. **Streaming** → `ROS2 Topic Subscriber` → **Start** -2. Links in der Liste erscheinen alle aktiven Topics – z.B. `/motor_feedback_full` aufklappen -3. Felder per Drag&Drop ins Plot-Fenster ziehen (z.B. `analog_input1[0]`) - -Bag-Datei auswerten: -1. **File** → `Load Data` → Bag-Ordner auswählen -2. Felder per Drag&Drop plotten - ---- - -## 7. Admittanzregelung - -### Regelungsgesetz -``` -M_v * v̇ + D_v * v + K_v * q = τ_int - -Euler-Integration (dt = 1 ms): - v̇ = (τ_int - D_v * v - K_v * q) / M_v - v += v̇ * dt - q += v * dt - -pos_target = pos_current + q -``` - -**Parameter:** -| Symbol | Bedeutung | Transparenter Modus | -|---|---|---| -| M_v | Virtuelle Masse | > 0 | -| D_v | Virtuelle Dämpfung | > 0 | -| K_v | Virtuelle Steifigkeit | **0** (transparent) | - -### Kraftsensor -- Wägezelle zwischen Manschette und Exoskelett -- Moment: `τ = F * l` (konstanter Hebelarm `l`) -- ADC-Wert in `AnalogInput1` (und ggf. `AnalogInput2`) der CiA402-PDOs -- Kalibrierung: ADC-Ticks → Newton → Nm (Woche 3) - -### Geplante ROS2-Nodes - -| Node | Datei | Status | -|---|---|---| -| `EthercatNode` | `ethercat_node.cpp` | vorhanden | -| `AdmittanzNode` | *geplant* | Woche 1 | -| `TestNode` | `testing_node.cpp` | vorhanden (Positions-Inkrement) | - -### Offene Punkte -- Velocity-Commands an Servo: restlicher RISE-OS Stack arbeitet mit Positionscommands → Integration noch offen -- Kalibrierung Kraftsensor: ADC-Ticks → Newton → Nm (Woche 3) - -### Safety (minimal) -- `v_max`: maximale Gelenkgeschwindigkeit -- `dv_max`: maximale Beschleunigung -- Watchdog: bei ausbleibendem Kraftsensor-Signal stoppen - ---- - -## 8. Entwicklungsplan - -### Woche 1 – Mock & Grundstruktur -- [x] Mock `ECManager` (lokale Tests ohne Hardware) -- [ ] `AdmittanzNode` mit Fake-Kraft-Input (`F = 10 * sin(2π * 0.5 * t)`) -- [ ] Mathematik validieren (Plots) - -### Woche 2 – Kraftsensor-Integration (Software) -- [x] `MotorFeedbackData` Struct (alle CiA402_Inputs Felder) -- [x] Zweites APSA `full_feedback_apsa` in ECManager und MockECManager -- [x] Neue Message `MotorFeedbackFull.msg` (alle PDO-Felder als parallele Arrays) -- [x] Neues Topic `/motor_feedback_full` in EthercatNode -- [x] Mock simuliert `analog_input1` als 1Hz-Sinus (ADC-Ticks, ±2.5V) - -### Woche 3 – Hardware-Tests -- [ ] Echten Kraftsensor testen (Linux-PC, Interface `enp1s0`) -- [ ] Kalibrierung (ADC-Ticks → Newton → Nm, Offset) -- [ ] Erste Admittanz-Tests am Exoskelett - -### Woche 4+ – Integration & Tuning -- [ ] `AdmittanzNode` implementieren (subscribed auf `/motor_feedback_full`) -- [ ] Parameter M, D, K über ROS2 Parameter Server konfigurierbar -- [ ] Integration in RISE-OS Stack (`motor_command_safe`) -- [ ] Safety-Logik: v_max, pos_limits, Watchdog, Notaus From c53b5d236322fa62a0e7ac5dfdb2a6ca749eb326 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 29 Apr 2026 13:55:15 +0200 Subject: [PATCH 72/83] Feat: increase to six motors * do velocity integration for rviz visualisation (joint moves for mock manager) --- .../include/rise_motion/ec_manager_mock.hpp | 2 ++ .../src/rise_motion/src/ec_manager_mock.cpp | 13 ++++++++----- .../src/rise_motion/src/main_mock.cpp | 2 +- .../rise_motion_messages/msg/AdmittanceDebug.msg | 3 +++ 4 files changed, 14 insertions(+), 6 deletions(-) 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 index 01a1ed0..af4307a 100644 --- 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 @@ -37,12 +37,14 @@ class MockECManager : public IECManager { std::vector mock_positions_; 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/src/ec_manager_mock.cpp b/rise_motion_dev_ws/src/rise_motion/src/ec_manager_mock.cpp index 8e03a18..3ff2fc5 100644 --- 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 @@ -10,6 +10,7 @@ MockECManager::MockECManager(int cycle_period_ms, int num_motors, float alpha) logger_(rclcpp::get_logger("MockECManager")), mock_positions_(num_motors, 0), motor_commands_(num_motors, 0), + velocity_commands_(num_motors, 0), period_(cycle_period_ms) {} int MockECManager::init_ec() { @@ -27,13 +28,15 @@ void MockECManager::cyclic_loop() { while (running_) { next_ += period_; - // Pull latest motor commands from ROS thread (wait-free) + // Pull latest commands from ROS thread (wait-free) cmd_apsa_.perf_read(motor_commands_); + vel_cmd_apsa_.perf_read(velocity_commands_); - // Simulate motor: track commanded position with configurable lag + // Integrate velocity commands into position (dt = period in seconds) + const double dt = period_.count() * 1e-3; for (int i = 0; i < num_motors_; ++i) { mock_positions_[i] += static_cast( - alpha_ * static_cast(motor_commands_[i] - mock_positions_[i])); + static_cast(velocity_commands_[i]) * dt); } // Publish feedback to ROS thread (wait-free) @@ -70,8 +73,8 @@ bool MockECManager::get_full_feedback_apsa(std::vector& feedb return full_feedback_apsa_.comm_read(feedback); } -bool MockECManager::set_motor_velocity_apsa(const std::vector& /*velocities*/) { - return true; +bool MockECManager::set_motor_velocity_apsa(const std::vector& velocities) { + return vel_cmd_apsa_.comm_write(velocities); } void MockECManager::set_operation_mode(int8_t mode) { 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 index 15c1a8b..3807dc2 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - MockECManager ec_manager(/*cycle_period_ms=*/1, /*num_motors=*/1); + MockECManager ec_manager(/*cycle_period_ms=*/1, /*num_motors=*/6); auto node = std::make_shared(ec_manager); rclcpp::spin(node); 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 index a9306d1..27e76cf 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -20,3 +20,6 @@ float64[] vdot_deg # [deg/s^2] float64[] velocity_raw_deg # [deg/s] float64[] velocity_output_deg # [deg/s] float64[] displacement_deg # [deg] + +# Gravity compensation +float64[] tau_gravity # [Nm] gravity torque added to tau (0 if gravity_enabled=false) From 02e347168ffb86a187a75a96b90b10958c6e36b8 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 13 May 2026 23:57:59 +0200 Subject: [PATCH 73/83] mock: add num_motors CLI argument - main_mock.cpp: parse num_motors from CLI argument (default 6) - AdmittanceDebug.msg: remove unused tau_gravity field --- .../include/rise_motion/ec_manager_mock.hpp | 1 + .../src/rise_motion/src/ec_manager_mock.cpp | 8 +++++--- rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp | 12 +++++++++++- .../src/rise_motion_messages/msg/AdmittanceDebug.msg | 3 --- 4 files changed, 17 insertions(+), 7 deletions(-) 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 index af4307a..0e8160d 100644 --- 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 @@ -36,6 +36,7 @@ class MockECManager : public IECManager { 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}; 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 index 3ff2fc5..22ca64e 100644 --- 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 @@ -1,3 +1,4 @@ +#include #include #include #include @@ -9,6 +10,7 @@ MockECManager::MockECManager(int cycle_period_ms, int num_motors, float alpha) 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) {} @@ -32,11 +34,11 @@ void MockECManager::cyclic_loop() { cmd_apsa_.perf_read(motor_commands_); vel_cmd_apsa_.perf_read(velocity_commands_); - // Integrate velocity commands into position (dt = period in seconds) + // 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_[i] += static_cast( - static_cast(velocity_commands_[i]) * dt); + 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) 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 index 3807dc2..3bfefb7 100644 --- a/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/main_mock.cpp @@ -4,9 +4,19 @@ #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=*/6); + MockECManager ec_manager(/*cycle_period_ms=*/1, num_motors); auto node = std::make_shared(ec_manager); rclcpp::spin(node); 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 index 27e76cf..a9306d1 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -20,6 +20,3 @@ float64[] vdot_deg # [deg/s^2] float64[] velocity_raw_deg # [deg/s] float64[] velocity_output_deg # [deg/s] float64[] displacement_deg # [deg] - -# Gravity compensation -float64[] tau_gravity # [Nm] gravity torque added to tau (0 if gravity_enabled=false) From 817ded9a57a35fea54c65e49d45659d635573344 Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 13 May 2026 17:58:45 +0200 Subject: [PATCH 74/83] add plotjuggler to launch file --- .gitignore | 1 + 1 file changed, 1 insertion(+) diff --git a/.gitignore b/.gitignore index 29b3824..f2972a6 100644 --- a/.gitignore +++ b/.gitignore @@ -8,3 +8,4 @@ rosbag2_*/ CLAUDE.md plans/ SOMANET Circulo Safe Motion.pdf +docs/ \ No newline at end of file From a4abf7f7e639dfe30a5e2230de92ab392aa0162b Mon Sep 17 00:00:00 2001 From: Florian Date: Wed, 20 May 2026 18:00:47 +0200 Subject: [PATCH 75/83] Add adc_voltage_raw --- .../src/rise_motion_messages/msg/AdmittanceDebug.msg | 1 + 1 file changed, 1 insertion(+) 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 index a9306d1..19e6aa3 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -4,6 +4,7 @@ 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 From 19b51a9ca79b0ee6148d875f45cb521857bab2e6 Mon Sep 17 00:00:00 2001 From: Florian Date: Thu, 21 May 2026 12:53:24 +0200 Subject: [PATCH 76/83] Feat: Add torque offset dataflow topic -> ethercat node -> apsa -> ethercat thread -> PDO --- .../rise_motion/include/rise_motion/ec_manager.hpp | 4 ++++ .../include/rise_motion/ec_manager_mock.hpp | 1 + .../include/rise_motion/ethercat_node.hpp | 3 +++ .../rise_motion/include/rise_motion/iec_manager.hpp | 1 + .../src/rise_motion/src/ec_manager.cpp | 7 +++++++ .../src/rise_motion/src/ec_manager_mock.cpp | 4 ++++ .../src/rise_motion/src/ethercat_node.cpp | 12 ++++++++++++ .../src/rise_motion_messages/CMakeLists.txt | 1 + .../rise_motion_messages/msg/MotorTorqueOffset.msg | 2 ++ 9 files changed, 35 insertions(+) create mode 100644 rise_motion_dev_ws/src/rise_motion_messages/msg/MotorTorqueOffset.msg 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 index 9042692..4fa1ba7 100755 --- 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 @@ -27,6 +27,7 @@ class ECManager : public IECManager { 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; void set_operation_mode(int8_t mode) override; // Wrappers for SOEM ecx_SDOwrite, ecx_SDOread @@ -58,6 +59,9 @@ class ECManager : public IECManager { // 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; + // feedback_apsa: EtherCAT → ROS (motor positions only, für /motor_feedback) APSA> feedback_apsa; 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 index 0e8160d..8737b06 100644 --- 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 @@ -22,6 +22,7 @@ class MockECManager : public IECManager { 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; void set_operation_mode(int8_t mode) override; bool sdo_read(uint16_t device_id, uint16_t index, 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 index 6f7c059..cf5199f 100755 --- 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 @@ -5,6 +5,7 @@ #include #include #include +#include #include #include #include @@ -23,6 +24,7 @@ class EthercatNode : public rclcpp::Node { rclcpp::Subscription::SharedPtr cmd_sub_; rclcpp::Subscription::SharedPtr cmd_vel_sub_; + rclcpp::Subscription::SharedPtr torque_offset_sub_; rclcpp::Publisher::SharedPtr feedback_pub_; rclcpp::TimerBase::SharedPtr feedback_timer_; rclcpp::Publisher::SharedPtr full_feedback_pub_; @@ -35,6 +37,7 @@ class EthercatNode : public rclcpp::Node { 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 publishFeedback(); void publishFullFeedback(); void enableServiceCallback( 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 index cfe7300..80f1663 100644 --- 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 @@ -18,6 +18,7 @@ class IECManager { 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 void set_operation_mode(int8_t mode) = 0; virtual bool sdo_read(uint16_t device_id, uint16_t index, 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 index 2c5962c..af6fdff 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -104,6 +104,7 @@ void ECManager::cyclic_loop() { std::vector motor_commands(ctx.slavecount, 0); std::vector motor_velocities(ctx.slavecount, 0); + std::vector torque_offsets(ctx.slavecount, 0); std::vector motor_feedback(ctx.slavecount, 0); std::vector full_feedback(ctx.slavecount); @@ -175,6 +176,7 @@ void ECManager::cyclic_loop() { // 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); const int8_t target = target_mode_.load(std::memory_order_relaxed); m.outputs->OpMode = target; const bool mode_confirmed = (m.inputs->OpModeDisplay == target); @@ -186,6 +188,7 @@ void ECManager::cyclic_loop() { } 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; @@ -302,6 +305,10 @@ 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); +} + 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); 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 index 22ca64e..0b6db55 100644 --- 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 @@ -79,6 +79,10 @@ bool MockECManager::set_motor_velocity_apsa(const std::vector& velociti return vel_cmd_apsa_.comm_write(velocities); } +bool MockECManager::set_torque_offset_apsa(const std::vector& /*offsets*/) { + return true; +} + void MockECManager::set_operation_mode(int8_t mode) { RCLCPP_INFO(logger_, "Mock: operation mode set to %d", mode); } 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 index d92bdb5..6086360 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -18,6 +18,10 @@ EthercatNode::EthercatNode(IECManager &ec_manager) "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)); + feedback_pub_ = create_publisher( "motor_feedback", 10); @@ -157,6 +161,14 @@ void EthercatNode::velocityCommandCallback( } } +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::setOperationModeCallback( std::shared_ptr request, std::shared_ptr response) { diff --git a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt index a6b1dcc..c14fc24 100755 --- a/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt +++ b/rise_motion_dev_ws/src/rise_motion_messages/CMakeLists.txt @@ -17,6 +17,7 @@ rosidl_generate_interfaces(${PROJECT_NAME} "msg/MotorFeedbackFull.msg" "msg/AdmittanceDebug.msg" "msg/MotorVelocity.msg" + "msg/MotorTorqueOffset.msg" "srv/EnableEthercatSrv.srv" "srv/SDOReadSrv.srv" "srv/SDOWriteSrv.srv" 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) From e6ae68c5a261a762d8b859f7ee205338276854bf Mon Sep 17 00:00:00 2001 From: Florian Date: Thu, 21 May 2026 12:58:43 +0200 Subject: [PATCH 77/83] AdmittanceDebug: add pos_rad field (absolute encoder position) --- .../src/rise_motion_messages/msg/AdmittanceDebug.msg | 3 +++ 1 file changed, 3 insertions(+) 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 index 19e6aa3..8c8a4bb 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -10,6 +10,9 @@ float64[] force # [N] voltage -> force (before clipping) float64[] force_clipped # [N] after f_max clipping float64[] torque_raw # [Nm] force_clipped * 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 + # Admittance integration (rad) float64[] vdot # [rad/s^2] angular acceleration float64[] velocity_raw # [rad/s] after Euler integration (before safety) From e086d760b7a9f2353c76123125d3c8a3004a32c7 Mon Sep 17 00:00:00 2001 From: Florian Date: Thu, 4 Jun 2026 08:59:14 +0200 Subject: [PATCH 78/83] Set EtherCAT interface to enp5s0 (new tower PC) --- rise_motion_dev_ws/src/rise_motion/src/main.cpp | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/rise_motion_dev_ws/src/rise_motion/src/main.cpp b/rise_motion_dev_ws/src/rise_motion/src/main.cpp index 1d50077..5687e8e 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/main.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/main.cpp @@ -6,7 +6,7 @@ int main(int argc, char *argv[]) { rclcpp::init(argc, argv); - ECManager ec_manager("enp1s0", 1); // TODO: Connect config from rise-os-core + ECManager ec_manager("enp5s0", 1); // TODO: Connect config from rise-os-core auto node = std::make_shared(ec_manager); rclcpp::spin(node); From 351401300942bc995b48e374996d40d4e5000fa2 Mon Sep 17 00:00:00 2001 From: Florian Pages Date: Thu, 4 Jun 2026 12:31:52 +0200 Subject: [PATCH 79/83] ec_manager: tolerate WKC jitter, recover from QUICK_STOP --- .../src/rise_motion/src/ec_manager.cpp | 19 ++++++++++++++++--- 1 file changed, 16 insertions(+), 3 deletions(-) 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 index af6fdff..8ce19c5 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -107,6 +107,8 @@ void ECManager::cyclic_loop() { std::vector torque_offsets(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 @@ -150,9 +152,16 @@ void ECManager::cyclic_loop() { ecx_mbxhandler(&ctx, 0, 4); if (wkc != expectedWKC) { - RCLCPP_ERROR(logger, "Not all nodes responded"); - shutdown(); - return; + 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 @@ -412,6 +421,10 @@ bool ECManager::transition_motors_to(CiA402Motor::State state) { } 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; From 678a4b80925425b752fb97ff5f56dc7e88f8b2f4 Mon Sep 17 00:00:00 2001 From: Florian Pages Date: Thu, 4 Jun 2026 18:18:59 +0200 Subject: [PATCH 80/83] AdmittanceDebug: add force_filtered field for EMA filter output --- .../src/rise_motion_messages/msg/AdmittanceDebug.msg | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) 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 index 8c8a4bb..98580be 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -8,7 +8,8 @@ float64[] adc_voltage_raw # [V] ADC ticks -> voltage (uncalibrated, 0..5V 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[] torque_raw # [Nm] force_clipped * lever_arm -> input to controller +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 From 0ede03e61b5a66cb6d5af95884e50de689b9c57d Mon Sep 17 00:00:00 2001 From: Florian Date: Fri, 5 Jun 2026 10:36:48 +0200 Subject: [PATCH 81/83] ethercat: add CyclicSyncTorque mode (mode 10) support - Add torque_cmd_apsa buffer and set_motor_torque_apsa() to ECManager - Route TargetTorque PDO in cyclic loop when mode 10 is confirmed - Accept mode 10 in setOperationModeCallback (was 8/9 only) - Add motor_commands_torque subscription in EthercatNode - Implement set_motor_torque_apsa() as no-op in MockECManager --- .../include/rise_motion/ec_manager.hpp | 6 +++++- .../include/rise_motion/ec_manager_mock.hpp | 1 + .../include/rise_motion/ethercat_node.hpp | 2 ++ .../include/rise_motion/iec_manager.hpp | 1 + .../src/rise_motion/src/ec_manager.cpp | 9 +++++++++ .../src/rise_motion/src/ec_manager_mock.cpp | 4 ++++ .../src/rise_motion/src/ethercat_node.cpp | 20 ++++++++++++++++--- 7 files changed, 39 insertions(+), 4 deletions(-) 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 index 4fa1ba7..35e3fc8 100755 --- 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 @@ -28,6 +28,7 @@ class ECManager : public IECManager { 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 @@ -62,13 +63,16 @@ class ECManager : public IECManager { // 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 + // 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 index 8737b06..59c625e 100644 --- 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 @@ -23,6 +23,7 @@ class MockECManager : public IECManager { 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, 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 index cf5199f..03c0771 100755 --- 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 @@ -25,6 +25,7 @@ class EthercatNode : public rclcpp::Node { 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_; @@ -38,6 +39,7 @@ class EthercatNode : public rclcpp::Node { 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( 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 index 80f1663..dd38720 100644 --- 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 @@ -19,6 +19,7 @@ class IECManager { 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, 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 index 8ce19c5..a925204 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ec_manager.cpp @@ -105,6 +105,7 @@ void ECManager::cyclic_loop() { 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; @@ -186,12 +187,16 @@ void ECManager::cyclic_loop() { 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) @@ -318,6 +323,10 @@ 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); 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 index 0b6db55..1546504 100644 --- 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 @@ -83,6 +83,10 @@ 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); } 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 index 6086360..e9833a2 100755 --- a/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp +++ b/rise_motion_dev_ws/src/rise_motion/src/ethercat_node.cpp @@ -22,6 +22,10 @@ EthercatNode::EthercatNode(IECManager &ec_manager) "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); @@ -169,6 +173,14 @@ void EthercatNode::torqueOffsetCallback( } } +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) { @@ -177,15 +189,17 @@ void EthercatNode::setOperationModeCallback( response->message = "EtherCAT not enabled"; return; } - if (request->mode != 8 && request->mode != 9) { + if (request->mode != 8 && request->mode != 9 && request->mode != 10) { response->success = false; - response->message = "Invalid mode " + std::to_string(request->mode) + ". Only mode 8 (CyclicSyncPosition) and 9 (CyclicSyncVelocity) are supported."; + 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; - response->message = request->mode == 9 ? "Velocity mode active (mode 9)" : "Position mode active (mode 8)"; + 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); } From 2063f33db1d13f818b207a8a5755a2525c4880ad Mon Sep 17 00:00:00 2001 From: Florian Date: Sun, 28 Jun 2026 14:12:13 +0200 Subject: [PATCH 82/83] AdmittanceDebug: add accel, velocity_fb_joint, torque_fb, tau_gravity_nm, tau_friction_nm --- .../rise_motion_messages/msg/AdmittanceDebug.msg | 15 +++++++++++++++ .../rise_motion_messages/msg/MotorVelocity.msg | 2 +- 2 files changed, 16 insertions(+), 1 deletion(-) 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 index 98580be..5327079 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -25,3 +25,18 @@ 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/MotorVelocity.msg b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg index 8f873a5..7ab4417 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/MotorVelocity.msg @@ -1,2 +1,2 @@ std_msgs/Header header # ROS timestamp when command was computed -int32[] velocities # velocity commands in enc-inc/s, one per motor +int32[] velocities # velocity commands in RPM (0x60FF target velocity), one per motor From 20de17c44d4e6942d762ba3a388b4572331ac19f Mon Sep 17 00:00:00 2001 From: Florian Pages Date: Wed, 8 Jul 2026 13:46:17 +0200 Subject: [PATCH 83/83] AdmittanceDebug: add pos_rel_rad/pos_rel_deg tare fields Absolute pos_rad has no meaningful zero point (raw multi-turn encoder origin, machine-specific). Add a relative-position pair that a consumer node can zero on demand, for experiments that need a sensible 0-degree reference without knowing the raw calibration. --- .../src/rise_motion_messages/msg/AdmittanceDebug.msg | 7 +++++++ 1 file changed, 7 insertions(+) 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 index 5327079..5b66380 100644 --- a/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg +++ b/rise_motion_dev_ws/src/rise_motion_messages/msg/AdmittanceDebug.msg @@ -14,6 +14,13 @@ float64[] torque_raw # [Nm] force_filtered * lever_arm -> input to co # 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)