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/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..c7363da --- /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 179de64..3f7fbd6 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 { @@ -26,10 +28,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 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,40 +43,61 @@ 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 + //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::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_; + std::string follower_ = "false"; // Franka robot std::shared_ptr robot_; std::shared_ptr model_; + //Franka gripper + std::shared_ptr gripper_; // ZMQ communication zmq::context_t context_; - zmq::socket_t pub_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 + 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 +105,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/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/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..bfde718 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 @@ -13,28 +16,55 @@ 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); } 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_; 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 229c692..baf5967 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,8 @@ class IdleControlMode : public AbstractControlMode { //void initialize(const franka::RobotState& initial_state) override; void start() override; void stop() override; - -private: - void updateOnce(); // 使用 readOnce 读取当前状态 + 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 new file mode 100644 index 0000000..6240b80 --- /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 + +class JointPDControlMode : public AbstractControlMode { +public: + JointPDControlMode(); + ~JointPDControlMode() override; + + 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 new file mode 100644 index 0000000..ecaa463 --- /dev/null +++ b/include/control_mode/zero_torque_mode.hpp @@ -0,0 +1,25 @@ +#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 ZeroTorqueMode : public AbstractControlMode { +public: + ZeroTorqueMode() = default; + ~ZeroTorqueMode() override = default; + + //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/include/protocol/byte_order.hpp b/include/protocol/byte_order.hpp index b056915..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 @@ -31,7 +33,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,7 +45,24 @@ inline uint32_t from_big_endian_u32(uint32_t val) { #endif } -//double +//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"); #if defined(_WIN32) || (defined(__BYTE_ORDER__) && __BYTE_ORDER__ == __ORDER_LITTLE_ENDIAN__) diff --git a/include/protocol/codec.hpp b/include/protocol/codec.hpp index a868d75..c222489 100644 --- a/include/protocol/codec.hpp +++ b/include/protocol/codec.hpp @@ -4,6 +4,8 @@ #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 @@ -59,6 +61,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,10 +77,51 @@ 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); +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 9f2dd91..ee1bb3f 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 @@ -36,7 +36,11 @@ 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; + }; diff --git a/include/protocol/franka_gripper_state.hpp b/include/protocol/franka_gripper_state.hpp new file mode 100644 index 0000000..6ca642e --- /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; +}; + + + +} // namespace protocol + +#endif // FRANKA_GRIPPER_STATE_HPP \ No newline at end of file diff --git a/include/protocol/mode_id.hpp b/include/protocol/mode_id.hpp index ccef466..524f3ab 100644 --- a/include/protocol/mode_id.hpp +++ b/include/protocol/mode_id.hpp @@ -12,13 +12,15 @@ 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"; - default: return "unknown"; + case ModeID::HUMAN_MODE: return "zero_torque"; + case ModeID::PD_TEST: return "joint_pd"; + 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/FrankaProxy.cpp b/src/FrankaProxy.cpp index 77af365..b7fda14 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,185 @@ static void signalHandler(int signum) { std::cout << "\n[INFO] Caught signal " << signum << ", shutting down..." << std::endl; running_flag = false; } +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 FrankaProxy::FrankaProxy(const std::string& config_path) - : context_(1), - pub_socket_(context_, ZMQ_PUB), - sub_socket_(context_, ZMQ_SUB), + : context_(1),// Initialize ZMQ context with 2 I/O threads,it can be adjusted + //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) { - initialize(config_path); - setupCommunication(); + 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"); + follower_ = config.getValue("follower"); + rep_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_); + model_ = std::make_shared(robot_->loadModel()); + + if(follower_ == "true" || follower_ == "True"){ + state_sub_addr_ = config.getValue("sub_addr"); + std::cout<<"sub_addr"<(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_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 + { + 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() { + std::cout<<"go start!"<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; + { + 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; + 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 rep"<< std::endl; + std::cout << "done service"<< std::endl; return true; } +//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; + } +} + +void FrankaProxy::stopArm() { in_control_ = false; + current_mode_->stop(); // try close ZeroMQ sockets try { - pub_socket_.close(); + pub_arm_socket_.close(); rep_socket_.close(); - //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"; } @@ -62,10 +204,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_gripper_socket_.close(); + rep_socket_.close(); + if(follower_ == "true" || follower_ == "True") sub_gripper_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,149 +241,230 @@ 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::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_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)); + } +} + +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_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] 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::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(3000)); } } -void FrankaProxy::responseSocketThread() { +/// subscribe threads(just for follower) +void FrankaProxy::stateSubscribeThread() { 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; + auto result = sub_arm_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 (protocol::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() { + while (in_control_) { + try{ + zmq::message_t message; + 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; + 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; + } } } + +//// 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); + std::cout << "[FrankaProxy] Sent response: msg size = " << response.size() << std::endl; + } } //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()); + 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()); - return; - } - + // if (request.size() != 4 + header.len) { + // response = encodeErrorMessage(0x02); + // return; + // } + 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): @@ -229,12 +473,19 @@ 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): - - resp = encodeModeMessage(5); //now modename is string, need to transfer after building the control_mode_id - break; - - case static_cast (protocol::MsgID::START_CONTROL_REQ): + case static_cast (protocol::MsgID::GET_CONTROL_MODE_REQ): + { + 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"<(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: + + 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.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 + } default: resp = encodeErrorMessage(0xFF); break; } - - response.assign(reinterpret_cast(resp.data()), resp.size()); + } + 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/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/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index a769e52..2d1adc5 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -7,31 +7,38 @@ // 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; + } + // // 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"; } void IdleControlMode::stop() { is_running_ = false; - std::cout << "[IdleControlMode] Stopped idle mode." << std::endl; + std::cout << "[IdleControlMode] Stopping 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; - } +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 new file mode 100644 index 0000000..a8a59c0 --- /dev/null +++ b/src/control_mode/joint_pd_test_mode.cpp @@ -0,0 +1,68 @@ +#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; + } + 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 { + 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::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"; +} + +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 new file mode 100644 index 0000000..88c379a --- /dev/null +++ b/src/control_mode/zero_torque_mode.cpp @@ -0,0 +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, 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}); + } + 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_->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.\n"; +} +int ZeroTorqueMode::getModeID() const { + return 4; +} \ No newline at end of file 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/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/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!"< +#include +#include +#include +#include +#include +#include +#include namespace protocol { // header + payload @@ -10,9 +22,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,21 +32,41 @@ 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}; 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); } - //START_CONTROL_RESP // std::vector encodeRespcontrolMessage() { // } +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), + static_cast(mode_id) + }; + return protocol::encodeMessage(header, payload); +} // ERROR @@ -47,4 +79,33 @@ 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, 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 b21b517..5d64e1e 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 FrankaArmState::toRobotState() const { + franka::RobotState rs; + + // transfer timestamp(need to test) + 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; +} + + } // namespace protocol diff --git a/src/protocol/franka_gripper_state.cpp b/src/protocol/franka_gripper_state.cpp new file mode 100644 index 0000000..33c2b3f --- /dev/null +++ b/src/protocol/franka_gripper_state.cpp @@ -0,0 +1,61 @@ +#include "protocol/franka_gripper_state.hpp" +#include "protocol/byte_order.hpp" +#include "protocol/codec.hpp" +#include +#include +#include +#include +namespace protocol { + +std::vector FrankaGripperState::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(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 diff --git a/test/follower_client.py b/test/follower_client.py new file mode 100644 index 0000000..2437e5c --- /dev/null +++ b/test/follower_client.py @@ -0,0 +1,97 @@ +import zmq +import struct +import time +import threading +import signal +import sys +#ARM &GRIPPER P1 follower +# === 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:51000" +ARM_SERVICE_ADDR = "tcp://141.3.53.152:51001" +GRIPPER_SUB_ADDR = "tcp://141.3.53.152:51003" +GRIPPER_SERVICE_ADDR = "tcp://141.3.53.152:51004" + +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 pd_test +print("\n--- Set Arm ControlMode to pd_follower ---") +send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.PD_TEST])) + + +#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/follower_interact_client.py b/test/follower_interact_client.py new file mode 100644 index 0000000..deccef6 --- /dev/null +++ b/test/follower_interact_client.py @@ -0,0 +1,115 @@ +import zmq +import struct +import time +import threading +import signal +import sys +#P2 follower 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:51000" +ARM_SERVICE_ADDR = "tcp://141.3.53.152:51001" +GRIPPER_SUB_ADDR = "tcp://141.3.53.152:51003" +GRIPPER_SERVICE_ADDR = "tcp://141.3.53.152:51004" + +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 pd_test": + send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ModeID.PD_TEST])) + + 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 pd_test") +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 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..2d1a501 --- /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