Topic #51 cleanup admittance branch for merge

.gitignore

@@ -1,5 +1,11 @@
 **/build/
 **/log/
 **/install/
+rosbag2_*/
 **/*~
-.vscode
+.vscode
+.claude
+CLAUDE.md
+plans/
+SOMANET Circulo Safe Motion.pdf
+docs/

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

rise_motion_dev_ws/src/rise_motion/CMakeLists.txt

@@ -7,10 +7,66 @@ endif()
 
 # find dependencies
 find_package(ament_cmake REQUIRED)
-# uncomment the following section in order to fill in
-# further dependencies manually.
-# find_package(<dependency> REQUIRED)
+find_package(rclcpp REQUIRED)
+find_package(rise_motion_messages REQUIRED)
+find_package(std_msgs REQUIRED)
 
+include_directories(include)
+
+add_subdirectory(SOEM)
+
+add_executable(
+	rise_motion_main
+	src/main.cpp
+	src/ec_manager.cpp
+	src/ethercat_node.cpp
+  src/cia402.cpp
+)
+ament_target_dependencies(
+	rise_motion_main
+	rclcpp
+	rise_motion_messages
+)
+target_link_libraries(
+	rise_motion_main
+	soem
+)
+
+#target_link_options(rise_motion_main PRIVATE -Wl,-rpath,/hello/home/rise/Projects/rise-motion/rise_motion_dev_ws/install/rise_motion_messages/lib)
+install(TARGETS
+  rise_motion_main
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+add_executable(rise_motion_mock
+  src/main_mock.cpp
+  src/ec_manager_mock.cpp
+  src/ethercat_node.cpp
+)
+ament_target_dependencies(
+  rise_motion_mock
+  rclcpp
+  rise_motion_messages
+)
+install(TARGETS
+  rise_motion_mock
+  DESTINATION lib/${PROJECT_NAME}
+)
+
+
+add_executable(testing_node
+  src/testing_node.cpp
+)
+
+target_link_libraries(testing_node PUBLIC
+  ${rise_motion_messages_TARGETS}
+  rclcpp::rclcpp
+)
+
+install(TARGETS
+  testing_node
+  DESTINATION lib/${PROJECT_NAME}
+)
 if(BUILD_TESTING)
   find_package(ament_lint_auto REQUIRED)
   # the following line skips the linter which checks for copyrights

rise_motion_dev_ws/src/rise_motion/include/rise_motion/apsa.hpp

@@ -0,0 +1,123 @@
+#pragma once
+
+#include <atomic>
+#include <cstring>
+#include <memory>
+
+
+template <typename T>
+class APSA {
+public:
+  APSA()
+    : c_mem(std::make_unique<T>()),    // Allocate communication buffer
+      a_mem(std::make_unique<T>()),    // Allocate atomic/shared buffer
+      p_mem(std::make_unique<T>()),    // 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<T> c_mem;  // Communication thread's buffer (ROS side)
+  std::unique_ptr<T> a_mem;  // Atomic/shared buffer (exchange point)
+  std::unique_ptr<T> p_mem;  // Performance thread's buffer (EtherCAT side)
+
+  std::atomic<T*> c_p;  // Communication pointer (ROS side access)
+  std::atomic<T*> a_p;  // Atomic pointer (shared exchange point)
+  std::atomic<T*> p_p;  // Performance pointer (EtherCAT side access)
+};

rise_motion_dev_ws/src/rise_motion/include/rise_motion/cia402.hpp

@@ -0,0 +1,151 @@
+#pragma once
+#include <cstdint>
+#include <optional>
+#include <string>
+
+#include <osal_defs.h>
+
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED {
+  uint16_t Statusword;
+  int8_t OpModeDisplay;
+  int32_t PositionValue;
+  int32_t VelocityValue;
+  int16_t TorqueValue;
+  uint16_t AnalogInput1;
+  uint16_t AnalogInput2;
+  uint16_t AnalogInput3;
+  uint16_t AnalogInput4;
+  uint32_t TuningStatus;
+  uint32_t DigitalInputs;
+  uint32_t UserMISO;
+  uint32_t Timestamp;
+  int32_t PositionDemandInternalValue;
+  int32_t VelocityDemandValue;
+  int16_t TorqueDemand;
+} CiA402_Inputs;
+OSAL_PACKED_END
+
+OSAL_PACKED_BEGIN
+typedef struct OSAL_PACKED {
+  uint16_t Controlword;
+  int8_t OpMode;
+  int16_t TargetTorque;
+  int32_t TargetPosition;
+  int32_t TargetVelocity;
+  int16_t TorqueOffset;
+  uint32_t TuningCommand;
+  uint32_t PhysicalOutputs;
+  uint32_t BitMask;
+  uint32_t UserMOSI;
+  int32_t VelocityOffset;
+} CiA402_Outputs;
+OSAL_PACKED_END
+
+class CiA402Motor {
+public:
+  enum class State {
+    NOT_READY_TO_SWITCH_ON,
+    SWITCH_ON_DISABLED,
+    READY_TO_SWITCH_ON,
+    SWITCHED_ON,
+    OPERATION_ENABLED,
+    QUICK_STOP_ACTIVE,
+    FAULT_REACTION_ACTIVE,
+    FAULT,
+  };
+
+  enum class Operation {
+    SHUTDOWN,
+    SWITCH_ON,
+    DISABLE_VOLTAGE,
+    QUICK_STOP,
+    DISABLE_OPERATION,
+    ENABLE_OPERATION,
+    FAULT_RESET
+  };
+
+  // https://doc.synapticon.com/circulo/sw5.4/objects_html/6xxx/6060.html
+  enum class ModeOfOperation : int8_t {
+    ImpedanceMode = -6,
+    JointTorqueMode = -5,
+    SystemIdentificationMode = -4,
+    OpenLoopFieldMode = -3,
+    DiagnosticsMode = -2,
+    CoggingCompensationRecordingMode = -1,
+    ProfilePositionMode = 1,
+    ProfileVelocityMode = 3,
+    TorqueProfileMode = 4,
+    HomingMode = 6,
+    CyclicSyncPositionMode = 8,
+    CyclicSyncVelocityMode = 9,
+    CyclicSyncTorqueMode = 10
+  };
+
+  CiA402Motor(CiA402_Inputs *inputs, CiA402_Outputs *outputs);
+
+  /**
+   * @brief Gets the motor's current state based on its status word.
+   *
+   * Compares the status word against predefined patterns. Returns the matching
+   * state, or `std::nullopt` if no match is found.
+   *
+   * @return The motor's state if a match is found; otherwise, `std::nullopt`.
+   *
+   * @warning Returning `std::nullopt` indicates an unrecognized status word.
+   *          Callers must handle this case to avoid undefined behavior.
+   *
+   * @see CiA402Motor::State, state_patterns
+   */
+  std::optional<State> get_state() const;
+  std::string state_as_string() const;
+  bool is_state(State s) const;
+  bool is_operation_enabled() const;
+  bool is_fault() const;
+
+  void to_operation_enabled();
+  void to_switch_on_disabled();
+  void transition_to(State s);
+  void reset_fault();
+  void set_control_word(Operation op);
+  void set_mode_of_operation(ModeOfOperation m);
+
+  CiA402_Inputs *inputs;
+  CiA402_Outputs *outputs;
+
+private:
+  struct StatePattern {
+    uint16_t mask;
+    uint16_t value;
+    State state;
+  };
+
+  static constexpr StatePattern state_patterns[] = {
+      // https://doc.synapticon.com/circulo/system_integration/status_and_controlword.html
+      // mask     value      state
+      {0b1001111, 0b0000000, State::NOT_READY_TO_SWITCH_ON},
+      {0b1001111, 0b1000000, State::SWITCH_ON_DISABLED},
+      {0b1101111, 0b0100001, State::READY_TO_SWITCH_ON},
+      {0b1101111, 0b0100011, State::SWITCHED_ON},
+      {0b1101111, 0b0100111, State::OPERATION_ENABLED},
+      {0b1101111, 0b0000111, State::QUICK_STOP_ACTIVE},
+      {0b1001111, 0b0001111, State::FAULT_REACTION_ACTIVE},
+      {0b1001111, 0b0001000, State::FAULT}};
+
+  struct ControlPattern {
+    uint16_t mask;
+    uint16_t value;
+    Operation op;
+  };
+
+  static constexpr ControlPattern control_patterns[] = {
+      // https://doc.synapticon.com/circulo/system_integration/status_and_controlword.html
+      // mask      value       operation
+      {0b10000111, 0b00000110, Operation::SHUTDOWN},
+      {0b10001111, 0b00000111, Operation::SWITCH_ON},
+      {0b10000010, 0b00000000, Operation::DISABLE_VOLTAGE},
+      {0b10000110, 0b00000010, Operation::QUICK_STOP},
+      {0b10001111, 0b00000111, Operation::DISABLE_OPERATION},
+      {0b10001111, 0b00001111, Operation::ENABLE_OPERATION},
+      {0b10000000, 0b10000000, Operation::FAULT_RESET}};
+};