From a13efbd8723e2931373990076348d29820e9ea7a Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Tue, 8 Jul 2025 20:30:15 +0200 Subject: [PATCH 1/7] reconstruct control mode abstract and add idle, zero-torque and pd-test mode --- include/control_mode/ZeroTorqueControlMode.h | 45 ---------------- include/control_mode/abstract_control_mode.h | 32 ++++++++++- include/control_mode/idle_control_mode.hpp | 15 ++++-- include/control_mode/joint_pd_test_mode.hpp | 19 +++++++ include/control_mode/zero_torque_mode.hpp | 24 +++++++++ src/control_mode/idle_control_mode.cpp | 34 +++++------- src/control_mode/joint_pd_test_mode.cpp | 56 ++++++++++++++++++++ src/control_mode/zero_torque_mode.cpp | 55 +++++++++++++++++++ 8 files changed, 206 insertions(+), 74 deletions(-) delete mode 100644 include/control_mode/ZeroTorqueControlMode.h create mode 100644 include/control_mode/joint_pd_test_mode.hpp create mode 100644 include/control_mode/zero_torque_mode.hpp create mode 100644 src/control_mode/joint_pd_test_mode.cpp create mode 100644 src/control_mode/zero_torque_mode.cpp diff --git a/include/control_mode/ZeroTorqueControlMode.h b/include/control_mode/ZeroTorqueControlMode.h deleted file mode 100644 index 6b78728..0000000 --- a/include/control_mode/ZeroTorqueControlMode.h +++ /dev/null @@ -1,45 +0,0 @@ -#include - -#include "AbstractControlMode.h" - -class ZeroTorqueControlMode : public AbstractControlMode { -public: - // Constructor - explicit ZeroTorqueControlMode() { - } - - // Destructor - ~ZeroTorqueControlMode() override = default; - - // Start the control mode - void start() override { - // Set up any necessary parameters for zero torque control - robot_->setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); - is_running_ = true; - std::function - joint_torque_callback = [this](const franka::RobotState &state, franka::Duration) -> franka::Torques - { - // TODO: quit the mode - if (!is_running_) { - return ; - } - franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - this->current_state_->updateState(state); - zero_torques = this->model_->coriolis(state); - return zero_torques; - }; - robot_->control(joint_torque_callback); - } - - // Stop the control mode - void stop() override { - // Cleanup if necessary - } - -protected: - // Setup control mode specifics - -}; diff --git a/include/control_mode/abstract_control_mode.h b/include/control_mode/abstract_control_mode.h index 4192579..98cbb82 100644 --- a/include/control_mode/abstract_control_mode.h +++ b/include/control_mode/abstract_control_mode.h @@ -5,6 +5,9 @@ #include #include #include +#include //for std::shared_ptr +#include +//todo:reform and check the leadter state get and the is_running class AbstractControlMode { public: // Virtual destructor for proper cleanup in derived classes @@ -20,14 +23,39 @@ class AbstractControlMode { void setModel(std::shared_ptr model) { model_ = std::move(model); } - virtual franka::RobotState UpdateState() const { + // Set the current state of the robot into variable current_state_ + void setCurrentState(const franka::RobotState& state) { + std::lock_guard lock(state_mutex_); + current_state_ = state; + } + + // get current state of robot + virtual franka::RobotState getRobotState() const { std::lock_guard lock(state_mutex_); return current_state_; } + + void setLeaderState(const franka::RobotState& leader_state) { + std::lock_guard lock(leader_mutex_); + leader_state_ = std::make_shared(leader_state); + } + + std::shared_ptr getLeaderState() const { + std::lock_guard lock(leader_mutex_); + return leader_state_; + } + //for mode use: + // auto leader_ptr = getLeaderState(); + // + + // const franka::RobotState& leader = *leader_state_ptr; + //franka::Torques tau = computeTorqueFromLeader(leader); + + + protected: // Protected constructor to prevent direct instantiation AbstractControlMode() = default; - // Protected setup function for derived classes std::shared_ptr robot_; std::shared_ptr model_; diff --git a/include/control_mode/idle_control_mode.hpp b/include/control_mode/idle_control_mode.hpp index 229c692..ae64269 100644 --- a/include/control_mode/idle_control_mode.hpp +++ b/include/control_mode/idle_control_mode.hpp @@ -2,9 +2,17 @@ #define IDLE_CONTROL_MODE_H #include "abstract_control_mode.h" -#include +#include +#include +#include +#include +#include //for std::shared_ptr +#include +#include #include + + class IdleControlMode : public AbstractControlMode { public: IdleControlMode() = default; @@ -13,9 +21,6 @@ class IdleControlMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; - -private: - void updateOnce(); // 使用 readOnce 读取当前状态 -}; +} #endif // IDLE_CONTROL_MODE_H \ No newline at end of file diff --git a/include/control_mode/joint_pd_test_mode.hpp b/include/control_mode/joint_pd_test_mode.hpp new file mode 100644 index 0000000..1f84b49 --- /dev/null +++ b/include/control_mode/joint_pd_test_mode.hpp @@ -0,0 +1,19 @@ +#ifndef JOINT_PD_CONTROL_TEST_HPP +#define JOINT_PD_CONTROL_TEST_HPP + +#include "abstract_control_mode.h" +#include +#include +#include +#include + +class JointPDControlMode : public AbstractControlMode { +public: + JointPDControlMode(); + ~JointPDControlMode() override; + + void start() override; + void stop() override; +}; + +#endif // JOINT_PD_CONTROL_TEST_HPP diff --git a/include/control_mode/zero_torque_mode.hpp b/include/control_mode/zero_torque_mode.hpp new file mode 100644 index 0000000..fa70814 --- /dev/null +++ b/include/control_mode/zero_torque_mode.hpp @@ -0,0 +1,24 @@ +#ifndef ZERO_TORQUE_MODE_HPP +#define ZERO_TORQUE_MODE_HPP + +#include "abstract_control_mode.h" +#include +#include +#include +#include +#include //for std::shared_ptr +#include +#include +#include + +class IdleControlMode : public AbstractControlMode { +public: + ZeroTorqueMode() = default; + ~ZeroTorqueMode() override = default; + + //void initialize(const franka::RobotState& initial_state) override; + void start() override; + void stop() override; +} + +#endif // IDLE_CONTROL_MODE_H \ No newline at end of file diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index a769e52..d96baa1 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -7,31 +7,21 @@ // std::cout << "[IdleControlMode] Initialized with initial state." << std::endl; // } void IdleControlMode::start() { - if (!robot_) { - throw std::runtime_error("[IdleControlMode] Robot not set."); - } std::cout << "[IdleControlMode] Entering idle mode. Reading state once..." << std::endl; is_running_ = true; - // while(is_running_) - // { - // updateOnce(); - // } - updateOnce(); - std::cout << "[IdleControlMode] Idle mode active. No control actions taken." << std::endl; + while (is_running_) { + try { + if (robot_) { + franka::RobotState state = robot_->readOnce(); + setCurrentState(state); + } + } catch (const franka::Exception& e) { + std::cerr << "[IdleMode] readOnce() failed: " << e.what() << std::endl; + } +} + std::cout << "[IdleControlMode] Exited.\n"; } void IdleControlMode::stop() { is_running_ = false; - std::cout << "[IdleControlMode] Stopped idle mode." << std::endl; -} -void IdleControlMode::updateOnce() { - try { - franka::RobotState new_state = robot_->readOnce(); - { - std::lock_guard lock(state_mutex_); - current_state_ = new_state; - } - //std::cout << current_state_<< std::endl; - } catch (const franka::Exception& e) { - std::cerr << "[IdleControlMode] Failed to read robot state: " << e.what() << std::endl; - } + std::cout << "[IdleControlMode] Stopping idle mode." << std::endl; } diff --git a/src/control_mode/joint_pd_test_mode.cpp b/src/control_mode/joint_pd_test_mode.cpp new file mode 100644 index 0000000..9689951 --- /dev/null +++ b/src/control_mode/joint_pd_test_mode.cpp @@ -0,0 +1,56 @@ +#include "joint_pd_test_mode.hpp" +#include +#include + +JointPDControlMode::JointPDControlMode() = default; +JointPDControlMode::~JointPDControlMode() = default; + +void JointPDControlMode::start() { + std::cout << "[JointPDControlMode] Started.\n"; + is_running_ = true; + + if (!robot_ || !model_) { + std::cerr << "[JointPDControlMode] Robot or model not set.\n"; + return; + } + + const std::array Kp = {60.0, 60.0, 60.0, 50.0, 20.0, 10.0, 10.0}; + const std::array Kd = {5.0, 5.0, 5.0, 3.0, 1.0, 0.5, 0.5}; + + std::function torque_callback = + [this, Kp, Kd](const franka::RobotState& state, franka::Duration) -> franka::Torques { + if (!is_running_) { + throw franka::ControlException("JointPDControlMode stopped."); + } + + setCurrentState(state); + + auto leader_ptr = getLeaderState(); + if (!leader_ptr) { + std::cerr << "[JointPDControlMode] No leader state available.\n"; + return franka::Torques({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); + } + + const franka::RobotState& leader = *leader_ptr; + std::array tau{}; + for (size_t i = 0; i < 7; ++i) { + double pos_error = leader.q[i] - state.q[i]; + double vel_error = leader.dq[i] - state.dq[i]; + tau[i] = Kp[i] * pos_error + Kd[i] * vel_error; + } + + return franka::Torques(tau); + }; + + try { + robot_->control(torque_callback); + } catch (const franka::Exception& e) { + std::cerr << "[JointPDControlMode] Exception: " << e.what() << std::endl; + } + + std::cout << "[JointPDControlMode] Exited.\n"; +} + +void JointPDControlMode::stop() { + is_running_ = false; +} diff --git a/src/control_mode/zero_torque_mode.cpp b/src/control_mode/zero_torque_mode.cpp new file mode 100644 index 0000000..35e2d6a --- /dev/null +++ b/src/control_mode/zero_torque_mode.cpp @@ -0,0 +1,55 @@ +#include "zero_torque_mode.hpp" +void start() override { + is_running_ = true; + std::cout << "[ZeroTorqueMode] Started.\n"; + if (!robot_ || !model_) { + std::cerr << "[ZeroTorqueMode] Robot or model not set.\n"; + return; + } + robot_->setCollisionBehavior( + {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); + std::function torque_callback = + [this](const franka::RobotState& state, franka::Duration) -> franka::Torques { + if (!is_running_) { + throw franka::ControlException("ZeroTorqueControlMode stopped."); + } + + setCurrentState(state); + + + // test get leader state + auto leader_ptr = getLeaderState(); + if (leader_ptr) { + const auto& leader = *leader_ptr; + + // print leader state + std::cout << "[Leader q] "; + for (double q_i : leader.q) { + std::cout << q_i << " "; + } + std::cout << std::endl; + } else { + std::cout << "[ZeroTorqueMode] No leader state available." << std::endl; + } + std::array zero_torque = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + return franka::Torques(zero_torque); + }; + + //start control callback + try { + robot_->control(torque_callback); + } catch (const franka::Exception& e) { + std::cerr << "[ZeroTorqueMode] Exception: " << e.what() << std::endl; + } + + std::cout << "[ZeroTorqueMode] Exited.\n"; + } + + void stop() override { + is_running_ = false; + std::cout << "[ZeroTorqueMode] Stopping Zero Torque mode." << std::endl; + } + From b1753d88a88affae4e013a3eaf10333fbd407daa Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Tue, 8 Jul 2025 20:31:52 +0200 Subject: [PATCH 2/7] update tool for parsing messages --- include/protocol/byte_order.hpp | 18 ++++++++- include/protocol/codec.hpp | 50 +++++++++++++++++++++++- include/protocol/mode_id.hpp | 2 +- include/protocol/msg_id.hpp | 9 +++-- src/debugger/state_debug.cpp | 6 +-- src/protocol/codec.cpp | 68 +++++++++++++++++++++++++++++++-- 6 files changed, 139 insertions(+), 14 deletions(-) diff --git a/include/protocol/byte_order.hpp b/include/protocol/byte_order.hpp index b056915..b9c9079 100644 --- a/include/protocol/byte_order.hpp +++ b/include/protocol/byte_order.hpp @@ -31,7 +31,7 @@ inline uint32_t to_big_endian_u32(uint32_t val) { #if defined(_WIN32) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) return bswap32(val); #else - return val; + return val; #endif } @@ -43,6 +43,22 @@ inline uint32_t from_big_endian_u32(uint32_t val) { #endif } +//uint16 +inline uint16_t to_big_endian_u16(uint16_t val) { +#if defined(_WIN32) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) + return bswap16(val); +#else + return val; +#endif +} + +inline uint16_t from_big_endian_u16(uint16_t val) { +#if defined(_WIN32) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) + return bswap16(val); +#else + return val; +#endif + //double inline double to_big_endian_f64(double val) { static_assert(sizeof(double) == 8, "Unexpected double size"); diff --git a/include/protocol/codec.hpp b/include/protocol/codec.hpp index a868d75..646c9e3 100644 --- a/include/protocol/codec.hpp +++ b/include/protocol/codec.hpp @@ -4,6 +4,7 @@ #include "protocol/message_header.hpp" #include "protocol/msg_id.hpp" #include "protocol/franka_arm_state.hpp" +#include "protocol/franka_gripper_state.hpp" #include #include #include @@ -59,6 +60,14 @@ inline void encode_u32(uint8_t*& ptr, uint32_t val) { ptr += sizeof(be_val); } +//encode uint16_t +inline void encode_u16(uint8_t*& ptr, uint16_t val) { + uint16_t be_val = to_big_endian_u16(val); + std::memcpy(ptr, &be_val, sizeof(be_val)); + ptr += sizeof(be_val); +} + + // decode uint32_t inline uint32_t decode_u32(const uint8_t*& ptr) { uint32_t raw; @@ -67,11 +76,50 @@ inline uint32_t decode_u32(const uint8_t*& ptr) { return from_big_endian_u32(raw); } +// decode uint16_t +inline uint16_t decode_u16(const uint8_t*& ptr) { + uint16_t raw; + std::memcpy(&raw, ptr, sizeof(raw)); + ptr += sizeof(raw); + return from_big_endian_u16(raw); +} + +//encode double +inline void encode_f64(uint8_t*& ptr, double val) { + double be_val = to_big_endian_f64(val); + std::memcpy(ptr, &be_val, sizeof(be_val)); + ptr += sizeof(be_val); +} +// decode double +inline double decode_f64(const uint8_t*& ptr) { + double raw; + std::memcpy(&raw, ptr, sizeof(raw)); + ptr += sizeof(raw); + return from_big_endian_f64(raw); +} + +// encode bool +inline void encode_bool(uint8_t*& ptr, bool val) { + uint8_t byte_val = val ? 1 : 0; // Convert bool to uint8_t + std::memcpy(ptr, &byte_val, sizeof(byte_val)); + ptr += sizeof(byte_val); +} + +// decode bool +inline bool decode_bool(const uint8_t*& ptr) { + uint8_t byte_val; + std::memcpy(&byte_val, ptr, sizeof(byte_val)); + ptr += sizeof(byte_val); + return byte_val != 0; // Convert uint8_t back to bool +} + std::vector encodeMessage(const MessageHeader& header, const std::vector& payload); std::vector encodeStateMessage(const FrankaArmState& state); std::vector encodeModeMessage(uint8_t mode_code); std::vector encodeErrorMessage(uint8_t error_code); - +std::vector encodeGripperMessage(const FrankaGripperState& gripper_state) +bool decodeStateMessage(const std::vector& data, FrankaArmState& arm_state); +bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state) diff --git a/include/protocol/mode_id.hpp b/include/protocol/mode_id.hpp index ccef466..812e1ed 100644 --- a/include/protocol/mode_id.hpp +++ b/include/protocol/mode_id.hpp @@ -18,7 +18,7 @@ inline std::string toString(ModeID mode) { switch (mode) { case ModeID::IDLE: return "idle"; case ModeID::JOINT_VELOCITY: return "joint_velocity"; - default: return "unknown"; + default: return "idle"; } } diff --git a/include/protocol/msg_id.hpp b/include/protocol/msg_id.hpp index 1f7fb51..d0c5632 100644 --- a/include/protocol/msg_id.hpp +++ b/include/protocol/msg_id.hpp @@ -7,14 +7,15 @@ namespace protocol { enum class MsgID : uint8_t { // Client → Server GET_STATE_REQ = 0x01, // Request a single FrankaArmState - QUERY_STATE_REQ = 0x02, // Ask for active control mode - START_CONTROL_REQ = 0x03, // Switch to desired mode + GET_CONTROL_MODE_REQ = 0x02, // Ask for active control mode + SET_CONTROL_MODE_REQ = 0x03, // Switch to desired mode GET_SUB_PORT_REQ = 0x04, // Query PUB port number + GRIPPER_COMMAND_REQ = 0x05, // Gripper command todo:add in client // Server → Client GET_STATE_RESP = 0x51, //Respond to GET_STATE_REQ with FrankaArmState - QUERY_STATE_RESP = 0x52, //Respond to QUERY_STATE_REQ (1 byte: ControlMode) - START_CONTROL_RESP = 0x53, //Respond to START_CONTROL_REQ (1 byte: status,0 = OK) + GET_CONTROL_MODE_RESP = 0x52, //Respond to QUERY_STATE_REQ (1 byte: ControlMode) + SET_CONTROL_MODE_RESP = 0x53, //Respond to START_CONTROL_REQ (1 byte: status,0 = OK) GET_SUB_PORT_RESP = 0x54, // Respond to GET_SUB_PORT_REQ (2 bytes: port number) // Server → Client (error) diff --git a/src/debugger/state_debug.cpp b/src/debugger/state_debug.cpp index 90f7000..d276e08 100644 --- a/src/debugger/state_debug.cpp +++ b/src/debugger/state_debug.cpp @@ -17,12 +17,12 @@ void debugPrintFrankaArmStateBuffer(const std::vector& buffer) { std::cout << "\n[DEBUG] MessageHeader:\n"; std::cout << " ID = " << static_cast(header.id) << "\n"; std::cout << " len = " << header.len << "\n"; - if (header.len != 648) { - std::cerr << "[ERROR] Header length field is not 648 (actual: " << header.len << ")" << std::endl; + if (header.len != 636) { + std::cerr << "[ERROR] Header length field is not 636 (actual: " << header.len << ")" << std::endl; } // Step 2: Decode FrankaArmState const uint8_t* payload = buffer.data() + 4; - FrankaArmState decoded = FrankaArmState::decode(payload, 648); + FrankaArmState decoded = FrankaArmState::decode(payload, 636); std::cout << std::fixed << std::setprecision(4); std::cout << "\n[DEBUG] FrankaArmState:\n"; std::cout << " timestamp_ms = " << decoded.timestamp_ms << "\n"; diff --git a/src/protocol/codec.cpp b/src/protocol/codec.cpp index 8fb4a9c..1ce43ae 100644 --- a/src/protocol/codec.cpp +++ b/src/protocol/codec.cpp @@ -1,5 +1,4 @@ #include "protocol/codec.hpp" -#include "protocol/franka_arm_state.hpp" namespace protocol { // header + payload @@ -10,9 +9,9 @@ std::vector encodeMessage(const MessageHeader& header, const std::vecto return result; } -// GET_STATE_RESP +// Arm:GET_STATE_RESP/PUB_STATE std::vector encodeStateMessage(const protocol::FrankaArmState& state) { - auto payload = state.encode(); // 648B + auto payload = state.encode(); // 636B MessageHeader header{ static_cast(MsgID::GET_STATE_RESP), static_cast(payload.size()) @@ -20,6 +19,17 @@ std::vector encodeStateMessage(const protocol::FrankaArmState& state) { return encodeMessage(header, payload); } +//Gripper:GET_STATE_RESP/PUB_STATE +std::vector encodeGripperMessage(const FrankaGripperState& gripper_state) { + auto payload = gripper_state.gripper_encode(); // 23B + MessageHeader header{ + static_cast(MsgID::GET_STATE_RESP), + static_cast(payload.size()) + }; + return encodeMessage(header, payload); +} + + // QUERY_STATE_RESP std::vector encodeModeMessage(uint8_t mode_code) { std::vector payload{mode_code}; @@ -30,11 +40,20 @@ std::vector encodeModeMessage(uint8_t mode_code) { return encodeMessage(header, payload); } - //START_CONTROL_RESP // std::vector encodeRespcontrolMessage() { // } +std::vector encodeStartControlResp(bool success, protocol::ModeID mode_id) { + protocol::Header header; + header.id = static_cast(protocol::MsgID::START_CONTROL_RESP); + header.len = 2; + std::vector payload = { + static_cast(success ? 0x00 : 0x01), + static_cast(mode_id) + }; + return protocol::encodeMessage(header, payload); +} // ERROR @@ -47,4 +66,45 @@ std::vector encodeErrorMessage(uint8_t error_code) { return encodeMessage(header, payload); } +//Arm:SUB_STATE need to check +bool decodeStateMessage(const std::vector& data, FrankaArmState& arm_state) { + if (data.size() != FrankaArmState::kSize + MessageHeader::SIZE) { + return false; // Size mismatch + } + const uint8_t* buffer = data.data() + MessageHeader::SIZE; // Skip header + try { + arm_state = FrankaArmState::decode(buffer, FrankaArmState::kSize); + return true; + } catch (const std::runtime_error& e) { + std::cerr << "[FrankaProxy] Decode error: " << e.what() << std::endl; + return false; + } +} +// Gripper:SUB_STATE need to check +bool decodeGripperMessage(const std::vector& data, FrankaGripper){ + if (data.size() != FrankaGripperState::kSize + MessageHeader::SIZE) { + return false; // Size mismatch + } + const uint8_t* buffer = data.data() + MessageHeader::SIZE; // Skip header + try { + gripper_state = FrankaGripperState::gripper_decode(buffer, FrankaGripperState::kSize); + return true; + } catch (const std::runtime_error& e) { + std::cerr << "[FrankaProxy] Decode error: " << e.what() << std::endl; + return false; + } +} + +bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state){ + if (data.size() != FrankaGripperState::kSize + MessageHeader::SIZE) { + return false; // Size mismatch + } + const uint8_t* buffer = data.data() + MessageHeader::SIZE; // Skip header + try { + gripper_state = FrankaGripperState::gripper_decode(buffer, FrankaGripperState::kSize); + return true; + } catch (const std::runtime_error& e) { + std::cerr << "[FrankaProxy] Decode error: " << e.what() << std::endl; + return false; + } } // namespace protocol \ No newline at end of file From 07454082f2c2a98fa7eed9e7cbd31c2ef21a78a0 Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Tue, 8 Jul 2025 20:32:34 +0200 Subject: [PATCH 3/7] debug the arm state and add gripper state --- include/protocol/franka_arm_state.hpp | 8 ++- include/protocol/franka_gripper_state.hpp | 43 ++++++++++++++++ src/protocol/franka_arm_state.cpp | 21 ++++++++ src/protocol/franka_gripper_state.cpp | 60 +++++++++++++++++++++++ 4 files changed, 130 insertions(+), 2 deletions(-) create mode 100644 include/protocol/franka_gripper_state.hpp create mode 100644 src/protocol/franka_gripper_state.cpp diff --git a/include/protocol/franka_arm_state.hpp b/include/protocol/franka_arm_state.hpp index 9f2dd91..cdebecc 100644 --- a/include/protocol/franka_arm_state.hpp +++ b/include/protocol/franka_arm_state.hpp @@ -14,8 +14,8 @@ namespace protocol { class FrankaArmState { public: - // RobotState Payload 648byte - static constexpr size_t kSize = 648; + // RobotState Payload 636 byte + static constexpr size_t kSize = 636; uint32_t timestamp_ms; //timestamp [ms] std::array O_T_EE; // 4×4 homogeneous EE pose std::array O_T_EE_d; // 4×4 homogeneous EE target pose @@ -37,6 +37,10 @@ class FrankaArmState { //Upstate from SDK static FrankaArmState fromRobotState(const franka::RobotState& robot_state); + + //Transfer to SDK(for follower) + franka::RobotState toRobotState(const FrankaArmState& state); + }; diff --git a/include/protocol/franka_gripper_state.hpp b/include/protocol/franka_gripper_state.hpp new file mode 100644 index 0000000..10412f0 --- /dev/null +++ b/include/protocol/franka_gripper_state.hpp @@ -0,0 +1,43 @@ +#ifndef FRANKA_GRIPPER_STATE_HPP +#define FRANKA_GRIPPER_STATE_HPP +#include +#include +#include +#include +#include +#include +#include +#include + +namespace protocol { + +class FrankaGripperState { + public: + // GripperState Payload 23byte + static constexpr size_t kSize = 23; + uint32_t timestamp_ms; //4 + double width;//8 + double max_width;//8 + bool grasped;//1 + uint16_t temperature;//2 + + // Thread safety + //mutable std::mutex mutex; //will be implicitly deleted + // Encode + std::vector gripper_encode() const; + + // Decode + static FrankaGripperState gripper_decode(const uint8_t* buffer, size_t size); + + //Upstate from SDK + static FrankaGripperState fromGripperState(const franka::GripperState& gripper_state); + + //Transfer to SDK(for follower) + franka::GripperState toGripperState(const FrankaGripperState& state) +}; + + + +} // namespace protocol + +#endif // FRANKA_GRIPPER_STATE_HPP \ No newline at end of file diff --git a/src/protocol/franka_arm_state.cpp b/src/protocol/franka_arm_state.cpp index b21b517..70b3be2 100644 --- a/src/protocol/franka_arm_state.cpp +++ b/src/protocol/franka_arm_state.cpp @@ -63,4 +63,25 @@ FrankaArmState FrankaArmState::fromRobotState(const franka::RobotState& rs) { return state; } +franka::RobotState toRobotState(const FrankaArmState& state) { + franka::RobotState rs; + + // transfer timestamp(need to test) + rs.time = franka::Duration(state.timestamp_ms * 1000000); // ms → ns + + std::copy(state.q.begin(), state.q.end(), rs.q.begin()); + std::copy(state.q_d.begin(), state.q_d.end(), rs.q_d.begin()); + std::copy(state.dq.begin(), state.dq.end(), rs.dq.begin()); + std::copy(state.dq_d.begin(), state.dq_d.end(), rs.dq_d.begin()); + std::copy(state.tau_ext_hat_filtered.begin(), state.tau_ext_hat_filtered.end(), rs.tau_ext_hat_filtered.begin()); + + std::copy(state.O_T_EE.begin(), state.O_T_EE.end(), rs.O_T_EE.begin()); + std::copy(state.O_T_EE_d.begin(), state.O_T_EE_d.end(), rs.O_T_EE_d.begin()); + std::copy(state.O_F_ext_hat_K.begin(), state.O_F_ext_hat_K.end(), rs.O_F_ext_hat_K.begin()); + std::copy(state.K_F_ext_hat_K.begin(), state.K_F_ext_hat_K.end(), rs.K_F_ext_hat_K.begin()); + + return rs; +} + + } // namespace protocol diff --git a/src/protocol/franka_gripper_state.cpp b/src/protocol/franka_gripper_state.cpp new file mode 100644 index 0000000..aa6d6b0 --- /dev/null +++ b/src/protocol/franka_gripper_state.cpp @@ -0,0 +1,60 @@ +#include "protocol/franka_gripper_state.hpp" +#include "protocol/byte_order.hpp" +#include "protocol/codec.hpp" +#include +#include +#include +namespace protocol { + + std::vector gripper_encode() const{ + std::vector buffer(kSize); + uint8_t* ptr = buffer.data(); + + encode_u32(ptr, timestamp_ms); + encode_f64(ptr, width); + encode_f64(ptr, max_width); + encode_bool(ptr, grasped); + encode_u16(ptr, temperature); + + return buffer; + } + FrankaGripperState FrankaGripperState::gripper_decode(const uint8_t* buffer, size_t size) { + if (size != kSize) { + throw std::runtime_error("FrankaGripperState::gripper_decode() size mismatch"); + } + + FrankaGripperState state; + const uint8_t* ptr = buffer; + + state.timestamp_ms = decode_u32(ptr); + state.width = decode_f64(ptr); + state.max_width = decode_f64(ptr);/ + state.grasped = decode_bool(ptr); + state.temperature = decode_u16(ptr); + + return state; + } + + FrankaGripperState FrankaGripperState::fromGripperState(const franka::GripperState& gripper_state) { + FrankaGripperState state; + state.timestamp_ms = static_cast(std::chrono::duration_cast(gripper_state.time).count()); + state.width = gripper_state.width; + state.max_width = gripper_state.max_width; + state.grasped = gripper_state.is_grasped; + state.temperature = static_cast(gripper_state.temperature); + + return state; + } + + franka::GripperState toGripperState(const FrankaGripperState& state) { + franka::GripperState gripper_state; + gripper_state.time = franka::Duration(state.timestamp_ms * 1000000);//need to be test + gripper_state.width = state.width; + gripper_state.max_width = state.max_width; + gripper_state.is_grasped = state.grasped; + gripper_state.temperature = static_cast(state.temperature); + + return gripper_state; + } + +} // namespace protocol \ No newline at end of file From 0b4e56f4b3c62e9ceab60b0d5ec25291e94fc0d7 Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Tue, 8 Jul 2025 20:33:22 +0200 Subject: [PATCH 4/7] reconstruct Proxy --- include/FrankaProxy.hpp | 35 +++- src/FrankaProxy.cpp | 405 +++++++++++++++++++++++++++++++--------- 2 files changed, 342 insertions(+), 98 deletions(-) diff --git a/include/FrankaProxy.hpp b/include/FrankaProxy.hpp index 179de64..445fb6a 100644 --- a/include/FrankaProxy.hpp +++ b/include/FrankaProxy.hpp @@ -26,10 +26,10 @@ class FrankaProxy { bool start();// Starts the Franka server, initializing the robot and communication sockets void stop();// Stops the server, cleaning up resources and shutting down communication void spin();// Main loop for processing requests - + std::string getType() const { return type_; } // Returns the type of the proxy (e.g., "Arm" or "Gripper") // State management franka::RobotState getCurrentState();// Return the current state of the robot - + protocol::FrankaGripperState FrankaProxy::getCurrentGripper();// Return the current state of the gripper // Mode management void registerControlMode(const std::string& mode, std::unique_ptr control_mode);//register the map void setControlMode(const std::string& mode); @@ -41,29 +41,34 @@ class FrankaProxy { private: // Initialization - void initialize(const std::string &filename);// Initializes the FrankaProxy with the given configuration file - void setupCommunication();// Sets up ZMQ communication sockets and threads? + void initialize(const std::string &filename);// Initializes the FrankaProxy with the given configuration file and set up communication sockets // Thread functions void statePublishThread();// ZMQ PUB, Publishes the current state of the robot at a fixed rate + void gripperPublishThread();// ZMQ PUB, Publishes the current gripper state at a fixed rate void responseSocketThread();// ZMQ REP,responds to incoming requests from clients void controlLoopThread();// Main control loop for processing commands and updating the robot state - // Message handling - void handleServiceRequest(const std::string& request, std::string& response); + void handleServiceRequest(const std::vector& request, std::vector& response) private: // Configuration YAML::Node proxy_config_; + std::string type_; std::string robot_ip_; - std::string state_pub_addr_; std::string service_addr_; - + std::string state_pub_addr_; + std::string gripper_pub_addr_; + std::string state_sub_addr_; + std::string gripper_sub_addr_; + bool follower_ = false; // Franka robot std::shared_ptr robot_; std::shared_ptr model_; + //Franka gripper + std::shared_ptr gripper_; // ZMQ communication zmq::context_t context_; @@ -72,9 +77,15 @@ class FrankaProxy { zmq::socket_t rep_socket_; // Threading + std::thread control_thread_;//controlLoopThread() + std::thread state_pub_thread_;//statePublishThread() + std::thread gripper_pub_thread_;//gripperPublishThread() + std::thread service_thread_;//responseSocketThread() - std::thread control_thread_;//controlLoopThread() + + std::thread state_sub_thread_;//stateSubscribeThread(),just for follower + std::thread gripper_sub_thread_;//gripperSubscribeThread(),just for follower // Synchronization @@ -82,15 +93,19 @@ class FrankaProxy { // State data franka::RobotState current_state_; + franka::RobotState leader_state_; //for follower to generate control + // Gripper data + franka::GripperState current_gripper_state_; + franka::GripperState leader_gripper_state_; //for follower //Control mode - std::string mode_name; AbstractControlMode* current_mode_ = nullptr; std::mutex control_mutex_; std::map> control_modes_map_; // TODO: put all the Constants to a config file static constexpr int STATE_PUB_RATE_HZ = 100; + static constexpr int GRIPPER_PUB_RATE_HZ = 100; static constexpr int SOCKET_TIMEOUT_MS = 100; static constexpr int MAX_MESSAGE_SIZE = 4096; }; diff --git a/src/FrankaProxy.cpp b/src/FrankaProxy.cpp index 77af365..6fc1a68 100644 --- a/src/FrankaProxy.cpp +++ b/src/FrankaProxy.cpp @@ -1,5 +1,7 @@ #include "FrankaProxy.hpp" #include "protocol/codec.hpp" +#include "protocol/franka_arm_state.hpp" +#include "protocol/franka_gripper_state.hpp" #include "protocol/msg_id.hpp" #include "protocol/message_header.hpp" #include @@ -16,45 +18,154 @@ static void signalHandler(int signum) { std::cout << "\n[INFO] Caught signal " << signum << ", shutting down..." << std::endl; running_flag = false; } +franka::RobotState default_state; +default_state.q = {{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; // 默认初始位置 +default_state.O_T_EE = {{ + 1.0, 0.0, 0.0, 0.3, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.5, + 0.0, 0.0, 0.0, 1.0 +}}; + +////main Proxy FrankaProxy::FrankaProxy(const std::string& config_path) - : context_(1), + : context_(1),// Initialize ZMQ context with 2 I/O threads,it can be adjusted pub_socket_(context_, ZMQ_PUB), sub_socket_(context_, ZMQ_SUB), rep_socket_(context_, ZMQ_REP), in_control_(false) { initialize(config_path); - setupCommunication(); +} + +void FrankaProxy::initialize(const std::string& filename) { + RobotConfig config(filename); + type_ = config.getValue("type", "Arm"); // Default to "Arm" if not specified + robot_ip_ = config.getValue("robot_ip"); + follower_ = config.getValue("follower", false); // Default to false if not specified + rep_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); + sub_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); + if (type_ == "Arm") + { + state_pub_addr_ = config.getValue("state_pub_addr"); + pub_socket_.bind(state_pub_addr_); + service_addr_ = config.getValue("service_addr"); + rep_socket_.bind(service_addr_); + robot_ = std::make_shared(robot_ip_); + model_ = std::make_shared(robot_->loadModel()); + if(follower_){ + state_sub_addr_ = config.getValue("state_sub_addr"); + std::cout << "[FrankaProxy] Initializing Arm Follower Proxy with IP: " << robot_ip_ << std::endl; + sub_socket_.connect(state_sub_addr_); + state_sub_thread_ = std::thread(&FrankaProxy::stateSubscribeThread, this); + leader_state_ = default_state; + } else { + std::cout << "[FrankaProxy] Initializing Arm Leader Proxy with IP: " << robot_ip_ << std::endl; + } + } + else if (type_ == "Gripper") + { + gripper_pub_addr_ = config.getValue("gripper_pub_addr"); + pub_socket_.bind(gripper_pub_addr_); + service_addr_ = config.getValue("service_addr"); + rep_socket_.bind(service_addr_); + gripper_ = std::make_shared(robot_ip_); + if(follower_){ + gripper_sub_addr_ = config.getValue("gripper_sub_addr"); + std::cout << "[FrankaProxy] Initializing Gripper Follower Proxy with IP: " << robot_ip_ << std::endl; + sub_socket_.connect(gripper_sub_addr_); + gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); + } + else + { + std::cout << "[FrankaProxy] Initializing Gripper Leader Proxy with IP: " << robot_ip_ << std::endl; + } + std::cout << "[FrankaProxy] Initializing Gripper Proxy with IP: " << robot_ip_ << std::endl; + } + else + { + throw std::runtime_error("Unsupported type in config: " + type_); + } + } FrankaProxy::~FrankaProxy() { stop(); } +////basic function //start and initialize threads bool FrankaProxy::start() { + in_control_ = True; + if (type_ == "Arm") { + if(follower_) { + state_sub_thread_ = std::thread(&FrankaProxy::stateSubscribeThread, this); + std::cout << "done arm sub"<< std::endl; + } + return startArm(); + + } else if (type_ == "Gripper") { + if(follower_) { + gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); + std::cout << "done gripper sub"<< std::endl; + } + return startGripper(); + } else { + std::cerr << "[ERROR] Unsupported type: " << type_ << std::endl; + return false; + } + +} + +bool FrankaProxy::startArm(){ in_control_ = true; std::cout << in_control_ <<"control"<< std::endl; std::cout << robot_<<"robot"<< std::endl; current_state_ = robot_->readOnce(); - //std::cout << current_state_<< std::endl; - control_thread_ = std::thread(&FrankaProxy::controlLoopThread, this); - std::cout << "done control"<< std::endl; state_pub_thread_ = std::thread(&FrankaProxy::statePublishThread, this); - std::cout << "done pub"<< std::endl; + std::cout << "done arm pub"<< std::endl; + control_thread_ = std::thread(&FrankaProxy::controlLoopThread, this); + std::cout << "done control loop"<< std::endl; + { + std::lock_guard lock(control_mutex_); + current_mode_ = control_modes_map_["idle"].get(); + } + service_thread_ = std::thread(&FrankaProxy::responseSocketThread, this); - std::cout << "done rep"<< std::endl; + std::cout << "done service"<< std::endl; return true; } +bool FrankaProxy::startGripper() { + gripper_pub_thread_ = std::thread(&FrankaProxy::gripperPublishThread, this); + std::cout << "done gripper pub"<< std::endl; + service_thread_ = std::thread(&FrankaProxy::responseSocketThread, this); + std::cout << "done service"<< std::endl; +} + +//stop sever and clean up resource void FrankaProxy::stop() { std::cout << "[INFO] Stopping FrankaProxy...\n"; + if (type_ == "Arm") { + return stopArm(); + std::cout << "[INFO] Shutdown complete.\n" + } else if (type_ == "Gripper") { + return stopGripper(); + std::cout << "[INFO] Shutdown complete.\n" + } else { + std::cerr << "[ERROR] Unsupported type: " << type_ << std::endl; + return false; + } +} + +void FrankaProxy::stopArm() { in_control_ = false; + current_mode_->stop(); // try close ZeroMQ sockets try { pub_socket_.close(); rep_socket_.close(); - //sub_socket_.close(); + sub_socket_.close(); } catch (const zmq::error_t& e) { std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; } @@ -62,10 +173,31 @@ void FrankaProxy::stop() { if (state_pub_thread_.joinable()) state_pub_thread_.join(); if (service_thread_.joinable()) service_thread_.join(); if (control_thread_.joinable()) control_thread_.join(); + if (state_sub_thread_.joinable()) state_sub_thread_.join();//if not a follower, this thread will not be created, but no hurt robot_.reset(); // release robot and model model_.reset(); - //std::cout << "[INFO] Shutdown complete.\n"; + current_mode_ = nullptr;// reset current control mode + } + +void FrankaProxy::stopGripper() { + in_control_ = false; + // try close ZeroMQ sockets + try { + pub_socket_.close(); + rep_socket_.close(); + sub_socket_.close(); + } catch (const zmq::error_t& e) { + std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; + } + // wait for closing + if (gripper_pub_thread_.joinable()) gripper_pub_thread_.join(); + if (service_thread_.joinable()) service_thread_.join(); + if (gripper_sub_thread_.joinable()) gripper_sub_thread_.join();//if not a follower, this thread will not be created, but no hurt + gripper_.reset(); // release gripper +} + + // Main loop for processing requests, ctrl-c to stop the server void FrankaProxy::spin() { std::signal(SIGINT, signalHandler); // Catch Ctrl+C to stop the server @@ -78,147 +210,224 @@ void FrankaProxy::spin() { std::cout << "[INFO] Shutdown complete.\n"; } +////get current arm & gripper state +//get current arm state franka::RobotState FrankaProxy::getCurrentState() { std::lock_guard lock(control_mutex_); if (current_mode_) { - return current_mode_->UpdateState(); + return current_mode_->getRobotState();//robot should always in a control mode, when there is no command it is idle } else { throw std::runtime_error("No control mode active."); } } -// Control mode management +//get current gripper state +protocol::FrankaGripperState FrankaProxy::getCurrentGripper(){ + franka::Gripper gripper(gripper_); + franka::GripperState gripper_state = gripper.readOnce(); + protocol::FrankaGripperState proto_gripper_state = protocol::FrankaGripperState::fromGripperState(gripper_state); + return proto_gripper_state; +} + + +//// Control mode register void FrankaProxy::registerControlMode(const std::string& mode, std::unique_ptr control_mode) { control_modes_map_[mode] = std::move(control_mode); std::cout << "Registered mode: " << mode << "\n"; } -void FrankaProxy::setControlMode(const std::string& mode) { - auto it = control_modes_map_.find(mode); - if (it != control_modes_map_.end()) { - { - std::lock_guard lock(control_mutex_); - current_mode_ = it->second.get(); - } - mode_name = mode; - std::cout << "Switched to control mode: " << mode << std::endl; - } else { - std::cerr << "Control mode not found: " << mode << std::endl; - } -} - - +//// with RobotConfig void FrankaProxy::displayConfig() const { std::cout << proxy_config_ << std::endl; } -// with RobotConfig -void FrankaProxy::initialize(const std::string& filename) { - RobotConfig config(filename); - robot_ip_ = config.getValue("robot_ip"); - state_pub_addr_ = config.getValue("state_pub_addr"); - service_addr_ = config.getValue("service_addr"); - robot_ = std::make_shared(robot_ip_); - model_ =std::make_shared(robot_->loadModel()); -} - -void FrankaProxy::setupCommunication() { - const int timeout_ms = 100; - rep_socket_.set(zmq::sockopt::rcvtimeo,timeout_ms); - - pub_socket_.bind(state_pub_addr_); - rep_socket_.bind(service_addr_); - - // sub_socket_ is unused for now -} - - +//// publish threads void FrankaProxy::statePublishThread() { while (in_control_) { protocol::FrankaArmState proto_state = protocol::FrankaArmState::fromRobotState(getCurrentState()); - auto t_start = std :: chrono::high_resolution_clock::now(); + //auto t_start = std :: chrono::high_resolution_clock::now(); auto msg = protocol::encodeStateMessage(proto_state); //debug:check with the message //protocol::debugPrintFrankaArmStateBuffer(msg); + pub_arm_socket_.send(zmq::buffer(msg), zmq::send_flags::none); + //auto t_end = std::chrono::high_resolution_clock::now(); + //auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); + //check the speed of decode and pack the message + //std::cout << "[FrankaProxy] encodeStateMessage took " << duration_us << " us" << std::endl; + //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + } +} + +void FrankaProxy::gripperPublishThread() { + while (in_control_) { + protocol::FrankaGripperState proto_gripper_state = getCurrentGripper(); + auto t_start = std::chrono::high_resolution_clock::now(); + auto msg = protocol::encodeGripperMessage(proto_gripper_state); + //debug:check with the message + //protocol::debugPrintFrankaGripperStateBuffer(msg);//maybe need pub_socket_.send(zmq::buffer(msg), zmq::send_flags::none); auto t_end = std::chrono::high_resolution_clock::now(); auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); //check the speed of decode and pack the message - //std::cout << "[FrankaProxy] encodeStateMessage took " << duration_us << " us" << std::endl; - //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ)); + std::cout << "[FrankaProxy] encodeGripperMessage took " << duration_us << " us" << std::endl; + //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ)); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } } -void FrankaProxy::responseSocketThread() { +/// subscribe threads(just for follower) +void FrankaProxy::stateSubscribeThread() { + sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages while (in_control_) { - zmq::message_t request; - if (!rep_socket_.recv(request, zmq::recv_flags::none)) continue;//skip,if fail - std::string req_str(static_cast(request.data()), request.size()); - std::string response; + try{ + zmq::message_t message; + sub_socket_.recv(message, zmq::recv_flags::none); + std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); + // Debug: Print the received message size + std::cout << "[FrankaProxy] Received state update: size = " << data.size() << std::endl; + protocol::FrankaArmState arm_state; + if(bool decodeStateMessage(data, arm_state)) + { + leader_state_ = arm_state.toRobotState(); // Convert to RobotState for follower + { + std::lock_guard lock(control_mutex_); + if (current_mode_) { + current_mode_->setLeaderState(leader_state_); + } + std::cout << "[FrankaProxy] Received state update from leader." << std::endl; + } + } + else{ + std::cerr << "[FrankaProxy] Failed to decode state message." << std::endl; + } + } catch (const zmq::error_t& e) { + std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; + break; + } + } +} - handleServiceRequest(req_str, response); - rep_socket_.send(zmq::buffer(response), zmq::send_flags::none); + +void FrankaProxy::gripperSubscribeThread() { + sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages + try{ + zmq::message_t message; + sub_socket_.recv(message, zmq::recv_flags::none); + std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); + // Debug: Print the received message size + std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; + protocol::FrankaGripperState gripper_state; + if (protocol::decodeGripperMessage(data, gripper_state)) { + leader_gripper_state_ = gripper_state.toGripperState(); // Convert to GripperState for follower + std::cout << "[FrankaProxy] Received gripper update from leader." << std::endl; + } else { + std::cerr << "[FrankaProxy] Failed to decode gripper message." << std::endl; + } + }catch (const zmq::error_t& e) { + std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; + break; } } + +//// control loop thread (just for arm) void FrankaProxy::controlLoopThread() { - AbstractControlMode* last_mode = nullptr; - std::cout << "in_control"< lock(control_mutex_); mode = current_mode_; }//control_thread read current_mode_ //std::cout << current_mode_ << "current_mode"<setRobot(robot_); - mode->setModel(model_); + //protection from nullptr + if (!mode) { std::cerr << "[Warning] No control mode selected. Waiting...\n"; std::this_thread::sleep_for(std::chrono::milliseconds(100)); continue; } - - if (mode!= last_mode) { - std::cout << "[Info] Switching control mode...\n"; - try { - if (last_mode) { - last_mode->stop(); - } - last_mode = mode; - mode->start(); // - } - catch (const std::exception& e) { - std::cerr << "[Exception] Control mode failed: " << e.what() << "\n"; - in_control_ = false; + try { + mode->setRobot(robot_); + mode->setModel(model_); + mode->start(); //block with franka::Robot::control() until stop() is called by service thread + } catch (const std::exception& e) { + std::cerr << "[Exception] Control mode failed, return idle: " << e.what() << "\n"; + // in_control_ = false; + std::lock_guard lock(control_mutex_); + current_mode_ = control_modes_map_["idle"].get(); } - } std::this_thread::sleep_for(std::chrono::milliseconds(10)); } - if (last_mode) { - last_mode->stop(); + + std::cout << "[ControlLoop] Thread exited.\n"; +} + + + +void FrankaProxy::setControlMode(const std::string& mode) { + auto it = control_modes_map_.find(mode); + if (it == control_modes_map_.end()) { + std::cerr << "[Error] Control mode not found: " << mode << std::endl; + return; } - std::cout << "[Info] Control loop thread exited.\n"; + { + std::lock_guard lock(control_mutex_); + + if (current_mode_) { + std::cout << "[Info] Stopping previous control mode...\n"; + current_mode_->stop(); // stopMotion + is_running_ = false + } + + current_mode_ = it->second.get(); + + std::cout << "[Info] Switched to control mode: " << mode << std::endl; + } +} + +////response socket thread, used for service request +void FrankaProxy::responseSocketThread() { + while (in_control_) { + //get request from client + zmq::message_t request; + if (!rep_socket_.recv(request, zmq::recv_flags::none)) continue;//skip,if fail + + std::vector req_data(static_cast(request.data()),//begin + static_cast(request.data()) + request.size())//end; + std::cout << "[FrankaProxy] Received request: msg size = " << req_data.size() << std::endl; + + //std::string response; + std::vector response; + handleServiceRequest(req_data, response); + //send response + rep_socket_.send(zmq::buffer(response), zmq::send_flags::none); + } } //todo:error message have not done -void FrankaProxy::handleServiceRequest(const std::string& request, std::string& response) { +void FrankaProxy::handleServiceRequest(const std::vector& request, std::vector& response) { + //check the request size using namespace protocol; if (request.size() < 4) { - auto err = encodeErrorMessage(0x01); - response.assign(reinterpret_cast(err.data()), err.size()); + auto response = encodeErrorMessage(0x01); return; } + + //parse the header const uint8_t* data = reinterpret_cast(request.data()); MessageHeader header = MessageHeader::decode(data); if (request.size() != 4 + header.len) { auto err = encodeErrorMessage(0x02); - response.assign(reinterpret_cast(err.data()), err.size()); + //response.assign(reinterpret_cast(err.data()), err.size()); + response = err; return; } - + const uint8_t* payload = data + 4; std::vector resp; //deal with different kinds of msg @@ -229,12 +438,12 @@ void FrankaProxy::handleServiceRequest(const std::string& request, std::string& resp = encodeStateMessage(protocol::FrankaArmState::fromRobotState(getCurrentState()));//send protocol_state to pack message break; } - case static_cast (protocol::MsgID::QUERY_STATE_REQ): + case static_cast (protocol::MsgID::GET_CONTROL_MODE_REQ): - resp = encodeModeMessage(5); //now modename is string, need to transfer after building the control_mode_id + resp = encodeModeMessage(5); //mode in string break; - case static_cast (protocol::MsgID::START_CONTROL_REQ): + case static_cast (protocol::MsgID::SET_CONTROL_MODE_REQ): { std::cout<<"header in control"<(payload[0]); std::string mode_str = protocol::toString(mode_id); std::cout<<" received the control_req"; setControlMode(mode_str); + bool success = true; + resp = encodeStartControlResp(success, mode_id); break; } case static_cast (protocol::MsgID::GET_SUB_PORT_REQ): //resp = encodeMessage(MessageHeader{protocol::MsgID::GET_SUB_PORT_RESP, 2}, {0x0C, 0x34}); // TODO: dynamic port break; - + case static_cast (protocol::MsgID::GRIPPER_COMMAND_REQ): + { + // Handle gripper command request + if (header.len < 1) { + resp = encodeErrorMessage(0x04); + break; + } + // todo:implement gripper command handling with function + //just the test: + uint8_t command = payload[0]; + if (command == 0) { // Example command, replace with actual logic + gripper_->stop(); // Stop the gripper + std::cout << "Gripper stop command received." << std::endl; + } else if (command == 1) { + gripper_->move(0.08, 0.05); // Move gripper to 8cm with speed 5cm/s + std::cout << "Gripper move command received." << std::endl; + } else { + resp = encodeErrorMessage(0x05); // Invalid command + } default: resp = encodeErrorMessage(0xFF); break; @@ -261,3 +489,4 @@ void FrankaProxy::handleServiceRequest(const std::string& request, std::string& response.assign(reinterpret_cast(resp.data()), resp.size()); } +} \ No newline at end of file From 141cd2da285949423bd645fd379256b8376a369b Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Tue, 8 Jul 2025 22:41:15 +0200 Subject: [PATCH 5/7] debug on Robot PC --- CMakeLists.txt | 3 +- include/FrankaProxy.hpp | 20 +- include/RobotConfig.hpp | 1 - include/control_mode/abstract_control_mode.h | 3 +- include/control_mode/idle_control_mode.hpp | 2 +- include/control_mode/joint_pd_test_mode.hpp | 1 - include/control_mode/zero_torque_mode.hpp | 6 +- include/protocol/byte_order.hpp | 5 +- include/protocol/codec.hpp | 7 +- include/protocol/franka_arm_state.hpp | 4 +- include/protocol/franka_gripper_state.hpp | 4 +- include/protocol/mode_id.hpp | 4 +- src/FrankaProxy.cpp | 61 +++-- src/control_mode/control_mode_registry.cpp | 5 +- src/control_mode/zero_torque_mode.cpp | 4 +- src/follow.cpp | 255 ------------------- src/human_controller.cpp | 190 -------------- src/protocol/codec.cpp | 35 +-- src/protocol/franka_arm_state.cpp | 26 +- src/protocol/franka_gripper_state.cpp | 71 +++--- 20 files changed, 145 insertions(+), 562 deletions(-) delete mode 100755 src/follow.cpp delete mode 100644 src/human_controller.cpp diff --git a/CMakeLists.txt b/CMakeLists.txt index 0dbfb6e..4f507e6 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -1,6 +1,5 @@ +project(FrankaControlProxy) cmake_minimum_required(VERSION 3.10) -project(Franka_Control_Proxy) - set(CMAKE_CXX_STANDARD 17) set(CMAKE_CXX_STANDARD_REQUIRED True) diff --git a/include/FrankaProxy.hpp b/include/FrankaProxy.hpp index 445fb6a..327500a 100644 --- a/include/FrankaProxy.hpp +++ b/include/FrankaProxy.hpp @@ -11,9 +11,11 @@ #include #include #include +#include #include #include "protocol/franka_arm_state.hpp" #include "control_mode/abstract_control_mode.h" +#include "protocol/franka_gripper_state.hpp" class FrankaProxy { @@ -29,7 +31,7 @@ class FrankaProxy { std::string getType() const { return type_; } // Returns the type of the proxy (e.g., "Arm" or "Gripper") // State management franka::RobotState getCurrentState();// Return the current state of the robot - protocol::FrankaGripperState FrankaProxy::getCurrentGripper();// Return the current state of the gripper + protocol::FrankaGripperState getCurrentGripper();// Return the current state of the gripper // Mode management void registerControlMode(const std::string& mode, std::unique_ptr control_mode);//register the map void setControlMode(const std::string& mode); @@ -42,15 +44,21 @@ class FrankaProxy { private: // Initialization void initialize(const std::string &filename);// Initializes the FrankaProxy with the given configuration file and set up communication sockets - + //Start + bool startArm();// Starts the arm control loop and initializes the necessary threads + bool startGripper();// Starts the gripper control loop and initializes the necessary threads + //Stop + void stopArm();// Stops the arm control loop and cleans up resources + void stopGripper();// Stops the gripper control loop and cleans up resources // Thread functions void statePublishThread();// ZMQ PUB, Publishes the current state of the robot at a fixed rate void gripperPublishThread();// ZMQ PUB, Publishes the current gripper state at a fixed rate void responseSocketThread();// ZMQ REP,responds to incoming requests from clients void controlLoopThread();// Main control loop for processing commands and updating the robot state - + void stateSubscribeThread();// ZMQ SUB, Subscribes to the state updates from a leader robot (for follower mode) + void gripperSubscribeThread();// ZMQ SUB, Subscribes to the gripper updates // Message handling - void handleServiceRequest(const std::vector& request, std::vector& response) + void handleServiceRequest(const std::vector& request, std::vector& response) ; private: @@ -63,7 +71,7 @@ class FrankaProxy { std::string gripper_pub_addr_; std::string state_sub_addr_; std::string gripper_sub_addr_; - bool follower_ = false; + std::string follower_ = "false"; // Franka robot std::shared_ptr robot_; std::shared_ptr model_; @@ -73,6 +81,8 @@ class FrankaProxy { // ZMQ communication zmq::context_t context_; zmq::socket_t pub_socket_; + zmq::socket_t pub_arm_socket_; + zmq::socket_t pub_gripper_socket_; zmq::socket_t sub_socket_; zmq::socket_t rep_socket_; diff --git a/include/RobotConfig.hpp b/include/RobotConfig.hpp index f772923..cc31820 100644 --- a/include/RobotConfig.hpp +++ b/include/RobotConfig.hpp @@ -46,7 +46,6 @@ class RobotConfig { } } - // Function to display the loaded configuration void display() const { diff --git a/include/control_mode/abstract_control_mode.h b/include/control_mode/abstract_control_mode.h index 98cbb82..4d935d7 100644 --- a/include/control_mode/abstract_control_mode.h +++ b/include/control_mode/abstract_control_mode.h @@ -62,7 +62,8 @@ class AbstractControlMode { franka::RobotState current_state_; mutable std::mutex state_mutex_; bool is_running_ = false; - + std::shared_ptr leader_state_; + mutable std::mutex leader_mutex_; // Protects access to leader_state_ }; #endif // ABSTRACT_CONTROL_MODE_H \ No newline at end of file diff --git a/include/control_mode/idle_control_mode.hpp b/include/control_mode/idle_control_mode.hpp index ae64269..1865084 100644 --- a/include/control_mode/idle_control_mode.hpp +++ b/include/control_mode/idle_control_mode.hpp @@ -21,6 +21,6 @@ class IdleControlMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; -} +}; #endif // IDLE_CONTROL_MODE_H \ No newline at end of file diff --git a/include/control_mode/joint_pd_test_mode.hpp b/include/control_mode/joint_pd_test_mode.hpp index 1f84b49..325bec3 100644 --- a/include/control_mode/joint_pd_test_mode.hpp +++ b/include/control_mode/joint_pd_test_mode.hpp @@ -3,7 +3,6 @@ #include "abstract_control_mode.h" #include -#include #include #include diff --git a/include/control_mode/zero_torque_mode.hpp b/include/control_mode/zero_torque_mode.hpp index fa70814..51263e2 100644 --- a/include/control_mode/zero_torque_mode.hpp +++ b/include/control_mode/zero_torque_mode.hpp @@ -11,7 +11,7 @@ #include #include -class IdleControlMode : public AbstractControlMode { +class ZeroTorqueMode : public AbstractControlMode { public: ZeroTorqueMode() = default; ~ZeroTorqueMode() override = default; @@ -19,6 +19,6 @@ class IdleControlMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; -} +}; -#endif // IDLE_CONTROL_MODE_H \ No newline at end of file +#endif // ZERO_TORQUE_MODE_HPP \ No newline at end of file diff --git a/include/protocol/byte_order.hpp b/include/protocol/byte_order.hpp index b9c9079..e07b08e 100644 --- a/include/protocol/byte_order.hpp +++ b/include/protocol/byte_order.hpp @@ -19,9 +19,11 @@ constexpr bool is_little_endian() { #if defined(_MSC_VER) #include + #define bswap16 _byteswap_ushort #define bswap32 _byteswap_ulong #define bswap64 _byteswap_uint64 #else + #define bswap16 __builtin_bswap16 #define bswap32 __builtin_bswap32 #define bswap64 __builtin_bswap64 #endif @@ -58,8 +60,9 @@ inline uint16_t from_big_endian_u16(uint16_t val) { #else return val; #endif +} -//double + //double inline double to_big_endian_f64(double val) { static_assert(sizeof(double) == 8, "Unexpected double size"); #if defined(_WIN32) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) diff --git a/include/protocol/codec.hpp b/include/protocol/codec.hpp index 646c9e3..c222489 100644 --- a/include/protocol/codec.hpp +++ b/include/protocol/codec.hpp @@ -5,6 +5,7 @@ #include "protocol/msg_id.hpp" #include "protocol/franka_arm_state.hpp" #include "protocol/franka_gripper_state.hpp" +#include "protocol/mode_id.hpp" #include #include #include @@ -117,9 +118,11 @@ std::vector encodeMessage(const MessageHeader& header, const std::vecto std::vector encodeStateMessage(const FrankaArmState& state); std::vector encodeModeMessage(uint8_t mode_code); std::vector encodeErrorMessage(uint8_t error_code); -std::vector encodeGripperMessage(const FrankaGripperState& gripper_state) +std::vector encodeGripperMessage(const FrankaGripperState& gripper_state); bool decodeStateMessage(const std::vector& data, FrankaArmState& arm_state); -bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state) +bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state); +std::vector encodeStartControlResp(bool success, ModeID mode_id); + diff --git a/include/protocol/franka_arm_state.hpp b/include/protocol/franka_arm_state.hpp index cdebecc..ee1bb3f 100644 --- a/include/protocol/franka_arm_state.hpp +++ b/include/protocol/franka_arm_state.hpp @@ -36,10 +36,10 @@ class FrankaArmState { static FrankaArmState decode(const uint8_t* buffer, size_t size); //Upstate from SDK - static FrankaArmState fromRobotState(const franka::RobotState& robot_state); + static FrankaArmState fromRobotState(const franka::RobotState& robot_state); //Transfer to SDK(for follower) - franka::RobotState toRobotState(const FrankaArmState& state); + franka::RobotState toRobotState() const; }; diff --git a/include/protocol/franka_gripper_state.hpp b/include/protocol/franka_gripper_state.hpp index 10412f0..6ca642e 100644 --- a/include/protocol/franka_gripper_state.hpp +++ b/include/protocol/franka_gripper_state.hpp @@ -30,10 +30,10 @@ class FrankaGripperState { static FrankaGripperState gripper_decode(const uint8_t* buffer, size_t size); //Upstate from SDK - static FrankaGripperState fromGripperState(const franka::GripperState& gripper_state); + static FrankaGripperState fromGripperState(const franka::GripperState& gripper_state); //Transfer to SDK(for follower) - franka::GripperState toGripperState(const FrankaGripperState& state) + franka::GripperState toGripperState() const; }; diff --git a/include/protocol/mode_id.hpp b/include/protocol/mode_id.hpp index 812e1ed..524f3ab 100644 --- a/include/protocol/mode_id.hpp +++ b/include/protocol/mode_id.hpp @@ -12,12 +12,14 @@ enum class ModeID : uint8_t { JOINT_VELOCITY = 3, HUMAN_MODE = 4, IDLE = 5, + PD_TEST = 6, }; inline std::string toString(ModeID mode) { switch (mode) { case ModeID::IDLE: return "idle"; - case ModeID::JOINT_VELOCITY: return "joint_velocity"; + case ModeID::HUMAN_MODE: return "zero_torque"; + case ModeID::PD_TEST: return "joint_pd"; default: return "idle"; } } diff --git a/src/FrankaProxy.cpp b/src/FrankaProxy.cpp index 6fc1a68..3b25c20 100644 --- a/src/FrankaProxy.cpp +++ b/src/FrankaProxy.cpp @@ -18,14 +18,17 @@ static void signalHandler(int signum) { std::cout << "\n[INFO] Caught signal " << signum << ", shutting down..." << std::endl; running_flag = false; } -franka::RobotState default_state; -default_state.q = {{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; // 默认初始位置 -default_state.O_T_EE = {{ - 1.0, 0.0, 0.0, 0.3, - 0.0, 1.0, 0.0, 0.0, - 0.0, 0.0, 1.0, 0.5, - 0.0, 0.0, 0.0, 1.0 -}}; +franka::RobotState default_state = []{ + franka::RobotState state; + state.q = {{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; //default joint positions + state.O_T_EE = {{ + 1.0, 0.0, 0.0, 0.3, + 0.0, 1.0, 0.0, 0.0, + 0.0, 0.0, 1.0, 0.5, + 0.0, 0.0, 0.0, 1.0 + }}; + return state; +}(); ////main Proxy @@ -42,7 +45,7 @@ void FrankaProxy::initialize(const std::string& filename) { RobotConfig config(filename); type_ = config.getValue("type", "Arm"); // Default to "Arm" if not specified robot_ip_ = config.getValue("robot_ip"); - follower_ = config.getValue("follower", false); // Default to false if not specified + follower_ = config.getValue("follower"); rep_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); sub_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); if (type_ == "Arm") @@ -53,7 +56,7 @@ void FrankaProxy::initialize(const std::string& filename) { rep_socket_.bind(service_addr_); robot_ = std::make_shared(robot_ip_); model_ = std::make_shared(robot_->loadModel()); - if(follower_){ + if(follower_ == "true" || follower_ == "True"){ state_sub_addr_ = config.getValue("state_sub_addr"); std::cout << "[FrankaProxy] Initializing Arm Follower Proxy with IP: " << robot_ip_ << std::endl; sub_socket_.connect(state_sub_addr_); @@ -70,7 +73,7 @@ void FrankaProxy::initialize(const std::string& filename) { service_addr_ = config.getValue("service_addr"); rep_socket_.bind(service_addr_); gripper_ = std::make_shared(robot_ip_); - if(follower_){ + if(follower_ == "true" || follower_ == "True"){ gripper_sub_addr_ = config.getValue("gripper_sub_addr"); std::cout << "[FrankaProxy] Initializing Gripper Follower Proxy with IP: " << robot_ip_ << std::endl; sub_socket_.connect(gripper_sub_addr_); @@ -96,16 +99,16 @@ FrankaProxy::~FrankaProxy() { ////basic function //start and initialize threads bool FrankaProxy::start() { - in_control_ = True; + in_control_ = true; if (type_ == "Arm") { - if(follower_) { + if(follower_ == "true" || follower_ == "True") { state_sub_thread_ = std::thread(&FrankaProxy::stateSubscribeThread, this); std::cout << "done arm sub"<< std::endl; } return startArm(); } else if (type_ == "Gripper") { - if(follower_) { + if(follower_ == "true" || follower_ == "True") { gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); std::cout << "done gripper sub"<< std::endl; } @@ -141,6 +144,7 @@ bool FrankaProxy::startGripper() { std::cout << "done gripper pub"<< std::endl; service_thread_ = std::thread(&FrankaProxy::responseSocketThread, this); std::cout << "done service"<< std::endl; + return true; } //stop sever and clean up resource @@ -148,13 +152,12 @@ void FrankaProxy::stop() { std::cout << "[INFO] Stopping FrankaProxy...\n"; if (type_ == "Arm") { return stopArm(); - std::cout << "[INFO] Shutdown complete.\n" + std::cout << "[INFO] Shutdown complete.\n"; } else if (type_ == "Gripper") { return stopGripper(); - std::cout << "[INFO] Shutdown complete.\n" + std::cout << "[INFO] Shutdown complete.\n"; } else { std::cerr << "[ERROR] Unsupported type: " << type_ << std::endl; - return false; } } @@ -222,8 +225,7 @@ franka::RobotState FrankaProxy::getCurrentState() { } //get current gripper state protocol::FrankaGripperState FrankaProxy::getCurrentGripper(){ - franka::Gripper gripper(gripper_); - franka::GripperState gripper_state = gripper.readOnce(); + franka::GripperState gripper_state = gripper_->readOnce(); protocol::FrankaGripperState proto_gripper_state = protocol::FrankaGripperState::fromGripperState(gripper_state); return proto_gripper_state; } @@ -282,13 +284,16 @@ void FrankaProxy::stateSubscribeThread() { while (in_control_) { try{ zmq::message_t message; - sub_socket_.recv(message, zmq::recv_flags::none); + auto result = sub_socket_.recv(message, zmq::recv_flags::none); + if (!result) { + std::cerr << "[FrankaProxy] Failed to receive state message." << std::endl; + continue; // Skip this iteration if no message received + } std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); // Debug: Print the received message size std::cout << "[FrankaProxy] Received state update: size = " << data.size() << std::endl; protocol::FrankaArmState arm_state; - if(bool decodeStateMessage(data, arm_state)) - { + if (protocol::decodeStateMessage(data, arm_state)) { leader_state_ = arm_state.toRobotState(); // Convert to RobotState for follower { std::lock_guard lock(control_mutex_); @@ -313,7 +318,8 @@ void FrankaProxy::gripperSubscribeThread() { sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages try{ zmq::message_t message; - sub_socket_.recv(message, zmq::recv_flags::none); + auto result = sub_socket_.recv(message, zmq::recv_flags::none); + if(!result) std::cerr << "[FrankaProxy] Failed to receive gripper message." << std::endl; std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); // Debug: Print the received message size std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; @@ -326,7 +332,6 @@ void FrankaProxy::gripperSubscribeThread() { } }catch (const zmq::error_t& e) { std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; - break; } } @@ -397,7 +402,7 @@ void FrankaProxy::responseSocketThread() { if (!rep_socket_.recv(request, zmq::recv_flags::none)) continue;//skip,if fail std::vector req_data(static_cast(request.data()),//begin - static_cast(request.data()) + request.size())//end; + static_cast(request.data()) + request.size());//end std::cout << "[FrankaProxy] Received request: msg size = " << req_data.size() << std::endl; //std::string response; @@ -430,6 +435,7 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: const uint8_t* payload = data + 4; std::vector resp; + uint8_t command = payload[0]; //deal with different kinds of msg switch (header.id) { case static_cast (protocol::MsgID::GET_STATE_REQ): @@ -472,7 +478,7 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: } // todo:implement gripper command handling with function //just the test: - uint8_t command = payload[0]; + if (command == 0) { // Example command, replace with actual logic gripper_->stop(); // Stop the gripper std::cout << "Gripper stop command received." << std::endl; @@ -487,6 +493,7 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: break; } - response.assign(reinterpret_cast(resp.data()), resp.size()); + //response.assign(reinterpret_cast(resp.data()), resp.size()); + response = resp; } } \ No newline at end of file diff --git a/src/control_mode/control_mode_registry.cpp b/src/control_mode/control_mode_registry.cpp index 0305209..ab64e50 100644 --- a/src/control_mode/control_mode_registry.cpp +++ b/src/control_mode/control_mode_registry.cpp @@ -1,8 +1,11 @@ #include "control_mode/control_mode_registry.h" #include "control_mode/idle_control_mode.hpp" +#include "control_mode/zero_torque_mode.hpp" +#include "control_mode/joint_pd_test_mode.hpp" void registerAllControlModes(FrankaProxy& proxy) { proxy.registerControlMode("idle", std::make_unique()); - + proxy.registerControlMode("zero_torque", std::make_unique()); + proxy.registerControlMode("joint_pd", std::make_unique()); } \ No newline at end of file diff --git a/src/control_mode/zero_torque_mode.cpp b/src/control_mode/zero_torque_mode.cpp index 35e2d6a..96bed75 100644 --- a/src/control_mode/zero_torque_mode.cpp +++ b/src/control_mode/zero_torque_mode.cpp @@ -1,5 +1,5 @@ #include "zero_torque_mode.hpp" -void start() override { +void ZeroTorqueMode::start() { is_running_ = true; std::cout << "[ZeroTorqueMode] Started.\n"; if (!robot_ || !model_) { @@ -48,7 +48,7 @@ void start() override { std::cout << "[ZeroTorqueMode] Exited.\n"; } - void stop() override { + void ZeroTorqueMode::stop() { is_running_ = false; std::cout << "[ZeroTorqueMode] Stopping Zero Torque mode." << std::endl; } diff --git a/src/follow.cpp b/src/follow.cpp deleted file mode 100755 index 602f736..0000000 --- a/src/follow.cpp +++ /dev/null @@ -1,255 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -void handleSIGINT(int signal) -{ - std::cout << "\nSIGINT received. Stopping..." << std::endl; - if (signal == SIGINT) - { - std::cout << "\nSIGINT received. Stopping..." << std::endl; - // isRunning = false; // Set flag to false to stop the main loop - } -} - -int main(int argc, char **argv) -{ - if (argc != 2) - { - std::cerr << "Please input the config path" << std::endl; - return 0; - } - YAML::Node config = YAML::LoadFile(argv[1]); - std::string robot_ip = config["RobotIP"].as(); - std::string publisher_addr = config["PublisherAddr"].as(); - std::string service_addr = config["ServiceAddr"].as(); - std::cout << "Robot IP: " << robot_ip << std::endl; - std::cout << "Publisher Address: " << publisher_addr << std::endl; - std::cout << "Service Address: " << service_addr << std::endl; - - // Read config file - franka::Robot robot(robot_ip); - franka::Gripper gripper(robot_ip); - // robot.setCollisionBehavior({{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - // {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, - // {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); - // Set gains for the joint impedance control. - // Stiffness - const std::array k_gains = {{600.0, 600.0, 600.0, 300.0, 100.0, 150.0, 150.0}}; - // Damping - const std::array d_gains = {{50.0, 50.0, 50.0, 50.0, 25.0, 25.0, 15.0}}; - - // const std::array k_gains = {{6.0, 6.0, 6.0, 6.0, 2.50, 1.50, 1.0}}; - // // Damping - // const std::array d_gains = {{5.0, 5.0, 5.0, 5.0, 3.0, 2.50, 1.50}}; - robot.setCollisionBehavior( - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, {{20.0, 20.0, 18.0, 18.0, 16.0, 14.0, 12.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, - {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}, {{20.0, 20.0, 20.0, 25.0, 25.0, 25.0}}); - // robot.setJointImpedance({{30, 30, 30, 25, 25, 20, 20}}); - robot.setJointImpedance({{0, 0, 0, 0, 0, 0, 0}}); - franka::Model model = robot.loadModel(); - // struct - // { - // std::mutex mutex; - // std::array target_joint_pose; - // std::array gripper; - // } _state{}; - struct - { - std::mutex mutex; - std::array target_joint_pose; - franka::RobotState robot_state; - } _state{}; - franka::RobotState initial_state = robot.readOnce(); - _state.target_joint_pose = initial_state.q; - _state.robot_state = initial_state; - std::atomic_bool running{true}; - - // ZMQ setting - zmq::context_t context{1}; - // zmq::socket_t pub_socket{context, zmq::socket_type::pub}; - // pub_socket.bind(publisher_addr); - // zmq::socket_t srv_socket(context, zmq::socket_type::rep); - // srv_socket.Connect(publisher_addr); - zmq::socket_t sub_socket(context, zmq::socket_type::sub); - sub_socket.connect(publisher_addr); - sub_socket.setsockopt(ZMQ_SUBSCRIBE, "", 0); - - - // std::thread state_publish_thread( - // [&pub_socket, &robot, &gripper, &running]() { - // while (running) - // { - // // ...existing code... - // std::this_thread::sleep_for(std::chrono::milliseconds(10)); - // std::vector msg_data; - // franka::RobotState robot_state = robot.readOnce(); - // franka::GripperState gripper_state = gripper.readOnce(); - // msg_data.insert(msg_data.end(), robot_state.q.begin(), robot_state.q.end()); - // msg_data.insert(msg_data.end(), robot_state.dq.begin(), robot_state.dq.end()); - // msg_data.push_back(static_cast(gripper_state.width)); - // msg_data.push_back(static_cast(gripper_state.max_width)); - // if (!msg_data.empty()) - // { - // pub_socket.send(zmq::buffer(msg_data.data(), msg_data.size() * sizeof(float)), - // zmq::send_flags::none); - // } - // } - // }); - - std::thread sub_thread( - [&sub_socket, &running, &_state]() { - while (running) - { - std::cout << "start sub" << std::endl; - zmq::message_t message; - sub_socket.recv(message, zmq::recv_flags::none); - std::cout << "received data" << std::endl; - if (message.size() != sizeof(std::array)) - { - std::cerr << "Received message size mismatch: expected " << sizeof(std::array) - << ", got " << message.size() << std::endl; - continue; - } - std::array target_joint_pose; - memcpy(target_joint_pose.data(), message.data(), message.size()); - for (const auto& pos : target_joint_pose) { - std::cout << pos << " "; - } - - std::cout << std::endl; - if (_state.mutex.try_lock()) - { - _state.target_joint_pose = target_joint_pose; - _state.mutex.unlock(); - } - } - }); - - - std::function joint_position_callback = - [&_state](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointPositions { - std::array joint_positions; - if (_state.mutex.try_lock()) - { - joint_positions = _state.target_joint_pose; - _state.mutex.unlock(); - } - franka::JointPositions output = franka::JointPositions(joint_positions); - return output; - }; - - // human controller - std::function - joint_torque_callback = [](const franka::RobotState &state, franka::Duration) -> franka::Torques - { - franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - return zero_torques; - }; - - // Define callback for the joint torque control loop. - std::function - impedance_control_callback = - [&_state, &model, &k_gains, &d_gains]( - const franka::RobotState &state, franka::Duration) -> franka::Torques - { - if (_state.mutex.try_lock()) - { - _state.robot_state = state; - // double sum = 0; - // for (size_t i = 0; i < state.q.size(); ++i) - // { - // // std::cout << _state.target_joint_pose[i] << " " << state.q[i] << std::endl; - // sum += std::abs(_state.target_joint_pose[i] - state.q[i]) / k_gains[i]; - // // std::cout << "sum: " << std::abs(_state.target_joint_pose[i] - state.q[i]) << " " << sum << std::endl; - // } - // std::cout << sum << std::endl; - _state.mutex.unlock(); - } - - std::array coriolis = model.coriolis(state); - std::array tau_d_calculated; - if (_state.mutex.try_lock()) - { - for (size_t i = 0; i < 7; i++) - { - // tau_d_calculated[i] = - // k_gains[i] * (_state.target_joint_pose[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i]; - tau_d_calculated[i] = k_gains[i] * (_state.target_joint_pose[i] - state.q[i]) - d_gains[i] * state.dq[i]; - } - _state.mutex.unlock(); - } - // if (in_controlled) - // { - // if (_state.mutex.try_lock()) - // { - // for (size_t i = 0; i < 7; i++) - // { - // tau_d_calculated[i] = - // k_gains[i] * (_state.target_joint_pose[i] - state.q[i]) - d_gains[i] * state.dq[i] + coriolis[i]; - // } - // _state.mutex.unlock(); - // } - // } - // else - // { - // for (size_t i = 0; i < 7; i++) - // { - // tau_d_calculated[i] = coriolis[i]; - // } - // } - std::array tau_d_rate_limited = franka::limitRate(franka::kMaxTorqueRate, tau_d_calculated, state.tau_J_d); - // Send torque command. - return tau_d_rate_limited; - }; - - - - - try - { - // robot.control(joint_torque_callback); - robot.control(impedance_control_callback); - } - catch(const std::exception& e) - { - std::cerr << e.what() << '\n'; - } - - // while (true) - // { - // std::string input; - // std::cin >> input; - // if (input == "q") - // { - // std::cout << "Exiting..." << std::endl; - // running = false; - // if (publish_thread.joinable()) - // { - // publish_thread.join(); - // std::cout << "publish thread is quit safely" << std::endl; - // } - // break; - // } - // } - - return 0; -} \ No newline at end of file diff --git a/src/human_controller.cpp b/src/human_controller.cpp deleted file mode 100644 index 5e94bc6..0000000 --- a/src/human_controller.cpp +++ /dev/null @@ -1,190 +0,0 @@ -#include -#include -#include -#include -#include -#include -#include -#include -#include -#include - -#include -#include -#include -#include -#include -#include -#include - -void handleSIGINT(int signal) -{ - std::cout << "\nSIGINT received. Stopping..." << std::endl; - if (signal == SIGINT) - { - std::cout << "\nSIGINT received. Stopping..." << std::endl; - // isRunning = false; // Set flag to false to stop the main loop - } -} - -int main(int argc, char **argv) -{ - if (argc != 2) - { - std::cerr << "Please input the config path" << std::endl; - return 0; - } - YAML::Node config = YAML::LoadFile(argv[1]); - std::string robot_ip = config["RobotIP"].as(); - std::string publisher_addr = config["PublisherAddr"].as(); - std::string service_addr = config["ServiceAddr"].as(); - std::cout << "Robot IP: " << robot_ip << std::endl; - std::cout << "Publisher Address: " << publisher_addr << std::endl; - std::cout << "Service Address: " << service_addr << std::endl; - - // Read config file - franka::Robot robot(robot_ip); - franka::Gripper gripper(robot_ip); - - franka::Model model = robot.loadModel(); - struct - { - std::mutex mutex; - std::array current_joint_pose; - std::array target_joint_pose; - std::array gripper; - } _state{}; - franka::RobotState initial_state = robot.readOnce(); - _state.current_joint_pose = initial_state.q; - _state.target_joint_pose = {{0, -M_PI_4, 0, -3 * M_PI_4, 0, M_PI_2, M_PI_4}}; - std::atomic_bool running{true}; - - // ZMQ setting - zmq::context_t context{1}; - zmq::socket_t pub_socket{context, zmq::socket_type::pub}; - pub_socket.bind(publisher_addr); - // zmq::socket_t srv_socket(context, zmq::socket_type::rep); - // srv_socket.bind(service_addr); - zmq::socket_t sub_socket(context, zmq::socket_type::sub); - - - std::thread state_publish_thread( - [&pub_socket, &_state, &running]() { - while (running) - { - // ...existing code... - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - std::vector msg_data; - std::array joint_positions; - if (_state.mutex.try_lock()) - { - joint_positions = _state.current_joint_pose; - _state.mutex.unlock(); - } - else - { - continue; - } - msg_data.insert(msg_data.end(), joint_positions.begin(), joint_positions.end()); - // msg_data.insert(msg_data.end(), robot_state.dq.begin(), robot_state.dq.end()); - // msg_data.push_back(static_cast(gripper_state.width)); - // msg_data.push_back(static_cast(gripper_state.max_width)); - if (!msg_data.empty()) - { - pub_socket.send(zmq::buffer(msg_data.data(), msg_data.size() * sizeof(double)), - zmq::send_flags::none); - for (const auto& pos : joint_positions) { - std::cout << pos << " "; - } - - std::cout << std::endl; - } - } - }); - - // std::thread sub_thread( - // [&sub_socket, &running, &_state]() { - // while (running) - // { - // zmq::message_t message; - // sub_socket.recv(message, zmq::recv_flags::none); - // if (message.size() != sizeof(std::array)) - // { - // std::cerr << "Received message size mismatch: expected " << sizeof(std::array) - // << ", got " << message.size() << std::endl; - // continue; - // } - // std::array target_joint_pose; - // memcpy(target_joint_pose.data(), message.data(), message.size()); - // if (_state.mutex.try_lock()) - // { - // _state.target_joint_pose = target_joint_pose; - // _state.mutex.unlock(); - // } - // } - // }); - - - // std::function joint_position_callback = - // [&_state](const franka::RobotState& robot_state, franka::Duration period) -> franka::JointPositions { - // std::array joint_positions; - // if (_state.mutex.try_lock()) - // { - // joint_positions = _state.target_joint_pose; - // _state.mutex.unlock(); - // } - // franka::JointPositions output = franka::JointPositions(joint_positions); - // return output; - // }; - - // human controller - std::function - joint_torque_callback = [&_state, &model](const franka::RobotState &state, franka::Duration) -> franka::Torques - { - franka::Torques zero_torques{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}}; - if (_state.mutex.try_lock()) - { - _state.current_joint_pose = state.q; - _state.mutex.unlock(); - } - // zero_torques = model.coriolis(state); - std::array velocity_compliance; - for (size_t i = 0; i < 7; ++i) { - // A simple compliance controller could be a small torque based on the position error - // For kinesthetic teaching, we allow the human to move the robot freely - velocity_compliance[i] = 1.0 * state.q_d[i]; // Torque based on velocity - } - - return zero_torques; - }; - - - - try - { - robot.control(joint_torque_callback); - } - catch(const std::exception& e) - { - std::cerr << e.what() << '\n'; - } - - // while (true) - // { - // std::string input; - // std::cin >> input; - // if (input == "q") - // { - // std::cout << "Exiting..." << std::endl; - // running = false; - // if (publish_thread.joinable()) - // { - // publish_thread.join(); - // std::cout << "publish thread is quit safely" << std::endl; - // } - // break; - // } - // } - - return 0; -} \ No newline at end of file diff --git a/src/protocol/codec.cpp b/src/protocol/codec.cpp index 1ce43ae..ddafc7c 100644 --- a/src/protocol/codec.cpp +++ b/src/protocol/codec.cpp @@ -1,4 +1,17 @@ #include "protocol/codec.hpp" +#include "protocol/message_header.hpp" +#include "protocol/msg_id.hpp" +#include "protocol/franka_arm_state.hpp" +#include "protocol/franka_gripper_state.hpp" +#include "protocol/mode_id.hpp" +#include +#include +#include +#include +#include +#include +#include +#include namespace protocol { // header + payload @@ -34,7 +47,7 @@ std::vector encodeGripperMessage(const FrankaGripperState& gripper_stat std::vector encodeModeMessage(uint8_t mode_code) { std::vector payload{mode_code}; MessageHeader header{ - static_cast(MsgID::QUERY_STATE_RESP), + static_cast(MsgID::GET_CONTROL_MODE_RESP), static_cast(payload.size()) }; // 1 Byte for mode code return encodeMessage(header, payload); @@ -44,9 +57,9 @@ std::vector encodeModeMessage(uint8_t mode_code) { // std::vector encodeRespcontrolMessage() { // } -std::vector encodeStartControlResp(bool success, protocol::ModeID mode_id) { - protocol::Header header; - header.id = static_cast(protocol::MsgID::START_CONTROL_RESP); +std::vector encodeStartControlResp(bool success, ModeID mode_id) { + MessageHeader header; + header.id = static_cast(protocol::MsgID::SET_CONTROL_MODE_RESP); header.len = 2; std::vector payload = { static_cast(success ? 0x00 : 0x01), @@ -81,7 +94,7 @@ bool decodeStateMessage(const std::vector& data, FrankaArmState& arm_st } } // Gripper:SUB_STATE need to check -bool decodeGripperMessage(const std::vector& data, FrankaGripper){ +bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state) { if (data.size() != FrankaGripperState::kSize + MessageHeader::SIZE) { return false; // Size mismatch } @@ -95,16 +108,4 @@ bool decodeGripperMessage(const std::vector& data, FrankaGripper){ } } -bool decodeGripperMessage(const std::vector& data, FrankaGripperState& gripper_state){ - if (data.size() != FrankaGripperState::kSize + MessageHeader::SIZE) { - return false; // Size mismatch - } - const uint8_t* buffer = data.data() + MessageHeader::SIZE; // Skip header - try { - gripper_state = FrankaGripperState::gripper_decode(buffer, FrankaGripperState::kSize); - return true; - } catch (const std::runtime_error& e) { - std::cerr << "[FrankaProxy] Decode error: " << e.what() << std::endl; - return false; - } } // namespace protocol \ No newline at end of file diff --git a/src/protocol/franka_arm_state.cpp b/src/protocol/franka_arm_state.cpp index 70b3be2..5d64e1e 100644 --- a/src/protocol/franka_arm_state.cpp +++ b/src/protocol/franka_arm_state.cpp @@ -63,22 +63,22 @@ FrankaArmState FrankaArmState::fromRobotState(const franka::RobotState& rs) { return state; } -franka::RobotState toRobotState(const FrankaArmState& state) { +franka::RobotState FrankaArmState::toRobotState() const { franka::RobotState rs; // transfer timestamp(need to test) - rs.time = franka::Duration(state.timestamp_ms * 1000000); // ms → ns - - std::copy(state.q.begin(), state.q.end(), rs.q.begin()); - std::copy(state.q_d.begin(), state.q_d.end(), rs.q_d.begin()); - std::copy(state.dq.begin(), state.dq.end(), rs.dq.begin()); - std::copy(state.dq_d.begin(), state.dq_d.end(), rs.dq_d.begin()); - std::copy(state.tau_ext_hat_filtered.begin(), state.tau_ext_hat_filtered.end(), rs.tau_ext_hat_filtered.begin()); - - std::copy(state.O_T_EE.begin(), state.O_T_EE.end(), rs.O_T_EE.begin()); - std::copy(state.O_T_EE_d.begin(), state.O_T_EE_d.end(), rs.O_T_EE_d.begin()); - std::copy(state.O_F_ext_hat_K.begin(), state.O_F_ext_hat_K.end(), rs.O_F_ext_hat_K.begin()); - std::copy(state.K_F_ext_hat_K.begin(), state.K_F_ext_hat_K.end(), rs.K_F_ext_hat_K.begin()); + rs.time = franka::Duration(timestamp_ms * 1000000); // ms → ns + + std::copy(q.begin(), q.end(), rs.q.begin()); + std::copy(q_d.begin(), q_d.end(), rs.q_d.begin()); + std::copy(dq.begin(), dq.end(), rs.dq.begin()); + std::copy(dq_d.begin(), dq_d.end(), rs.dq_d.begin()); + std::copy(tau_ext_hat_filtered.begin(), tau_ext_hat_filtered.end(), rs.tau_ext_hat_filtered.begin()); + + std::copy(O_T_EE.begin(), O_T_EE.end(), rs.O_T_EE.begin()); + std::copy(O_T_EE_d.begin(), O_T_EE_d.end(), rs.O_T_EE_d.begin()); + std::copy(O_F_ext_hat_K.begin(), O_F_ext_hat_K.end(), rs.O_F_ext_hat_K.begin()); + std::copy(K_F_ext_hat_K.begin(), K_F_ext_hat_K.end(), rs.K_F_ext_hat_K.begin()); return rs; } diff --git a/src/protocol/franka_gripper_state.cpp b/src/protocol/franka_gripper_state.cpp index aa6d6b0..33c2b3f 100644 --- a/src/protocol/franka_gripper_state.cpp +++ b/src/protocol/franka_gripper_state.cpp @@ -4,9 +4,10 @@ #include #include #include +#include namespace protocol { - std::vector gripper_encode() const{ +std::vector FrankaGripperState::gripper_encode() const { std::vector buffer(kSize); uint8_t* ptr = buffer.data(); @@ -18,43 +19,43 @@ namespace protocol { return buffer; } - FrankaGripperState FrankaGripperState::gripper_decode(const uint8_t* buffer, size_t size) { - if (size != kSize) { - throw std::runtime_error("FrankaGripperState::gripper_decode() size mismatch"); - } - FrankaGripperState state; - const uint8_t* ptr = buffer; +FrankaGripperState FrankaGripperState::gripper_decode(const uint8_t* buffer, size_t size) { + if (size != kSize) { + throw std::runtime_error("FrankaGripperState::gripper_decode() size mismatch"); + } - state.timestamp_ms = decode_u32(ptr); - state.width = decode_f64(ptr); - state.max_width = decode_f64(ptr);/ - state.grasped = decode_bool(ptr); - state.temperature = decode_u16(ptr); + FrankaGripperState state; + const uint8_t* ptr = buffer; - return state; - } - - FrankaGripperState FrankaGripperState::fromGripperState(const franka::GripperState& gripper_state) { - FrankaGripperState state; - state.timestamp_ms = static_cast(std::chrono::duration_cast(gripper_state.time).count()); - state.width = gripper_state.width; - state.max_width = gripper_state.max_width; - state.grasped = gripper_state.is_grasped; - state.temperature = static_cast(gripper_state.temperature); - - return state; - } + state.timestamp_ms = decode_u32(ptr); + state.width = decode_f64(ptr); + state.max_width = decode_f64(ptr); + state.grasped = decode_bool(ptr); + state.temperature = decode_u16(ptr); - franka::GripperState toGripperState(const FrankaGripperState& state) { - franka::GripperState gripper_state; - gripper_state.time = franka::Duration(state.timestamp_ms * 1000000);//need to be test - gripper_state.width = state.width; - gripper_state.max_width = state.max_width; - gripper_state.is_grasped = state.grasped; - gripper_state.temperature = static_cast(state.temperature); - - return gripper_state; + return state; } -} // namespace protocol \ No newline at end of file + FrankaGripperState FrankaGripperState::fromGripperState(const franka::GripperState& gripper_state) { + FrankaGripperState state; + state.timestamp_ms = static_cast(gripper_state.time.toMSec()); + state.width = gripper_state.width; + state.max_width = gripper_state.max_width; + state.grasped = gripper_state.is_grasped; + state.temperature = static_cast(gripper_state.temperature); + + return state; +} + +franka::GripperState FrankaGripperState::toGripperState() const { + franka::GripperState gripper_state; + gripper_state.time = franka::Duration(timestamp_ms * 1000000);//need to be test + gripper_state.width = width; + gripper_state.max_width = max_width; + gripper_state.is_grasped = grasped; + gripper_state.temperature = static_cast(temperature); + + return gripper_state; +} +} \ No newline at end of file From c6e307f16d557fe395a0cc61f4df2454f4248fe2 Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Wed, 9 Jul 2025 21:46:35 +0200 Subject: [PATCH 6/7] debug and add test client --- P1_arm.yaml | 7 ++ P1_gripper.yaml | 7 ++ P2_arm.yaml | 6 ++ P2_gripper.yaml | 6 ++ RobotConfig copy.yaml | 7 ++ RobotConfig.yaml | 13 ++- include/FrankaProxy.hpp | 4 +- src/FrankaProxy.cpp | 76 ++++++++++------- src/control_mode/zero_torque_mode.cpp | 10 ++- src/main.cpp | 7 +- test/follower_client.py | 97 ++++++++++++++++++++++ test/follower_interact_client.py | 115 ++++++++++++++++++++++++++ test/leader_client.py | 99 ++++++++++++++++++++++ test/leader_interact_client.py | 115 ++++++++++++++++++++++++++ 14 files changed, 527 insertions(+), 42 deletions(-) create mode 100644 P1_arm.yaml create mode 100644 P1_gripper.yaml create mode 100644 P2_arm.yaml create mode 100644 P2_gripper.yaml create mode 100644 RobotConfig copy.yaml create mode 100644 test/follower_client.py create mode 100644 test/follower_interact_client.py create mode 100644 test/leader_client.py create mode 100644 test/leader_interact_client.py diff --git a/P1_arm.yaml b/P1_arm.yaml new file mode 100644 index 0000000..199ab72 --- /dev/null +++ b/P1_arm.yaml @@ -0,0 +1,7 @@ +type:Arm +follower:true +robot_ip:172.16.1.2 +state_pub_addr:tcp://141.3.53.152:51000 +service_addr:tcp://141.3.53.152:51001 +sub_addr:tcp://141.3.53.152:52000 +useGripper:true diff --git a/P1_gripper.yaml b/P1_gripper.yaml new file mode 100644 index 0000000..22cb03f --- /dev/null +++ b/P1_gripper.yaml @@ -0,0 +1,7 @@ +type:Gripper +follower:true +robot_ip:172.16.1.2 +gripper_pub_addr:tcp://141.3.53.152:51003 +service_addr:tcp://141.3.53.152:51004 +sub_addr_:tcp://141.3.53.152:52003 +useGripper:true diff --git a/P2_arm.yaml b/P2_arm.yaml new file mode 100644 index 0000000..aa6656e --- /dev/null +++ b/P2_arm.yaml @@ -0,0 +1,6 @@ +type:Arm +follower:false +robot_ip:172.16.2.2 +state_pub_addr:tcp://141.3.53.152:52000 +service_addr:tcp://141.3.53.152:52001 +useGripper:true diff --git a/P2_gripper.yaml b/P2_gripper.yaml new file mode 100644 index 0000000..a5a5bc0 --- /dev/null +++ b/P2_gripper.yaml @@ -0,0 +1,6 @@ +type:Gripper +follower:false +robot_ip:172.16.2.2 +gripper_pub_addr:tcp://141.3.53.152:52003 +service_addr:tcp://141.3.53.152:52004 +useGripper:true diff --git a/RobotConfig copy.yaml b/RobotConfig copy.yaml new file mode 100644 index 0000000..01b811a --- /dev/null +++ b/RobotConfig copy.yaml @@ -0,0 +1,7 @@ +type:Gripper +follower:true +robot_ip:172.16.1.2 +gripper_pub_addr:tcp://141.3.53.152:51003 +service_addr:tcp://141.3.53.152:51004 +sub_addr:tcp://141.3.53.152:51005 +useGripper:true diff --git a/RobotConfig.yaml b/RobotConfig.yaml index d8ed00e..9e1e380 100644 --- a/RobotConfig.yaml +++ b/RobotConfig.yaml @@ -1,4 +1,9 @@ -robot_ip: 172.16.2.2 -state_pub_addr: tcp://141.3.53.152:5555 -service_addr: tcp://141.3.53.152:5556 -useGripper: true +type:Arm +follower:false +robot_ip:172.16.2.2 +state_pub_addr:tcp://141.3.53.152:5555 +service_addr:tcp://141.3.53.152:5556 +gripper_pub_addr:tcp://141.3.53.152:5557 +sub_addr:tcp://141.3.53.152:5558 +gripper_sub_addr:tcp://141.3.53.152:5559 +useGripper:true diff --git a/include/FrankaProxy.hpp b/include/FrankaProxy.hpp index 327500a..6401e82 100644 --- a/include/FrankaProxy.hpp +++ b/include/FrankaProxy.hpp @@ -81,8 +81,8 @@ class FrankaProxy { // ZMQ communication zmq::context_t context_; zmq::socket_t pub_socket_; - zmq::socket_t pub_arm_socket_; - zmq::socket_t pub_gripper_socket_; + //zmq::socket_t pub_arm_socket_; + //zmq::socket_t pub_gripper_socket_; zmq::socket_t sub_socket_; zmq::socket_t rep_socket_; diff --git a/src/FrankaProxy.cpp b/src/FrankaProxy.cpp index 3b25c20..d0ec41c 100644 --- a/src/FrankaProxy.cpp +++ b/src/FrankaProxy.cpp @@ -37,11 +37,13 @@ FrankaProxy::FrankaProxy(const std::string& config_path) pub_socket_(context_, ZMQ_PUB), sub_socket_(context_, ZMQ_SUB), rep_socket_(context_, ZMQ_REP), - in_control_(false) { - initialize(config_path); -} + in_control_(false) + { + initialize(config_path); + } void FrankaProxy::initialize(const std::string& filename) { + RobotConfig config(filename); type_ = config.getValue("type", "Arm"); // Default to "Arm" if not specified robot_ip_ = config.getValue("robot_ip"); @@ -51,17 +53,22 @@ void FrankaProxy::initialize(const std::string& filename) { if (type_ == "Arm") { state_pub_addr_ = config.getValue("state_pub_addr"); + std::cout<<"state_pub"<(robot_ip_); model_ = std::make_shared(robot_->loadModel()); + if(follower_ == "true" || follower_ == "True"){ - state_sub_addr_ = config.getValue("state_sub_addr"); + state_sub_addr_ = config.getValue("sub_addr"); + std::cout<<"sub_addr"<(robot_ip_); if(follower_ == "true" || follower_ == "True"){ - gripper_sub_addr_ = config.getValue("gripper_sub_addr"); + gripper_sub_addr_ = config.getValue("sub_addr"); std::cout << "[FrankaProxy] Initializing Gripper Follower Proxy with IP: " << robot_ip_ << std::endl; sub_socket_.connect(gripper_sub_addr_); gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); @@ -87,6 +94,7 @@ void FrankaProxy::initialize(const std::string& filename) { } else { + throw std::runtime_error("Unsupported type in config: " + type_); } @@ -99,9 +107,11 @@ FrankaProxy::~FrankaProxy() { ////basic function //start and initialize threads bool FrankaProxy::start() { + std::cout<<"go start!"<readOnce(); state_pub_thread_ = std::thread(&FrankaProxy::statePublishThread, this); std::cout << "done arm pub"<< std::endl; - control_thread_ = std::thread(&FrankaProxy::controlLoopThread, this); - std::cout << "done control loop"<< std::endl; { - std::lock_guard lock(control_mutex_); - current_mode_ = control_modes_map_["idle"].get(); + std::lock_guard lock(control_mutex_); + current_mode_ = control_modes_map_["idle"].get(); } + control_thread_ = std::thread(&FrankaProxy::controlLoopThread, this); + std::cout << "done control loop"<< std::endl; service_thread_ = std::thread(&FrankaProxy::responseSocketThread, this); std::cout << "done service"<< std::endl; @@ -168,7 +178,7 @@ void FrankaProxy::stopArm() { try { pub_socket_.close(); rep_socket_.close(); - sub_socket_.close(); + if(follower_ == "true" || follower_ == "True") sub_socket_.close(); } catch (const zmq::error_t& e) { std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; } @@ -189,7 +199,7 @@ void FrankaProxy::stopGripper() { try { pub_socket_.close(); rep_socket_.close(); - sub_socket_.close(); + if(follower_ == "true" || follower_ == "True") sub_socket_.close(); } catch (const zmq::error_t& e) { std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; } @@ -251,7 +261,7 @@ void FrankaProxy::statePublishThread() { auto msg = protocol::encodeStateMessage(proto_state); //debug:check with the message //protocol::debugPrintFrankaArmStateBuffer(msg); - pub_arm_socket_.send(zmq::buffer(msg), zmq::send_flags::none); + pub_socket_.send(zmq::buffer(msg), zmq::send_flags::none); //auto t_end = std::chrono::high_resolution_clock::now(); //auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); //check the speed of decode and pack the message @@ -272,7 +282,7 @@ void FrankaProxy::gripperPublishThread() { auto t_end = std::chrono::high_resolution_clock::now(); auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); //check the speed of decode and pack the message - std::cout << "[FrankaProxy] encodeGripperMessage took " << duration_us << " us" << std::endl; + //std::cout << "[FrankaProxy] encodeGripperMessage took " << duration_us << " us" << std::endl; //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ)); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } @@ -281,14 +291,18 @@ void FrankaProxy::gripperPublishThread() { /// subscribe threads(just for follower) void FrankaProxy::stateSubscribeThread() { sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages + sub_socket_.set(zmq::sockopt::rcvtimeo, 1000); while (in_control_) { try{ zmq::message_t message; + //std::cout<<"ja bis hier"< data(static_cast(message.data()), static_cast(message.data()) + message.size()); // Debug: Print the received message size std::cout << "[FrankaProxy] Received state update: size = " << data.size() << std::endl; @@ -315,23 +329,25 @@ void FrankaProxy::stateSubscribeThread() { void FrankaProxy::gripperSubscribeThread() { - sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages - try{ - zmq::message_t message; - auto result = sub_socket_.recv(message, zmq::recv_flags::none); - if(!result) std::cerr << "[FrankaProxy] Failed to receive gripper message." << std::endl; - std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); - // Debug: Print the received message size - std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; - protocol::FrankaGripperState gripper_state; - if (protocol::decodeGripperMessage(data, gripper_state)) { - leader_gripper_state_ = gripper_state.toGripperState(); // Convert to GripperState for follower - std::cout << "[FrankaProxy] Received gripper update from leader." << std::endl; - } else { - std::cerr << "[FrankaProxy] Failed to decode gripper message." << std::endl; + sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all message + while (in_control_) { + try{ + zmq::message_t message; + auto result = sub_socket_.recv(message, zmq::recv_flags::none); + if(!result) std::cerr << "[FrankaProxy] Failed to receive gripper message." << std::endl; + std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); + // Debug: Print the received message size + std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; + protocol::FrankaGripperState gripper_state; + if (protocol::decodeGripperMessage(data, gripper_state)) { + leader_gripper_state_ = gripper_state.toGripperState(); // Convert to GripperState for follower + std::cout << "[FrankaProxy] Received gripper update from leader." << std::endl; + } else { + std::cerr << "[FrankaProxy] Failed to decode gripper message." << std::endl; + } + }catch (const zmq::error_t& e) { + std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; } - }catch (const zmq::error_t& e) { - std::cerr << "[FrankaProxy] ZMQ recv error: " << e.what() << std::endl; } } @@ -483,7 +499,7 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: gripper_->stop(); // Stop the gripper std::cout << "Gripper stop command received." << std::endl; } else if (command == 1) { - gripper_->move(0.08, 0.05); // Move gripper to 8cm with speed 5cm/s + gripper_->move(0.01, 0.01); // Move gripper to 8cm with speed 5cm/s std::cout << "Gripper move command received." << std::endl; } else { resp = encodeErrorMessage(0x05); // Invalid command diff --git a/src/control_mode/zero_torque_mode.cpp b/src/control_mode/zero_torque_mode.cpp index 96bed75..781c850 100644 --- a/src/control_mode/zero_torque_mode.cpp +++ b/src/control_mode/zero_torque_mode.cpp @@ -11,7 +11,7 @@ void ZeroTorqueMode::start() { {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); - std::function torque_callback = + std::function joint_torque_callback = [this](const franka::RobotState& state, franka::Duration) -> franka::Torques { if (!is_running_) { throw franka::ControlException("ZeroTorqueControlMode stopped."); @@ -34,13 +34,15 @@ void ZeroTorqueMode::start() { } else { std::cout << "[ZeroTorqueMode] No leader state available." << std::endl; } - std::array zero_torque = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - return franka::Torques(zero_torque); + // std::array zero_torque = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; + // return zero_torque; + return model_->gravity(state); }; + //start control callback try { - robot_->control(torque_callback); + robot_->control(joint_torque_callback); } catch (const franka::Exception& e) { std::cerr << "[ZeroTorqueMode] Exception: " << e.what() << std::endl; } diff --git a/src/main.cpp b/src/main.cpp index 3d40ad9..c466359 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -13,9 +13,12 @@ int main(int argc, char **argv) std::string config_path = argv[1]; FrankaProxy proxy(config_path); registerAllControlModes(proxy); - proxy.setControlMode("idle"); + std::string type = proxy.getType(); + std::cout << "[INFO] FrankaProxy initialized with type: " << type << std::endl; + std::cout<<"go start!"< ") + handle_command(command) + except KeyboardInterrupt: + print("\n[Client] exiting...") + break diff --git a/test/leader_client.py b/test/leader_client.py new file mode 100644 index 0000000..361cf02 --- /dev/null +++ b/test/leader_client.py @@ -0,0 +1,99 @@ +import zmq +import struct +import time +import threading +import signal +import sys +#ARM &GRIPPER P2 leader +# === MSGID&MODEID === +class MsgID: + GET_STATE_REQ = 0x01 #Request a single FrankaArmState + GET_CONTROL_MODE_REQ = 0x02 #Ask for active control mode + SET_CONTROL_MODE_REQ = 0x03 #Switch to desired mode + GET_SUB_PORT_REQ = 0x04 #Query PUB port number + GRIPPER_COMMAND_REQ = 0x05 #Gripper command todo:add in client + + #Server → Client + GET_STATE_RESP = 0x51 #Respond to GET_STATE_REQ with FrankaArmState + GET_CONTROL_MODE_RESP = 0x52 #Respond to QUERY_STATE_REQ (1 byte: ControlMode) + SET_CONTROL_MODE_RESP = 0x53 #Respond to START_CONTROL_REQ (1 byte: status,0 = OK) + GET_SUB_PORT_RESP = 0x54 #Respond to GET_SUB_PORT_REQ (2 bytes: port number) + + #Server → Client (error) + ERROR = 0xFF # 1 byte error code + #error details + +class ModeID: + HUMAN_MODE = 4 + IDLE = 5 + PD_TEST = 6 + +# === Server ADDR === +ARM_SUB_ADDR = "tcp://141.3.53.152:52000" +ARM_SERVICE_ADDR = "tcp://141.3.53.152:52001" +GRIPPER_SUB_ADDR = "tcp://141.3.53.152:52003" +GRIPPER_SERVICE_ADDR = "tcp://141.3.53.152:52004" + +context = zmq.Context() + +# === 1. sub === +def sub_thread(name, addr): + socket = context.socket(zmq.SUB) + socket.connect(addr) + socket.setsockopt(zmq.SUBSCRIBE, b"") # all messages + while True: + msg = socket.recv() + print(f"[{name}] receive {len(msg)} byte") + #todo:parse msg and print + #if name == "ArmState": + + +threading.Thread(target=sub_thread, args=("ArmState", ARM_SUB_ADDR), daemon=True).start() +threading.Thread(target=sub_thread, args=("GripperState", GRIPPER_SUB_ADDR), daemon=True).start() + +# === 2. req === +def send_service_request(addr, msg_id, payload): + socket = context.socket(zmq.REQ) + socket.connect(addr) + header = struct.pack("BBH", msg_id, 0, len(payload)) + socket.send(header + payload) + reply = socket.recv() + print(f"[Service {addr}] receive resp {len(reply)} byte, ID={msg_id}") + socket.close() + return reply +#arm service +def send_arm_request(msg_id, payload=b""): + return send_service_request(ARM_SERVICE_ADDR, msg_id, payload) +#gripper service +def send_gripper_request(msg_id, payload=b""): + return send_service_request(GRIPPER_SERVICE_ADDR, msg_id, payload) +def signal_handler(sig, frame): + print("\n[Client] keyboard Ctrl+C, exit...") + sys.exit(0) +signal.signal(signal.SIGINT, signal_handler) + + +# === begin test === +time.sleep(1) +#1.Gripper Move command = 1 +print("\n--- Gripper Move ---") +send_gripper_request(MsgID.GRIPPER_COMMAND_REQ, b"\x01") +#2.set Arm ControlMode to HUMAN_MODE +#print("\n--- Set Arm ControlMode to Human_mode ---") +#send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.HUMAN_MODE])) + +# print("\n--- Set Arm ControlMode to JOINT_VELOCITY (Follower) ---") +# send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.JOINT_VELOCITY])) + +#3.get arm state +print("\n--- Get Arm State ---") +send_arm_request(MsgID.GET_STATE_REQ) + +#4.get current control mode +print("\n--- Get Arm ControlMode ---") +send_arm_request(MsgID.GET_CONTROL_MODE_REQ) + + +# === keep running === +while True: + time.sleep(1) diff --git a/test/leader_interact_client.py b/test/leader_interact_client.py new file mode 100644 index 0000000..814b8c8 --- /dev/null +++ b/test/leader_interact_client.py @@ -0,0 +1,115 @@ +import zmq +import struct +import time +import threading +import signal +import sys +#P1 leader interact +# === MSGID & MODEID === +class MsgID: + GET_STATE_REQ = 0x01 + GET_CONTROL_MODE_REQ = 0x02 + SET_CONTROL_MODE_REQ = 0x03 + GET_SUB_PORT_REQ = 0x04 + GRIPPER_COMMAND_REQ = 0x05 + + GET_STATE_RESP = 0x51 + GET_CONTROL_MODE_RESP = 0x52 + SET_CONTROL_MODE_RESP = 0x53 + GET_SUB_PORT_RESP = 0x54 + ERROR = 0xFF + +class ModeID: + HUMAN_MODE = 4 + IDLE = 5 + PD_TEST = 6 + +# === Server ADDR === +ARM_SUB_ADDR = "tcp://141.3.53.152:52000" +ARM_SERVICE_ADDR = "tcp://141.3.53.152:52001" +GRIPPER_SUB_ADDR = "tcp://141.3.53.152:52003" +GRIPPER_SERVICE_ADDR = "tcp://141.3.53.152:52004" + +context = zmq.Context() + +# === sub_thread === +def sub_thread(name, addr): + socket = context.socket(zmq.SUB) + socket.connect(addr) + socket.setsockopt(zmq.SUBSCRIBE, b"") # all messages + while True: + msg = socket.recv() + print(f"[{name}] received {len(msg)} bytes") + #todo:add msg parsing and printing + +threading.Thread(target=sub_thread, args=("ArmState", ARM_SUB_ADDR), daemon=True).start() +threading.Thread(target=sub_thread, args=("GripperState", GRIPPER_SUB_ADDR), daemon=True).start() + +# === request === +def send_service_request(addr, msg_id, payload): + socket = context.socket(zmq.REQ) + socket.connect(addr) + header = struct.pack("BBH", msg_id, 0, len(payload)) + socket.send(header + payload) + reply = socket.recv() + print(f"[Service {addr}] received {len(reply)} bytes, ID={msg_id}") + socket.close() + return reply + +def send_arm_request(msg_id, payload=b""): + return send_service_request(ARM_SERVICE_ADDR, msg_id, payload) + +def send_gripper_request(msg_id, payload=b""): + return send_service_request(GRIPPER_SERVICE_ADDR, msg_id, payload) + +# === interact logic === +def handle_command(cmd: str): + cmd = cmd.strip().lower() + if cmd == "exit": + print("[Client] Exiting...") + sys.exit(0) + + elif cmd == "gripper_move": + send_gripper_request(MsgID.GRIPPER_COMMAND_REQ, b"\x01") + + elif cmd == "set_mode human_mode": + send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.HUMAN_MODE])) + + elif cmd == "set_mode idle": + send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.IDLE])) + + elif cmd == "get_arm_state": + send_arm_request(MsgID.GET_STATE_REQ) + + elif cmd == "get_control_mode": + send_arm_request(MsgID.GET_CONTROL_MODE_REQ) + + + else: + print("[Client] Unknown command.") + +# === Ctrl+C === +def signal_handler(sig, frame): + print("\n[Client] Keyboard Ctrl+C, exit...") + sys.exit(0) + +signal.signal(signal.SIGINT, signal_handler) + +# === main loop === +print("=== Franka Client Ready ===") +print("Command List:") +print(" gripper_move") +print(" set_mode human_mode") +print(" set_mode idle") +print(" get_arm_state") +print(" get_control_mode") +print(" exit") +print("===========================\n") + +while True: + try: + command = input("> ") + handle_command(command) + except KeyboardInterrupt: + print("\n[Client] exiting...") + break From bd71e7ce699918920edf2342e7ef55f44f5ac485 Mon Sep 17 00:00:00 2001 From: Zhuoyue Date: Mon, 14 Jul 2025 21:57:49 +0200 Subject: [PATCH 7/7] debug subthread and control reflix error --- P1_gripper.yaml | 14 +-- include/FrankaProxy.hpp | 10 +- include/control_mode/abstract_control_mode.h | 3 +- include/control_mode/idle_control_mode.hpp | 2 + include/control_mode/joint_pd_test_mode.hpp | 1 + include/control_mode/zero_torque_mode.hpp | 1 + src/FrankaProxy.cpp | 107 +++++++++++-------- src/control_mode/idle_control_mode.cpp | 17 +++ src/control_mode/joint_pd_test_mode.cpp | 20 +++- src/control_mode/zero_torque_mode.cpp | 96 ++++++++--------- test/follower_interact_client.py | 2 +- test/leader_interact_client.py | 2 +- 12 files changed, 163 insertions(+), 112 deletions(-) diff --git a/P1_gripper.yaml b/P1_gripper.yaml index 22cb03f..c7363da 100644 --- a/P1_gripper.yaml +++ b/P1_gripper.yaml @@ -1,7 +1,7 @@ -type:Gripper -follower:true -robot_ip:172.16.1.2 -gripper_pub_addr:tcp://141.3.53.152:51003 -service_addr:tcp://141.3.53.152:51004 -sub_addr_:tcp://141.3.53.152:52003 -useGripper:true +type:Gripper +follower:true +robot_ip:172.16.1.2 +gripper_pub_addr:tcp://141.3.53.152:51003 +service_addr:tcp://141.3.53.152:51004 +sub_addr:tcp://141.3.53.152:52003 +useGripper:true diff --git a/include/FrankaProxy.hpp b/include/FrankaProxy.hpp index 6401e82..3f7fbd6 100644 --- a/include/FrankaProxy.hpp +++ b/include/FrankaProxy.hpp @@ -80,10 +80,12 @@ class FrankaProxy { // ZMQ communication zmq::context_t context_; - zmq::socket_t pub_socket_; - //zmq::socket_t pub_arm_socket_; - //zmq::socket_t pub_gripper_socket_; - zmq::socket_t sub_socket_; + //zmq::socket_t pub_socket_; + zmq::socket_t pub_arm_socket_; + zmq::socket_t pub_gripper_socket_; + //zmq::socket_t sub_socket_; + zmq::socket_t sub_arm_socket_; + zmq::socket_t sub_gripper_socket_; zmq::socket_t rep_socket_; // Threading diff --git a/include/control_mode/abstract_control_mode.h b/include/control_mode/abstract_control_mode.h index 4d935d7..bfde718 100644 --- a/include/control_mode/abstract_control_mode.h +++ b/include/control_mode/abstract_control_mode.h @@ -16,7 +16,8 @@ class AbstractControlMode { //virtual void initialize(const RobotState& initial_state); virtual void start() = 0; virtual void stop() = 0; - + // Get the mode ID for this control mode + virtual int getModeID() const = 0; // Return the mode ID as an integer void setRobot(std::shared_ptr robot) { robot_ = std::move(robot); } diff --git a/include/control_mode/idle_control_mode.hpp b/include/control_mode/idle_control_mode.hpp index 1865084..baf5967 100644 --- a/include/control_mode/idle_control_mode.hpp +++ b/include/control_mode/idle_control_mode.hpp @@ -21,6 +21,8 @@ class IdleControlMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; + int getModeID() const override; }; + #endif // IDLE_CONTROL_MODE_H \ No newline at end of file diff --git a/include/control_mode/joint_pd_test_mode.hpp b/include/control_mode/joint_pd_test_mode.hpp index 325bec3..6240b80 100644 --- a/include/control_mode/joint_pd_test_mode.hpp +++ b/include/control_mode/joint_pd_test_mode.hpp @@ -13,6 +13,7 @@ class JointPDControlMode : public AbstractControlMode { void start() override; void stop() override; + int getModeID() const override; }; #endif // JOINT_PD_CONTROL_TEST_HPP diff --git a/include/control_mode/zero_torque_mode.hpp b/include/control_mode/zero_torque_mode.hpp index 51263e2..ecaa463 100644 --- a/include/control_mode/zero_torque_mode.hpp +++ b/include/control_mode/zero_torque_mode.hpp @@ -19,6 +19,7 @@ class ZeroTorqueMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; + int getModeID() const override; }; #endif // ZERO_TORQUE_MODE_HPP \ No newline at end of file diff --git a/src/FrankaProxy.cpp b/src/FrankaProxy.cpp index d0ec41c..b7fda14 100644 --- a/src/FrankaProxy.cpp +++ b/src/FrankaProxy.cpp @@ -34,8 +34,12 @@ franka::RobotState default_state = []{ ////main Proxy FrankaProxy::FrankaProxy(const std::string& config_path) : context_(1),// Initialize ZMQ context with 2 I/O threads,it can be adjusted - pub_socket_(context_, ZMQ_PUB), - sub_socket_(context_, ZMQ_SUB), + //pub_socket_(context_, ZMQ_PUB), + pub_arm_socket_(context_, ZMQ_PUB), + pub_gripper_socket_(context_, ZMQ_PUB), + //sub_socket_(context_, ZMQ_SUB), + sub_arm_socket_(context_, ZMQ_SUB), + sub_gripper_socket_(context_, ZMQ_SUB), rep_socket_(context_, ZMQ_REP), in_control_(false) { @@ -49,12 +53,11 @@ void FrankaProxy::initialize(const std::string& filename) { robot_ip_ = config.getValue("robot_ip"); follower_ = config.getValue("follower"); rep_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); - sub_socket_.set(zmq::sockopt::rcvtimeo, SOCKET_TIMEOUT_MS); if (type_ == "Arm") { state_pub_addr_ = config.getValue("state_pub_addr"); std::cout<<"state_pub"<(robot_ip_); + // gripper_ = std::make_shared(robot_ip_); + try { + gripper_ = std::make_shared(robot_ip_); + std::cout << "[FrankaProxy] Gripper homing..." << std::endl; + if (!gripper_->homing()) { + std::cerr << "[FrankaProxy] Gripper homing failed!" << std::endl; + } else { + std::cout << "[FrankaProxy] Gripper homing successful." << std::endl; + } + } catch (const franka::Exception& e) { + std::cerr << "[FrankaProxy] Exception while initializing Gripper: " << e.what() << std::endl; + } if(follower_ == "true" || follower_ == "True"){ gripper_sub_addr_ = config.getValue("sub_addr"); std::cout << "[FrankaProxy] Initializing Gripper Follower Proxy with IP: " << robot_ip_ << std::endl; - sub_socket_.connect(gripper_sub_addr_); - gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); + sub_gripper_socket_.connect(gripper_sub_addr_); + sub_gripper_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages + sub_gripper_socket_.set(zmq::sockopt::rcvtimeo, 50000); + //gripper_sub_thread_ = std::thread(&FrankaProxy::gripperSubscribeThread, this); } else { @@ -176,9 +194,9 @@ void FrankaProxy::stopArm() { current_mode_->stop(); // try close ZeroMQ sockets try { - pub_socket_.close(); + pub_arm_socket_.close(); rep_socket_.close(); - if(follower_ == "true" || follower_ == "True") sub_socket_.close(); + if(follower_ == "true" || follower_ == "True") sub_arm_socket_.close(); } catch (const zmq::error_t& e) { std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; } @@ -197,9 +215,9 @@ void FrankaProxy::stopGripper() { in_control_ = false; // try close ZeroMQ sockets try { - pub_socket_.close(); + pub_gripper_socket_.close(); rep_socket_.close(); - if(follower_ == "true" || follower_ == "True") sub_socket_.close(); + if(follower_ == "true" || follower_ == "True") sub_gripper_socket_.close(); } catch (const zmq::error_t& e) { std::cerr << "[ZMQ ERROR] " << e.what() << "\n"; } @@ -261,13 +279,13 @@ void FrankaProxy::statePublishThread() { auto msg = protocol::encodeStateMessage(proto_state); //debug:check with the message //protocol::debugPrintFrankaArmStateBuffer(msg); - pub_socket_.send(zmq::buffer(msg), zmq::send_flags::none); + pub_arm_socket_.send(zmq::buffer(msg), zmq::send_flags::none); //auto t_end = std::chrono::high_resolution_clock::now(); //auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); //check the speed of decode and pack the message //std::cout << "[FrankaProxy] encodeStateMessage took " << duration_us << " us" << std::endl; - //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ)); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::this_thread::sleep_for(std::chrono::milliseconds(1000 / STATE_PUB_RATE_HZ)); + //std::this_thread::sleep_for(std::chrono::milliseconds(1000)); } } @@ -278,26 +296,22 @@ void FrankaProxy::gripperPublishThread() { auto msg = protocol::encodeGripperMessage(proto_gripper_state); //debug:check with the message //protocol::debugPrintFrankaGripperStateBuffer(msg);//maybe need - pub_socket_.send(zmq::buffer(msg), zmq::send_flags::none); + pub_gripper_socket_.send(zmq::buffer(msg), zmq::send_flags::none); auto t_end = std::chrono::high_resolution_clock::now(); auto duration_us = std::chrono::duration_cast(t_end - t_start).count(); //check the speed of decode and pack the message - //std::cout << "[FrankaProxy] encodeGripperMessage took " << duration_us << " us" << std::endl; + //-std::cout << "[FrankaProxy] encodeGripperMessage took " << duration_us << " us" << std::endl; //std::this_thread::sleep_for(std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ)); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + std::this_thread::sleep_for(std::chrono::milliseconds(3000)); } } /// subscribe threads(just for follower) void FrankaProxy::stateSubscribeThread() { - sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages - sub_socket_.set(zmq::sockopt::rcvtimeo, 1000); while (in_control_) { try{ zmq::message_t message; - //std::cout<<"ja bis hier"< data(static_cast(message.data()), static_cast(message.data()) + message.size()); // Debug: Print the received message size - std::cout << "[FrankaProxy] Received state update: size = " << data.size() << std::endl; + //std::cout << "[FrankaProxy] Received state update: size = " << data.size() << std::endl; protocol::FrankaArmState arm_state; if (protocol::decodeStateMessage(data, arm_state)) { leader_state_ = arm_state.toRobotState(); // Convert to RobotState for follower @@ -314,7 +328,7 @@ void FrankaProxy::stateSubscribeThread() { if (current_mode_) { current_mode_->setLeaderState(leader_state_); } - std::cout << "[FrankaProxy] Received state update from leader." << std::endl; + //std::cout << "[FrankaProxy] Received state update from leader." << std::endl; } } else{ @@ -329,15 +343,14 @@ void FrankaProxy::stateSubscribeThread() { void FrankaProxy::gripperSubscribeThread() { - sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all message while (in_control_) { try{ zmq::message_t message; - auto result = sub_socket_.recv(message, zmq::recv_flags::none); + auto result = sub_gripper_socket_.recv(message, zmq::recv_flags::none); if(!result) std::cerr << "[FrankaProxy] Failed to receive gripper message." << std::endl; std::vector data(static_cast(message.data()), static_cast(message.data()) + message.size()); // Debug: Print the received message size - std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; + //std::cout << "[FrankaProxy] Received gripper update: size = " << data.size() << std::endl; protocol::FrankaGripperState gripper_state; if (protocol::decodeGripperMessage(data, gripper_state)) { leader_gripper_state_ = gripper_state.toGripperState(); // Convert to GripperState for follower @@ -426,6 +439,7 @@ void FrankaProxy::responseSocketThread() { handleServiceRequest(req_data, response); //send response rep_socket_.send(zmq::buffer(response), zmq::send_flags::none); + std::cout << "[FrankaProxy] Sent response: msg size = " << response.size() << std::endl; } } @@ -435,19 +449,18 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: //check the request size using namespace protocol; if (request.size() < 4) { - auto response = encodeErrorMessage(0x01); + response = encodeErrorMessage(0x01); + std::cerr << "[FrankaProxy] Invalid request size: " << request.size() << ". Expected at least 4 bytes." << std::endl; return; } //parse the header const uint8_t* data = reinterpret_cast(request.data()); MessageHeader header = MessageHeader::decode(data); - if (request.size() != 4 + header.len) { - auto err = encodeErrorMessage(0x02); - //response.assign(reinterpret_cast(err.data()), err.size()); - response = err; - return; - } + // if (request.size() != 4 + header.len) { + // response = encodeErrorMessage(0x02); + // return; + // } const uint8_t* payload = data + 4; std::vector resp; @@ -461,10 +474,17 @@ void FrankaProxy::handleServiceRequest(const std::vector& request, std: break; } case static_cast (protocol::MsgID::GET_CONTROL_MODE_REQ): - - resp = encodeModeMessage(5); //mode in string - break; - + { + std::lock_guard lock(control_mutex_); + if (current_mode_) { + resp = encodeModeMessage(current_mode_->getModeID()); + std::cout<<"resp size: "<getModeID() << std::endl; + } else { + resp = encodeErrorMessage(0x02); // No active control mode + } + break; + } case static_cast (protocol::MsgID::SET_CONTROL_MODE_REQ): { std::cout<<"header in control"<& request, std: resp = encodeErrorMessage(0xFF); break; } - + +} + std::cout << "[FrankaProxy] Response prepared, size = " << resp.size() << std::endl; //response.assign(reinterpret_cast(resp.data()), resp.size()); response = resp; -} } \ No newline at end of file diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index d96baa1..2d1adc5 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -18,6 +18,20 @@ void IdleControlMode::start() { } catch (const franka::Exception& e) { std::cerr << "[IdleMode] readOnce() failed: " << e.what() << std::endl; } + // // test get leader state + // auto leader_ptr = getLeaderState(); + // if (leader_ptr) { + // const auto& leader = *leader_ptr; + + // // print leader state + // std::cout << "[Leader q] "; + // for (double q_i : leader.q) { + // std::cout << q_i << " "; + // } + // std::cout << std::endl; + // } else { + // std::cout << "No leader state available." << std::endl; + // } } std::cout << "[IdleControlMode] Exited.\n"; } @@ -25,3 +39,6 @@ void IdleControlMode::stop() { is_running_ = false; std::cout << "[IdleControlMode] Stopping idle mode." << std::endl; } +int IdleControlMode::getModeID() const { + return 5; // Return a unique ID for the idle mode +} diff --git a/src/control_mode/joint_pd_test_mode.cpp b/src/control_mode/joint_pd_test_mode.cpp index 9689951..a8a59c0 100644 --- a/src/control_mode/joint_pd_test_mode.cpp +++ b/src/control_mode/joint_pd_test_mode.cpp @@ -13,9 +13,9 @@ void JointPDControlMode::start() { std::cerr << "[JointPDControlMode] Robot or model not set.\n"; return; } - - const std::array Kp = {60.0, 60.0, 60.0, 50.0, 20.0, 10.0, 10.0}; - const std::array Kd = {5.0, 5.0, 5.0, 3.0, 1.0, 0.5, 0.5}; + robot_->automaticErrorRecovery(); + const std::array Kp = {700.0, 700.0, 700.0, 600.0, 300.0, 300.0, 250.0}; + const std::array Kd = {50.0, 50.0, 50.0, 50.0, 25.0, 25, 15}; std::function torque_callback = [this, Kp, Kd](const franka::RobotState& state, franka::Duration) -> franka::Torques { @@ -44,9 +44,17 @@ void JointPDControlMode::start() { try { robot_->control(torque_callback); - } catch (const franka::Exception& e) { + } catch (const franka::ControlException& e) { std::cerr << "[JointPDControlMode] Exception: " << e.what() << std::endl; + if (std::string(e.what()).find("reflex") != std::string::npos) { + std::cout << "Reflex detected, attempting automatic recovery...\n"; + try { + robot_->automaticErrorRecovery(); + } catch (const franka::Exception& recovery_error) { + std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; + } } +} std::cout << "[JointPDControlMode] Exited.\n"; } @@ -54,3 +62,7 @@ void JointPDControlMode::start() { void JointPDControlMode::stop() { is_running_ = false; } + +int JointPDControlMode::getModeID() const { + return 6; // Return a unique ID for the Joint PD control mode +} diff --git a/src/control_mode/zero_torque_mode.cpp b/src/control_mode/zero_torque_mode.cpp index 781c850..88c379a 100644 --- a/src/control_mode/zero_torque_mode.cpp +++ b/src/control_mode/zero_torque_mode.cpp @@ -1,57 +1,51 @@ + #include "zero_torque_mode.hpp" +#include +#include void ZeroTorqueMode::start() { - is_running_ = true; - std::cout << "[ZeroTorqueMode] Started.\n"; - if (!robot_ || !model_) { - std::cerr << "[ZeroTorqueMode] Robot or model not set.\n"; - return; - } - robot_->setCollisionBehavior( - {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 300.0, 300.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, - {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); - std::function joint_torque_callback = - [this](const franka::RobotState& state, franka::Duration) -> franka::Torques { - if (!is_running_) { - throw franka::ControlException("ZeroTorqueControlMode stopped."); - } - - setCurrentState(state); - - - // test get leader state - auto leader_ptr = getLeaderState(); - if (leader_ptr) { - const auto& leader = *leader_ptr; - - // print leader state - std::cout << "[Leader q] "; - for (double q_i : leader.q) { - std::cout << q_i << " "; + is_running_ = true; + std::cout << "[ZeroTorqueMode] Started.\n"; + if (!robot_ || !model_) { + std::cerr << "[ZeroTorqueMode] Robot or model not set.\n"; + return; + } + robot_->setCollisionBehavior( + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}, + {{100.0, 100.0, 100.0, 100.0, 100.0, 100.0}}); + robot_->automaticErrorRecovery(); + std::function callback = + [this](const franka::RobotState& state, franka::Duration) -> franka::Torques { + if (!is_running_) { + return franka::Torques({0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}); } - std::cout << std::endl; - } else { - std::cout << "[ZeroTorqueMode] No leader state available." << std::endl; - } - // std::array zero_torque = {0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - // return zero_torque; - return model_->gravity(state); - }; - - - //start control callback + setCurrentState(state); + std::array damping_torque{}; + for (size_t i = 0; i < 7; ++i) { + damping_torque[i] = -0.5 * state.dq[i]; // 0.5 Nm·s/rad + } + return franka::Torques(damping_torque); + }; + try { + robot_->control(callback); + } catch (const franka::ControlException& e) { + std::cerr << "[ZeroTorqueMode] Exception: " << e.what() << std::endl; + if (std::string(e.what()).find("reflex") != std::string::npos) { + std::cout << "Reflex detected, attempting automatic recovery...\n"; try { - robot_->control(joint_torque_callback); - } catch (const franka::Exception& e) { - std::cerr << "[ZeroTorqueMode] Exception: " << e.what() << std::endl; + robot_->automaticErrorRecovery(); + } catch (const franka::Exception& recovery_error) { + std::cerr << "Recovery failed: " << recovery_error.what() << std::endl; } - - std::cout << "[ZeroTorqueMode] Exited.\n"; } - - void ZeroTorqueMode::stop() { - is_running_ = false; - std::cout << "[ZeroTorqueMode] Stopping Zero Torque mode." << std::endl; - } - +} + std::cout << "[ZeroTorqueMode] Exited.\n"; +} +void ZeroTorqueMode::stop() { + is_running_ = false; + std::cout << "[ZeroTorqueMode] Stopping Zero Torque mode.\n"; +} +int ZeroTorqueMode::getModeID() const { + return 4; +} \ No newline at end of file diff --git a/test/follower_interact_client.py b/test/follower_interact_client.py index 981b7f3..deccef6 100644 --- a/test/follower_interact_client.py +++ b/test/follower_interact_client.py @@ -39,7 +39,7 @@ def sub_thread(name, addr): socket.setsockopt(zmq.SUBSCRIBE, b"") # all messages while True: msg = socket.recv() - print(f"[{name}] received {len(msg)} bytes") + # print(f"[{name}] received {len(msg)} bytes") #todo:add msg parsing and printing threading.Thread(target=sub_thread, args=("ArmState", ARM_SUB_ADDR), daemon=True).start() diff --git a/test/leader_interact_client.py b/test/leader_interact_client.py index 814b8c8..2d1a501 100644 --- a/test/leader_interact_client.py +++ b/test/leader_interact_client.py @@ -39,7 +39,7 @@ def sub_thread(name, addr): socket.setsockopt(zmq.SUBSCRIBE, b"") # all messages while True: msg = socket.recv() - print(f"[{name}] received {len(msg)} bytes") + # print(f"[{name}] received {len(msg)} bytes") #todo:add msg parsing and printing threading.Thread(target=sub_thread, args=("ArmState", ARM_SUB_ADDR), daemon=True).start()