diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fdaffa..7414210 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -15,7 +15,7 @@ endif() # ----------------------------- # Build options # ----------------------------- -option(NO_ROBOT_TESTING "Enable local test mode (skip robot connection etc.)" ON) +option(NO_ROBOT_TESTING "Enable local test mode (skip robot connection etc.)" OFF) if(NO_ROBOT_TESTING) message(STATUS "NO_ROBOT_TESTING mode is ON") @@ -53,7 +53,6 @@ find_package(glfw3 REQUIRED) find_package(Franka REQUIRED) find_package(yaml-cpp REQUIRED) find_package(zerolancom REQUIRED) - # ----------------------------- # Source collection # ----------------------------- @@ -64,13 +63,17 @@ file(GLOB_RECURSE SOURCES # ----------------------------- # Executable # ----------------------------- -add_executable(proxy src/main.cpp ${SOURCES}) +add_executable(proxy src/main.cpp ${SOURCES} + third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc + third_party/robotiq-gripper-interface/src/helpers.cc + third_party/robotiq-gripper-interface/src/timeout_reader.cc) # ----------------------------- # Include dirs & libs # ----------------------------- target_include_directories(proxy PRIVATE include + third_party/robotiq-gripper-interface/include ${Franka_INCLUDE_DIRS} ${YAML_CPP_INCLUDE_DIR} # ${pinocchio_INCLUDE_DIRS} diff --git a/config/GripperConfig.yaml b/config/GripperConfig.yaml deleted file mode 100644 index 92771f1..0000000 --- a/config/GripperConfig.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# Gripper settings -gripper_pub_rate_hz: 100 -gripper_command_rcvtimeo_ms: 500 - -# Gripper defaults -gripper_default_close_open_threshold: 0.01 -gripper_default_speed_slow: 0.01 -gripper_default_speed_fast: 20.0 -gripper_default_force: 20.0 \ No newline at end of file diff --git a/config/ProxyConfig.yaml b/config/ProxyConfig.yaml deleted file mode 100644 index 7c766eb..0000000 --- a/config/ProxyConfig.yaml +++ /dev/null @@ -1,7 +0,0 @@ -# Franka Arm Configuration for P10 -node_name: FrankaPanda -robot_ip: 10.10.10.10 -proxy_ip: 127.0.0.1 - -arm_config_path: config/RobotArmConfig.yaml -gripper_config_path: config/GripperConfig.yaml \ No newline at end of file diff --git a/config/SafetyLimitConfig.cfg b/config/SafetyLimitConfig.cfg index 0661f47..1a5a512 100644 --- a/config/SafetyLimitConfig.cfg +++ b/config/SafetyLimitConfig.cfg @@ -4,12 +4,12 @@ lpf_cutoff_freq: 100.0 cartesian_pos_upper_limits: [0.5, 0.5, 0.5] cartesian_pos_lower_limits: [-0.5, -0.5, -0.5] - +#a liitle smaller than the real limits to avoid hitting the mechanical limits joint_pos_upper_limits: [2.8973, 1.7628, 2.8973, -0.0698, 2.8973, 3.7525, 2.8973] joint_pos_lower_limits: [-2.8973, -1.7628, -2.8973, -3.0718, -2.8973, -0.0175, -2.8973] joint_vel_limits: [2.075, 2.075, 2.075, 2.075, 2.51, 2.51, 2.51] elbow_vel_limit: 2.1750 -joint_torques_limits: [86.0, 86.0, 86.0, 86.0, 11.5, 11.5, 11.5] +joint_torques_limits: [40.0, 40.0, 40.0, 40.0, 5.0, 5.0, 5.0] margin_joint_pos: 0.1 margin_joint_vel: 0.1 diff --git a/config/controller/CartesianVelocityController.yaml b/config/controller/CartesianVelocityController.yaml deleted file mode 100644 index a184537..0000000 --- a/config/controller/CartesianVelocityController.yaml +++ /dev/null @@ -1,8 +0,0 @@ -name: CartesianVelocity -command_topic: FRANKA_CARTESIAN_VELOCITY_CMD - -k_gains: [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0] -d_gains: [50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0] - -limit_rate: true -cutoff_frequency: 1 \ No newline at end of file diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index ce6d269..99a7809 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,9 +1,20 @@ name: HybridJointImpedance command_topic: FrankaPanda/joint_position_command +#polymetis default +kq_gains: [20, 30, 25, 25, 15, 10, 10] +kqd_gains: [1.0, 1.5, 1.0, 1.0, 0.5, 0.5, 0.5] +kx_gains: [100, 100, 100, 40, 40, 40] +kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2] -kq_gains: [600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0] -kqd_gains: [20.0, 20.0, 20.0, 20.0, 12.0, 8.0, 5.0] -kx_gains: [150.0, 150.0, 150.0, 20.0, 20.0, 20.0] -kxd_gains: [20.0, 20.0, 20.0, 2.0, 2.0, 2.0] -ignore_gravity: true +ignore_gravity: true # use for now +#real_robot using +# kq_gains: [16, 24, 20, 20, 12.75, 8.5, 8.5] +# kqd_gains: [0.5, 0.75, 0.5, 0.5, 0.25, 0.25, 0.25] +# kx_gains: [35, 35, 35, 14, 14, 14] +# kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] +#polymetis default +# kq_gains: [20, 30, 25, 25, 15, 10, 10] * 0.85 +# kqd_gains: [1.0, 1.5, 1.0, 1.0, 0.5, 0.5, 0.5] * 0.5 +# kx_gains: [100, 100, 100, 40, 40, 40] *0.35 +# kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2] *0.25 diff --git a/config/gripper/droid_robotiq.yaml b/config/gripper/droid_robotiq.yaml new file mode 100644 index 0000000..cedb179 --- /dev/null +++ b/config/gripper/droid_robotiq.yaml @@ -0,0 +1,20 @@ +# Robotiq gripper settings +name: FrankaPanda + +# Serial communication +gripper_port: /dev/ttyUSB0 +gripper_baud: 115200 + +# Scaling (see robotiq interface docs) +scale_alpha: 1.0 +scale_beta: 0.0 + +# Receive timeout (ms) +receive_timeout_ms: 500 + +# topics +command_topic: FrankaPanda/robotiq_gripper_command +state_topic: robotiq_gripper_state + +# State publish rate (Hz) +gripper_pub_rate_hz: 10 diff --git a/config/gripper/robot_lab_gripper_201.yaml b/config/gripper/robot_lab_gripper_201.yaml new file mode 100644 index 0000000..68f5735 --- /dev/null +++ b/config/gripper/robot_lab_gripper_201.yaml @@ -0,0 +1,10 @@ +# Gripper settings +name: FrankaFollowerGripper +gripper_ip: 10.10.10.202 + +command_topic: FrankaFollowerGripper/franka_gripper_command +state_topic: franka_gripper_state +enable_control: true + +gripper_pub_rate_hz: 100 +gripper_command_rcvtimeo_ms: 500 diff --git a/config/gripper/robot_lab_gripper_202.yaml b/config/gripper/robot_lab_gripper_202.yaml new file mode 100644 index 0000000..caaf59d --- /dev/null +++ b/config/gripper/robot_lab_gripper_202.yaml @@ -0,0 +1,10 @@ +# Gripper settings +name: FrankaLeaderGripper +gripper_ip: 10.10.10.201 + +command_topic: FrankaLeaderGripper/franka_gripper_command +state_topic: franka_gripper_state +enable_control: false + +gripper_pub_rate_hz: 100 +gripper_command_rcvtimeo_ms: 500 diff --git a/config/proxy/droid.yaml b/config/proxy/droid.yaml new file mode 100644 index 0000000..57c310d --- /dev/null +++ b/config/proxy/droid.yaml @@ -0,0 +1,14 @@ +# Franka Arm Configuration (follower) +node_name: FrankaPanda +robot_ip: 10.10.10.202 +proxy_ip: 192.168.0.232 +group: 224.0.0.1 +group_port: 7730 +group_name: DroidGroup + +robot: + - type: panda + config_path: config/robot/droid_panda.yaml +grippers: + - type: robotiq_gripper + config_path: config/gripper/droid_robotiq.yaml diff --git a/config/proxy/robot_lab_teleop_201_202.yaml b/config/proxy/robot_lab_teleop_201_202.yaml new file mode 100644 index 0000000..9fe72b0 --- /dev/null +++ b/config/proxy/robot_lab_teleop_201_202.yaml @@ -0,0 +1,17 @@ +# Franka Arm Configuration (leader) +node_name: FrankaLeader +proxy_ip: 141.3.53.63 +group: 224.0.0.1 +group_port: 7721 +group_name: hardware_collection + +robot: + - type: panda + config_path: config/robot/robot_lab_201.yaml + - type: panda + config_path: config/robot/robot_lab_202.yaml +grippers: + - type: franka_gripper + config_path: config/gripper/robot_lab_gripper_201.yaml + - type: franka_gripper + config_path: config/gripper/robot_lab_gripper_202.yaml \ No newline at end of file diff --git a/config/RobotArmConfig.yaml b/config/robot/droid_panda.yaml similarity index 92% rename from config/RobotArmConfig.yaml rename to config/robot/droid_panda.yaml index 94b473c..c425ff4 100644 --- a/config/RobotArmConfig.yaml +++ b/config/robot/droid_panda.yaml @@ -3,7 +3,7 @@ name: FrankaPanda robot_ip: 10.10.10.10 # Arm default state -arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] +arm_default_state_q: [0.1775, -0.2385, -0.1525, -2.0549, -0.1030, 1.7526, 0] arm_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] # Arm settings diff --git a/config/robot/robot_lab_panda_201.yaml b/config/robot/robot_lab_panda_201.yaml new file mode 100644 index 0000000..c11daa4 --- /dev/null +++ b/config/robot/robot_lab_panda_201.yaml @@ -0,0 +1,24 @@ +# Franka Arm Configuration (leader) +name: FrankaLeader +robot_ip: 10.10.10.201 + +# Arm default state +arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] +arm_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] + +# Arm settings +arm_state_pub_rate_hz: 100 +arm_socket_timeout_ms: 100 +arm_max_message_size: 4096 + +# Collision thresholds (acceleration) +arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30] +arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45] +arm_collision_lower_force_thresholds_acc: [8, 8, 8, 8, 8, 8] +arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15] + +# Collision thresholds (nominal) +arm_collision_lower_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35] +arm_collision_upper_torque_thresholds_nom: [50, 50, 50, 50, 50, 50, 50] +arm_collision_lower_force_thresholds_nom: [12, 12, 12, 12, 12, 12] +arm_collision_upper_force_thresholds_nom: [20, 20, 20, 20, 20, 20] diff --git a/config/robot/robot_lab_panda_202.yaml b/config/robot/robot_lab_panda_202.yaml new file mode 100644 index 0000000..347a13d --- /dev/null +++ b/config/robot/robot_lab_panda_202.yaml @@ -0,0 +1,24 @@ +# Franka Arm Configuration (follower) +name: FrankaFollower +robot_ip: 10.10.10.202 + +# Arm default state +arm_default_state_q: [0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785] +arm_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] + +# Arm settings +arm_state_pub_rate_hz: 100 +arm_socket_timeout_ms: 100 +arm_max_message_size: 4096 + +# Collision thresholds (acceleration) +arm_collision_lower_torque_thresholds_acc: [30, 30, 30, 30, 30, 30, 30] +arm_collision_upper_torque_thresholds_acc: [45, 45, 45, 45, 45, 45, 45] +arm_collision_lower_force_thresholds_acc: [8, 8, 8, 8, 8, 8] +arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15] + +# Collision thresholds (nominal) +arm_collision_lower_torque_thresholds_nom: [35, 35, 35, 35, 35, 35, 35] +arm_collision_upper_torque_thresholds_nom: [50, 50, 50, 50, 50, 50, 50] +arm_collision_lower_force_thresholds_nom: [12, 12, 12, 12, 12, 12] +arm_collision_upper_force_thresholds_nom: [20, 20, 20, 20, 20, 20] diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 6f715c0..721f653 100644 --- a/include/control_mode/abstract_control_mode.hpp +++ b/include/control_mode/abstract_control_mode.hpp @@ -1,5 +1,6 @@ #pragma once +#include #include #include #include @@ -66,8 +67,12 @@ class AbstractControlMode void startControl(); void stopControl(); const std::string getModeName(); - void controlTask(); - + virtual void controlTask(); + bool moveToJointPosition(const std::array& target_q, + double max_velocity = 0.1, double tolerance = 1e-2); + bool moveToCartesianPose(const Eigen::Vector3d& target_position, + const Eigen::Quaterniond& target_orientation, + double max_velocity = 0.5, double tolerance = 1e-3); protected: AbstractControlMode(const SafetyLimitConfig& safety_config) : safety_config_(safety_config) {} FrankaPanda* robot_; diff --git a/include/control_mode/control_mode.hpp b/include/control_mode/control_mode.hpp index cd0297d..2b8cc75 100644 --- a/include/control_mode/control_mode.hpp +++ b/include/control_mode/control_mode.hpp @@ -7,6 +7,7 @@ #include #include #include +#include class ControlModeFactory { @@ -32,6 +33,7 @@ class ControlModeFactory const SafetyLimitConfig& safety_config) { registry["Idle"] = std::make_unique(safety_config); + registry["GravityComp"] = std::make_unique(safety_config); registry["HybridJointImpedance"] = std::make_unique(safety_config); for (const auto& pair : registry) diff --git a/include/control_mode/gravity_comp_control.hpp b/include/control_mode/gravity_comp_control.hpp new file mode 100644 index 0000000..78d15e5 --- /dev/null +++ b/include/control_mode/gravity_comp_control.hpp @@ -0,0 +1,30 @@ +#pragma once +#include "control_mode/abstract_control_mode.hpp" +#include + +// Hand-guiding controller with joint-space spring + damping (no gravity compensation). +// Intended for the "leader" robot so a human can move it by hand. + +class GravityCompControl : public AbstractControlMode +{ +public: + explicit GravityCompControl(const SafetyLimitConfig& safety_config) + : AbstractControlMode(safety_config) + { + controller_name = "GravityComp"; + } + ~GravityCompControl() override = default; + + // Reset internal state. Call this right before starting control. + void reset(); + +private: + void initController(FrankaPanda& robot, PandaPinocchioModel& pinocchio_model, + AtomicDoubleBuffer& state_buffer) override; + + franka::Torques controlLoop(const franka::RobotState& robot_state, + franka::Duration duration) override; + + bool hold_initialized_{false}; + JointPosition hold_q_{JointPosition::Zero()}; +}; diff --git a/include/control_mode/idle_control_mode.hpp b/include/control_mode/idle_control_mode.hpp index cc2c234..4a5a58a 100644 --- a/include/control_mode/idle_control_mode.hpp +++ b/include/control_mode/idle_control_mode.hpp @@ -14,6 +14,7 @@ class IdleControlMode : public AbstractControlMode private: void initController(FrankaPanda& robot, PandaPinocchioModel& model, AtomicDoubleBuffer& state_buffer) override; + void controlTask() override; franka::Torques controlLoop(const franka::RobotState& robot_state, franka::Duration duration) override; }; diff --git a/include/debugger/fake_franka.hpp b/include/debugger/fake_franka.hpp deleted file mode 100644 index df59c42..0000000 --- a/include/debugger/fake_franka.hpp +++ /dev/null @@ -1,109 +0,0 @@ -#include - -#include -#include -#include - -class FakeFrankaModel -{ - public: - FakeFrankaModel() - { - zlc::info("Initialized FakeFrankaModel for testing purposes."); - } - ~FakeFrankaModel() = default; - // Add other necessary fake methods as needed for testing - std::array coriolis(const franka::RobotState& state) - { - return std::array{0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0}; - } -}; - -class FakeFrankaRobot -{ - public: - FakeFrankaRobot() = delete; - FakeFrankaRobot(const std::string&) - { - zlc::info("Initialized FakeFrankaRobot for testing purposes."); - } - - ~FakeFrankaRobot() - { - zlc::info("Destroyed FakeFrankaRobot."); - } - - void automaticErrorRecovery() - { - zlc::info("FakeFrankaRobot: automaticErrorRecovery called."); - } - - template - void control(ControllerCallback callback, bool limit_rate = true, - double cutoff_frequency = (100.0)) - { - zlc::info("FakeFrankaRobot: control loop started."); - isOnControlLoop = true; - while (isOnControlLoop) - { - franka::RobotState state = current_state_; - franka::Duration period(0.001); - auto motion = callback(state, period); - if (motion.motion_finished) - { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - zlc::info("FakeFrankaRobot: control loop stopped."); - isOnControlLoop = false; - } - - template - void control(ControllerCallback callback, franka::ControllerMode mode, bool limit_rate = true, - double cutoff_frequency = (100.0)) - { - control(callback, limit_rate, cutoff_frequency); - } - - template - void control(MotionGeneratorCallback motion_callback, TorqueControlCallback torque_callback, - bool limit_rate = true, double cutoff_frequency = (100.0)) - { - zlc::info("FakeFrankaRobot: control loop started."); - isOnControlLoop = true; - while (isOnControlLoop) - { - franka::RobotState state = current_state_; - franka::Duration period(0.001); - auto motion = motion_callback(state, period); - auto torque = torque_callback(state, period); - if (motion.motion_finished || torque.motion_finished) - { - break; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1)); - } - zlc::info("FakeFrankaRobot: control loop stopped."); - isOnControlLoop = false; - } - // Add other necessary fake methods as needed for testing - - franka::RobotState readOnce() - { - if (isOnControlLoop) - { - throw std::runtime_error("Cannot read state while on control loop."); - } - return current_state_; - } - - FakeFrankaModel loadModel() - { - return FakeFrankaModel(); - } - - private: - std::atomic isOnControlLoop = false; - franka::RobotState current_state_; -}; \ No newline at end of file diff --git a/include/debugger/state_debug.hpp b/include/debugger/state_debug.hpp deleted file mode 100644 index 8d2cd0b..0000000 --- a/include/debugger/state_debug.hpp +++ /dev/null @@ -1,11 +0,0 @@ -#ifndef STATE_DEBUG_HPP -#define STATE_DEBUG_HPP -#include -#include - -namespace protocol -{ - void debugPrintFrankaArmStateBuffer(const std::vector& buffer); -} - -#endif // STATE_DEBUG_HPP \ No newline at end of file diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp deleted file mode 100644 index cd26414..0000000 --- a/include/franka_gripper_proxy.hpp +++ /dev/null @@ -1,258 +0,0 @@ -// #pragma once -// #include -// #include -// #include -// #include -// #include -// #include - -// #include -// #include -// #include "control_mode/abstract_control_mode.hpp" -// #include "utils/atomic_double_buffer.hpp" -// #include "utils/franka_config.hpp" - -// #include "protocol/grasp_command.hpp" -// #include - -// #include -// #include -// #include - -// enum class FrankaGripperFlag { -// STOP = 0, -// STOPPING = 1, -// CLOSING = 2, -// OPENING = 3, -// }; - -// class FrankaGripperProxy { - -// public: -// // Constructor & Destructor -// explicit FrankaGripperProxy(const FrankaGripperConfigData& config): -// state_pub_socket_(ZmqContext::instance(), ZMQ_PUB),//gripper state publish socket -// is_running(false), -// is_on_control_mode(false), -// gripper_flag(FrankaGripperFlag::STOP), -// current_state_(AtomicDoubleBuffer(franka::GripperState{})), -// command_(AtomicDoubleBuffer(protocol::GraspCommand{})), -// config_(config) -// { -// gripper_ip_ = config_.gripper_ip; -// state_pub_addr_ = config_.gripper_state_pub_addr; -// // Bind state pub socket -// zlc::info("Gripper state publisher bound to {}", state_pub_addr_); -// state_pub_socket_.bind(state_pub_addr_); -// // Removed service_registry_.bindSocket as ZeroLanCom handles this -// //initialize franka gripper -// # if !LOCAL_TESTING -// gripper_ = std::make_shared(gripper_ip_); -// gripper_->homing(); -// current_state_.write(gripper_->readOnce()); -// command_.write(protocol::GraspCommand{ -// current_state_.read().width, -// static_cast(config_.gripper_default_speed_slow), -// static_cast(config_.gripper_default_force) -// }); -// # else -// current_state_.write(franka::GripperState{0.0f, 0.0f, false, false, franka::Duration{}}); -// command_.write(protocol::GraspCommand{ -// 0.0f, -// static_cast(config_.gripper_default_speed_slow), -// static_cast(config_.gripper_default_force) -// }); -// # endif -// initializeService(); -// }; -// ~FrankaGripperProxy() { -// stop(); -// }; - -// // Core server operations -// void start() { -// is_running = true; -// zlc::info("Gripper proxy running flag set to {}", is_running.load()); -// state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); -// command_.write(protocol::GraspCommand{ -// (float)current_state_.read().width, -// static_cast(config_.gripper_default_speed_fast), -// static_cast(config_.gripper_default_force) -// }); -// state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); -// check_thread_ = std::thread(&FrankaGripperProxy::checkWidthThread, this); -// control_thread_ = std::thread(&FrankaGripperProxy::controlLoopThread, this); -// }; - -// void stop() { -// zlc::info("Stopping FrankaGripperProxy..."); -// is_running = false; -// is_on_control_mode = false; -// if (state_pub_thread_.joinable()) state_pub_thread_.join(); -// if (control_thread_.joinable()) control_thread_.join(); -// if (check_thread_.joinable()) check_thread_.join(); -// if (command_sub_thread_.joinable()) command_sub_thread_.join(); - -// // Removed service_registry_.stop() as ZeroLanCom handles this -// gripper_.reset(); -// zlc::info("FrankaGripperProxy stopped successfully."); -// }; - -// private: -// // Initialization -// void initializeService() { -// // Register service handlers -// node_.registerServiceHandler("START_FRANKA_GRIPPER_CONTROL", &FrankaGripperProxy::startFrankaGripperControl, this); -// node_.registerServiceHandler("GET_FRANKA_GRIPPER_STATE_PUB_PORT", &FrankaGripperProxy::getFrankaGripperStatePubPort, this); -// }; - -// std::string gripper_ip_; -// std::string service_addr_; -// std::string state_pub_addr_; -// // Franka robot -// std::shared_ptr gripper_; -// // ZMQ communication -// zmq::socket_t state_pub_socket_;//arm state publish socket - -// // Threading -// std::thread state_pub_thread_; -// std::thread control_thread_; -// std::thread check_thread_; -// std::thread command_sub_thread_; - -// // Threading Tasks -// void controlLoopThread() { -// while (is_running) { -// float current_width = current_state_.read().width; -// float target_width = command_.read().width; -// if (gripper_flag.load() == FrankaGripperFlag::STOPPING) { -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// continue; -// } -// try -// { -// if (current_width > target_width + config_.gripper_default_close_open_threshold) { -// zlc::info("[Gripper Close] Closing gripper to target width: {}", target_width); -// gripper_flag.store(FrankaGripperFlag::CLOSING); -// gripper_->grasp(target_width, command_.read().speed, 60.0); -// } else if (current_width < target_width - config_.gripper_default_close_open_threshold) { -// zlc::info("[Gripper Open] Opening gripper to target width: {}", target_width); -// gripper_flag.store(FrankaGripperFlag::OPENING); -// gripper_->move(target_width, command_.read().speed); -// } -// } -// catch(const std::exception& e) -// { -// zlc::error("{}", e.what()); -// } -// gripper_flag.store(FrankaGripperFlag::STOP); -// } -// }; - -// void statePubThread() { -// zerolancom::Publisher<> state_pub(ZmqContext::instance());; -// while (is_running) { -// #if !LOCAL_TESTING -// franka::GripperState gs = gripper_->readOnce(); -// current_state_.write(gs); - -// // Convert to FrankaGripperState and serialize with msgpack -// FrankaGripperState gripper_state_msg; -// gripper_state_msg.width = gs.width; -// gripper_state_msg.max_width = gs.max_width; -// gripper_state_msg.is_grasped = gs.is_grasped; -// gripper_state_msg.temperature = gs.temperature; - -// msgpack::sbuffer buffer; -// msgpack::pack(buffer, gripper_state_msg); - -// state_pub_socket_.send(zmq::buffer(buffer.data(), buffer.size()), zmq::send_flags::none); -// #endif -// const int rate = config_.gripper_pub_rate_hz > 0 ? config_.gripper_pub_rate_hz : GRIPPER_PUB_RATE_HZ; -// std::this_thread::sleep_for(std::chrono::milliseconds(1000 / rate)); -// } -// try { -// state_pub_socket_.close(); -// } catch (const zmq::error_t& e) { -// zlc::error("[ZMQ ERROR] {}", e.what()); -// } -// }; - -// void checkWidthThread() { -// while (is_running) { -// #if !LOCAL_TESTING -// float current_width = current_state_.read().width; -// float target_width = command_.read().width; -// if ( -// gripper_flag.load() == FrankaGripperFlag::STOPPING -// || (gripper_flag.load() == FrankaGripperFlag::CLOSING && current_width >= target_width + config_.gripper_default_close_open_threshold) -// || (gripper_flag.load() == FrankaGripperFlag::OPENING && current_width <= target_width - config_.gripper_default_close_open_threshold) -// || (gripper_flag.load() == FrankaGripperFlag::STOP && std::abs(current_width - target_width) <= config_.gripper_default_close_open_threshold) -// ) { -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// continue; -// } -// try -// { -// zlc::info("[Gripper Stop] Stopping gripper at current width: {}", current_width); -// gripper_flag.store(FrankaGripperFlag::STOP); -// gripper_->stop(); -// } -// catch(const std::exception& e) -// { -// zlc::error("{}", e.what()); -// } -// #endif -// std::this_thread::sleep_for(std::chrono::milliseconds(1)); -// } -// }; - -// void commandSubThread(const std::string& command_sub_addr) { -// is_on_control_mode = true; -// zmq::socket_t sub_socket_(ZmqContext::instance(), ZMQ_SUB); -// sub_socket_.set(zmq::sockopt::rcvtimeo, config_.gripper_command_rcvtimeo_ms); // timeout -// sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages -// sub_socket_.connect(command_sub_addr); -// while (is_running && is_on_control_mode) { -// sub_socket_.set(zmq::sockopt::subscribe, ""); // Subscribe to all messages -// zmq::message_t message; -// if (!sub_socket_.recv(message, zmq::recv_flags::none)) { -// continue; -// } -// protocol::ByteView data{ -// static_cast(message.data()), -// message.size() -// }; -// command_.write(protocol::decode(data)); -// std::this_thread::sleep_for(std::chrono::milliseconds(10)); -// } -// }; - -// // Synchronization -// std::atomic is_running; // for threads -// std::atomic is_moving; -// std::atomic is_on_control_mode; -// std::atomic gripper_flag; - -// AtomicDoubleBuffer current_state_; -// AtomicDoubleBuffer command_; -// FrankaGripperConfigData config_; -// zerolancom::ZeroLanComNode& node_; -// void startFrankaGripperControl(const std::string& command_sub_addr) { -// if (is_on_control_mode) { -// zlc::warn("[FrankaGripperProxy] Already in control mode, stopping previous command subscriber."); -// is_on_control_mode = false; -// if (command_sub_thread_.joinable()) command_sub_thread_.join(); -// } -// command_sub_thread_ = std::thread(&FrankaGripperProxy::commandSubThread, this, command_sub_addr); -// }; -// const std::string& getFrankaGripperStatePubPort() { -// return state_pub_addr_; -// }; - -// // 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/motion_generator/cartesian_pose_motion_generator.hpp b/include/motion_generator/cartesian_pose_motion_generator.hpp new file mode 100644 index 0000000..d94d7c6 --- /dev/null +++ b/include/motion_generator/cartesian_pose_motion_generator.hpp @@ -0,0 +1,106 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "utils/atomic_double_buffer.hpp" + +class CartesianPoseMotionGenerator +{ + public: + /** + * Creates a new CartesianPoseMotionGenerator instance for a target pose. + * + * @param[in] speed_factor General speed factor in range [0, 1]. + * @param[in] goal_position Target position [x, y, z] in meters. + * @param[in] goal_orientation Target orientation as quaternion. + * @param[in] state_buffer Buffer to write robot state updates during motion. + * @param[in] tolerance If position error is below this threshold, motion finishes immediately (default: 1e-3 m). + * @param[in] dx_max Maximum translational velocity in m/s (default: 0.3). + * @param[in] ddx_max_start Maximum translational acceleration at start in m/s² (default: 1.0). + * @param[in] ddx_max_goal Maximum translational acceleration at goal in m/s² (default: 1.0). + * @param[in] omega_max Maximum rotational velocity in rad/s (default: 0.5). + * @param[in] domega_max_start Maximum rotational acceleration at start in rad/s² (default: 1.0). + * @param[in] domega_max_goal Maximum rotational acceleration at goal in rad/s² (default: 1.0). + */ + CartesianPoseMotionGenerator(double speed_factor, + const Eigen::Vector3d& goal_position, + const Eigen::Quaterniond& goal_orientation, + AtomicDoubleBuffer& state_buffer, + double tolerance = 1e-3, + double dx_max = 0.3, + double ddx_max_start = 1.0, + double ddx_max_goal = 1.0, + double omega_max = 0.5, + double domega_max_start = 1.0, + double domega_max_goal = 1.0); + + /** + * Sends Cartesian pose calculations + * + * @param[in] robot_state Current state of the robot. + * @param[in] period Duration of execution. + * + * @return Cartesian pose for use inside a control loop. + */ + franka::CartesianPose operator()(const franka::RobotState& robot_state, franka::Duration period); + + private: + using Vector3d = Eigen::Vector3d; + + bool calculateDesiredValues(double time, Vector3d* delta_pos_d, double* alpha) const; + void calculateSynchronizedValues(); + std::array poseToMatrix(const Vector3d& position, const Eigen::Quaterniond& orientation) const; + + static constexpr double kDeltaPosMotionFinished = 1e-6; // meters + static constexpr double kDeltaRotMotionFinished = 1e-6; // radians + + // Goal pose + Vector3d pos_goal_; + Eigen::Quaterniond q_goal_; + + // Start pose + Vector3d pos_start_; + Eigen::Quaterniond q_start_; + + // Position delta + Vector3d delta_pos_; + + // Rotation angle (total angle to rotate) + double delta_rot_; + + // Position timing (synchronized for x, y, z) + Vector3d dx_max_sync_; + Vector3d t_1_sync_pos_; + Vector3d t_2_sync_pos_; + Vector3d t_f_sync_pos_; + Vector3d pos_1_; // position at end of acceleration phase + + // Rotation timing + double omega_max_sync_; + double t_1_sync_rot_; + double t_2_sync_rot_; + double t_f_sync_rot_; + double rot_1_; // rotation at end of acceleration phase + + // Global synchronized finish time + double t_f_sync_; + + double time_ = 0.0; + + // Translational velocity/acceleration limits + Vector3d dx_max_; + Vector3d ddx_max_start_; + Vector3d ddx_max_goal_; + + // Rotational velocity/acceleration limits + double omega_max_; + double domega_max_start_; + double domega_max_goal_; + + AtomicDoubleBuffer* state_buffer_; + double tolerance_; +}; diff --git a/include/motion_generator/joint_position_motion_generator.hpp b/include/motion_generator/joint_position_motion_generator.hpp new file mode 100644 index 0000000..a83e88e --- /dev/null +++ b/include/motion_generator/joint_position_motion_generator.hpp @@ -0,0 +1,63 @@ +#pragma once + +#include +#include +#include +#include +#include + +#include "utils/atomic_double_buffer.hpp" + +class JointPositionMotionGenerator +{ + public: + /** + * Creates a new JointPositionMotionGenerator instance for a target q. + * + * @param[in] speed_factor General speed factor in range [0, 1]. + * @param[in] q_goal Target joint positions. + * @param[in] state_buffer Buffer to write robot state updates during motion. + * @param[in] tolerance If max joint error is below this threshold, motion finishes immediately (default: 1e-3 rad). + */ + JointPositionMotionGenerator(double speed_factor, const std::array& q_goal, + AtomicDoubleBuffer& state_buffer, + double tolerance = 1e-3); + + /** + * Sends joint position calculations + * + * @param[in] robot_state Current state of the robot. + * @param[in] period Duration of execution. + * + * @return Joint positions for use inside a control loop. + */ + franka::JointPositions operator()(const franka::RobotState& robot_state, franka::Duration period); + + private: + using Vector7d = Eigen::Matrix; + using Vector7i = Eigen::Matrix; + + bool calculateDesiredValues(double time, Vector7d* delta_q_d) const; + void calculateSynchronizedValues(); + + static constexpr double kDeltaQMotionFinished = 1e-6; + Vector7d q_goal_; + + Vector7d q_start_; + Vector7d delta_q_; + + Vector7d dq_max_sync_; + Vector7d t_1_sync_; + Vector7d t_2_sync_; + Vector7d t_f_sync_; + Vector7d q_1_; + + double time_ = 0.0; + + Vector7d dq_max_ = (Vector7d() << 2.0, 2.0, 2.0, 2.0, 2.5, 2.5, 2.5).finished(); + Vector7d ddq_max_start_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); + Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); + + AtomicDoubleBuffer* state_buffer_; + double tolerance_; +}; diff --git a/include/mujoco_sim/mujoco_robot.hpp b/include/mujoco_sim/mujoco_robot.hpp index 0fb30b4..1f75de2 100644 --- a/include/mujoco_sim/mujoco_robot.hpp +++ b/include/mujoco_sim/mujoco_robot.hpp @@ -8,6 +8,7 @@ #include #include +#include #include #include #include @@ -16,6 +17,7 @@ #include "mujoco_sim/mujoco_panda_env.hpp" #include "mujoco_sim/mujoco_viewer.hpp" +#include "utils/robot_model.hpp" class MujocoModel { @@ -51,6 +53,22 @@ class MujocoRobot control_callback, bool limit_rate = true, double cutoff_frequency = franka::kMaxCutoffFrequency); + void control(std::function + motion_generator_callback, + bool limit_rate = true, double cutoff_frequency = franka::kMaxCutoffFrequency); + + void control(std::function + motion_generator_callback, + bool limit_rate = true, double cutoff_frequency = franka::kMaxCutoffFrequency); + + void control(std::function + motion_generator_callback, + bool limit_rate = true, double cutoff_frequency = franka::kMaxCutoffFrequency); + + void control(std::function + motion_generator_callback, + bool limit_rate = true, double cutoff_frequency = franka::kMaxCutoffFrequency); + void read(std::function read_callback); franka::RobotState readOnce(); @@ -79,10 +97,30 @@ class MujocoRobot MujocoRobot& operator=(const MujocoRobot&) = delete; private: + // Control conversion helpers + franka::Torques jointPositionToTorque(const franka::JointPositions& desired_positions, + const franka::RobotState& state); + franka::Torques jointVelocityToTorque(const franka::JointVelocities& desired_velocities, + const franka::RobotState& state); + std::array cartesianPoseToJointPosition(const franka::CartesianPose& desired_pose, + const franka::RobotState& state); + std::array cartesianVelocityToJointVelocity(const franka::CartesianVelocities& desired_velocities, + const franka::RobotState& state); + + // Default PD gains (libfranka defaults) + static constexpr std::array kDefaultStiffness = {600.0, 600.0, 600.0, 600.0, 250.0, 150.0, 50.0}; + static constexpr std::array kDefaultDamping = {50.0, 50.0, 50.0, 50.0, 30.0, 25.0, 15.0}; + + // IK parameters + static constexpr double kIKErrorThreshold = 1e-4; + static constexpr int kIKMaxIterations = 100; + static constexpr double kIKStepSize = 0.5; + bool running_{false}; std::unique_ptr env_; std::unique_ptr viewer_; + std::unique_ptr model_; mutable std::mutex state_mutex_; std::mutex control_mutex_; diff --git a/include/protocol/codec.hpp b/include/protocol/codec.hpp deleted file mode 100644 index 685117d..0000000 --- a/include/protocol/codec.hpp +++ /dev/null @@ -1,184 +0,0 @@ -// #pragma once -// -// #include -// #include -// #include -// #include -// #include -// #include -// #include "protocol/grasp_command.hpp" -// #include - -// namespace protocol { - -// class RequestResult; // forward declaration? - -// struct ByteView { -// const uint8_t* data; -// size_t size; - -// const uint8_t* begin() const { return data; } -// const uint8_t* end() const { return data + size; } -// }; - -// // encode std::array (fixed size) -// template -// inline void encode_array_f64(uint8_t* ptr, const std::array& in) { -// for (size_t i = 0; i < N; ++i) { -// double be_val = to_big_endian_f64(in[i]); -// std::memcpy(ptr, &be_val, sizeof(double)); -// ptr += sizeof(double); -// } -// } - -// // encoede std::vector (dynamic size) -// inline void encode_array_f64(uint8_t* ptr, const std::vector& in) { -// for (const auto& val : in) { -// double be_val = to_big_endian_f64(val); -// std::memcpy(ptr, &be_val, sizeof(double)); -// ptr += sizeof(double); -// } -// } - -// // decode std::array (fixed size) -// template -// inline void decode_array_f64(const uint8_t* ptr, std::array& out) { -// for (size_t i = 0; i < N; ++i) { -// double raw; -// std::memcpy(&raw, ptr, sizeof(double)); -// out[i] = from_big_endian_f64(raw); -// ptr += sizeof(double); -// } -// } - -// // decode std::vector（dynamic size） -// inline void decode_array_f64(const uint8_t* ptr, std::vector& out, size_t count) { -// out.resize(count); -// for (size_t i = 0; i < count; ++i) { -// double raw; -// std::memcpy(&raw, ptr, sizeof(double)); -// out[i] = from_big_endian_f64(raw); -// ptr += sizeof(double); -// } -// } - -// // encode uint32_t -// inline void encode_u32(uint8_t* ptr, uint32_t val) { -// uint32_t be_val = to_big_endian_u32(val); -// std::memcpy(ptr, &be_val, sizeof(be_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; -// std::memcpy(&raw, ptr, sizeof(raw)); -// ptr += sizeof(raw); -// 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 -// } - -// // encode overloads by argument type (valid C++ overloading) -// std::vector encode(const std::string& v); -// std::vector encode(uint8_t v); -// std::vector encode(uint16_t v); -// std::vector encode(const franka::RobotState& rs); -// std::vector encode(const franka::GripperState& gs); -// //RequestResult has it own enocdeMessage function, due to flag need to indicate presence of detail string - -// // ============================================================================ -// // template T decode(ByteView payload); -// // ============================================================================ -// template -// T decode(ByteView payload); - -// // ============================================================================ -// // std::string decode -// // ============================================================================ -// template <> -// std::string decode(ByteView payload); - -// // ============================================================================ -// // uint16_t decode -// // ============================================================================ -// template <> -// uint16_t decode(ByteView payload); - -// // ============================================================================ -// // franka control objects -// // ============================================================================ - -// template <> -// franka::JointPositions decode(ByteView payload); - -// template <> -// franka::JointVelocities decode(ByteView payload); - -// template <> -// franka::CartesianPose decode(ByteView payload); - -// template <> -// franka::CartesianVelocities decode(ByteView payload); - -// template <> -// franka::Torques decode(ByteView payload); - -// // ============================================================================ -// // protocol::FrankaArmControlMode -// // ============================================================================ - -// template <> -// protocol::FrankaArmControlMode decode(ByteView payload); - -// template <> -// protocol::GraspCommand decode(ByteView payload); - -// // ...existing code... -// template <> -// franka::RobotState decode(ByteView payload); -// // ...existing code... - -// } // namespace protocol \ No newline at end of file diff --git a/include/protocol/grasp_command.hpp b/include/protocol/grasp_command.hpp deleted file mode 100644 index a401d19..0000000 --- a/include/protocol/grasp_command.hpp +++ /dev/null @@ -1,16 +0,0 @@ -#pragma once -#include -#include - -namespace protocol -{ - struct GraspCommand - { - double width; // Desired width of the gripper in meters - double speed; // Speed at which to close the gripper in meters per second - double force; // Force to apply when closing the gripper in Newtons - - GraspCommand() : width(0.0f), speed(0.0f), force(0.0f) {} - GraspCommand(float w, float s, float f) : width(w), speed(s), force(f) {} - }; -} // namespace protocol \ No newline at end of file diff --git a/include/protocol/msg.hpp b/include/protocol/msg.hpp index 359bd99..0c571ec 100644 --- a/include/protocol/msg.hpp +++ b/include/protocol/msg.hpp @@ -1,4 +1,5 @@ -// #include +#pragma once + #include #include @@ -20,15 +21,20 @@ struct FrankaArmState O_F_ext_hat_K, K_F_ext_hat_K); FrankaArmState(const franka::RobotState& state) - : q(state.q.begin(), state.q.end()), + : time_ms(0), + O_T_EE(state.O_T_EE.begin(), state.O_T_EE.end()), + O_T_EE_d(state.O_T_EE_d.begin(), state.O_T_EE_d.end()), + q(state.q.begin(), state.q.end()), q_d(state.q_d.begin(), state.q_d.end()), dq(state.dq.begin(), state.dq.end()), dq_d(state.dq_d.begin(), state.dq_d.end()), tau_ext_hat_filtered(state.tau_ext_hat_filtered.begin(), - state.tau_ext_hat_filtered.end()){}; + state.tau_ext_hat_filtered.end()), + O_F_ext_hat_K(state.O_F_ext_hat_K.begin(), state.O_F_ext_hat_K.end()), + K_F_ext_hat_K(state.K_F_ext_hat_K.begin(), state.K_F_ext_hat_K.end()){}; }; -struct FrankaGripperState +struct PandaGripperState { double width; double max_width; @@ -57,4 +63,4 @@ struct FrankaResponseMsg std::string code; std::vector payload; MSGPACK_DEFINE_MAP(code, payload); -}; +}; \ No newline at end of file diff --git a/include/franka_arm_proxy.hpp b/include/robots/panda_arm.hpp similarity index 94% rename from include/franka_arm_proxy.hpp rename to include/robots/panda_arm.hpp index 153d3bf..d120a73 100644 --- a/include/franka_arm_proxy.hpp +++ b/include/robots/panda_arm.hpp @@ -11,6 +11,7 @@ #include "control_mode/control_mode.hpp" #include "protocol/msg.hpp" +#include "protocol/request_result.hpp" #include "utils/atomic_double_buffer.hpp" #include "utils/config_file_reader.hpp" #include "utils/robot_model.hpp" @@ -79,14 +80,14 @@ struct FrankaArmConfig } }; -class FrankaArmProxy +class PandaArm { public: // Constructor & Destructor - explicit FrankaArmProxy( + explicit PandaArm( const std::string& config_path); // Constructor that initializes the proxy with a configuration file - ~FrankaArmProxy(); // Destructor to clean up resources + ~PandaArm(); // Destructor to clean up resources // Core server operations void stop(); // Stops the server, cleaning up resources and shutting down communication @@ -129,6 +130,8 @@ class FrankaArmProxy // Service callbacks FrankaArmState getFrankaArmState(const zlc::Empty&); // Gets the current state of the Franka arm std::string getFrankaArmControlMode(const zlc::Empty&); + std::pair> moveFrankaArmToJointPosition( + const std::vector& target_q); void statePublishThread(); -}; +}; \ No newline at end of file diff --git a/include/robots/panda_gripper.hpp b/include/robots/panda_gripper.hpp new file mode 100644 index 0000000..5208e93 --- /dev/null +++ b/include/robots/panda_gripper.hpp @@ -0,0 +1,90 @@ +#pragma once +#include +#include +#include +#include +#include + +#include +#include +#include +#include "control_mode/abstract_control_mode.hpp" +#include "utils/atomic_double_buffer.hpp" + +struct GripperStateMsg { + double width; + double max_width; + bool is_grasped; + uint16_t temperature; + uint64_t time; + + MSGPACK_DEFINE_MAP(width, max_width, is_grasped, temperature, time) +}; + +struct GraspCommand +{ + double width; // Desired width of the gripper in meters + double speed; // Speed at which to close the gripper in meters per second + + GraspCommand() : width(0.0f), speed(0.0f) {} + GraspCommand(float w, float s) : width(w), speed(s) {} + + MSGPACK_DEFINE_MAP(width, speed) +}; + +struct PandaGripperConfig +{ + // communication + std::string name; + std::string gripper_ip; + + std::string command_topic; + std::string state_topic; + bool enable_control{true}; + + PandaGripperConfig(const std::string& gripper_config_path) + { + fromFile(gripper_config_path); + } + + void fromFile(const std::string& gripper_config_path) + { + ConfigFileReader reader(gripper_config_path); + name = reader.getValue("name"); + gripper_ip = reader.getValue("gripper_ip"); + command_topic = reader.getValue("command_topic"); + state_topic = reader.getValue("state_topic"); + enable_control = reader.getValue("enable_control", true); + } +}; + +class PandaGripper { +public: + // Constructor & Destructor + explicit PandaGripper(const std::string& config_path); + ~PandaGripper(); + + void stop(); + +private: + // Franka gripper + std::shared_ptr gripper_; + + // Threading + std::thread state_pub_thread_; + std::thread control_thread_; + + // Threading Tasks + void controlLoopThread(); + void statePubThread(); + void updateCommand(const GraspCommand& cmd); + + // Synchronization + std::atomic is_running_; + + AtomicDoubleBuffer command_; + + PandaGripperConfig config_; + + static constexpr int GRIPPER_PUB_RATE_HZ = 100; +}; diff --git a/include/robots/robotiq_gripper.hpp b/include/robots/robotiq_gripper.hpp new file mode 100644 index 0000000..ab47ba0 --- /dev/null +++ b/include/robots/robotiq_gripper.hpp @@ -0,0 +1,105 @@ +#pragma once +#include +#include +#include +#include +#include +#include + +#include +#include "control_mode/abstract_control_mode.hpp" +#include "utils/atomic_double_buffer.hpp" +#include "robotiq/robotiq_gripper_interface.h" + +struct RobotiqGripperStateMsg { + float commanded_position; + float commanded_speed; + float commanded_force; + float position; + float current; // how powerful the gripper is closing + uint8_t raw_commanded_position; + uint8_t raw_position; + + MSGPACK_DEFINE_MAP(commanded_position, commanded_speed, commanded_force, position, current, + raw_commanded_position, raw_position); +}; + +struct RobotiqGraspCommand +{ + float position; // Desired position, scaled by config scale_alpha/scale_beta + float speed; // Desired speed, scaled by config scale_alpha/scale_beta + float force; // Desired force, scaled by config scale_alpha/scale_beta + bool blocking; // Wait for completion if true + + RobotiqGraspCommand() : position(0.0f), speed(0.1f), force(0.1f), blocking(false) {} + RobotiqGraspCommand(float p, float s, float f, bool b = false) + : position(p), speed(s), force(f), blocking(b) {} + + MSGPACK_DEFINE_MAP(position, speed, force, blocking) +}; + +struct RobotiqGripperConfig +{ + // communication + std::string name; + std::string port; + std::size_t baud{robotiq::DEFAULT_BAUD}; + + std::string command_topic; + std::string state_topic; + double scale_alpha{robotiq::DEFAULT_SCALE_ALPHA}; + double scale_beta{robotiq::DEFAULT_SCALE_BETA}; + std::size_t receive_timeout_ms{robotiq::DEFAULT_RECEIVE_TIMEOUT_MS}; + int gripper_pub_rate_hz{100}; + + RobotiqGripperConfig(const std::string& gripper_config_path) + { + fromFile(gripper_config_path); + } + + void fromFile(const std::string& gripper_config_path) + { + ConfigFileReader reader(gripper_config_path); + name = reader.getValue("name", "RobotiqGripper"); + port = reader.getValue("gripper_port", robotiq::DEFAULT_PORT); + baud = reader.getValue("gripper_baud", robotiq::DEFAULT_BAUD); + command_topic = reader.getValue("command_topic", "robotiq_gripper_command"); + state_topic = reader.getValue("state_topic", "robotiq_gripper_state"); + scale_alpha = reader.getValue("scale_alpha", robotiq::DEFAULT_SCALE_ALPHA); + scale_beta = reader.getValue("scale_beta", robotiq::DEFAULT_SCALE_BETA); + receive_timeout_ms = + reader.getValue("receive_timeout_ms", robotiq::DEFAULT_RECEIVE_TIMEOUT_MS); + gripper_pub_rate_hz = reader.getValue("gripper_pub_rate_hz", 100); + } +}; + +class RobotiqGripper { +public: + // Constructor & Destructor + explicit RobotiqGripper(const std::string& config_path); + ~RobotiqGripper(); + + void stop(); + +private: + bool commandChanged(const RobotiqGraspCommand& a, const RobotiqGraspCommand& b) const; + + // Robotiq gripper + std::shared_ptr gripper_; + + // Threading + std::thread state_pub_thread_; + std::thread control_thread_; + + // Threading Tasks + void controlLoopThread(); + void statePubThread(); + void updateCommand(const RobotiqGraspCommand& cmd); + + // Synchronization + std::atomic is_running_; + + AtomicDoubleBuffer command_; + + RobotiqGripperConfig config_; +}; diff --git a/include/utils/config_file_reader.hpp b/include/utils/config_file_reader.hpp index 7772aee..0bbb523 100644 --- a/include/utils/config_file_reader.hpp +++ b/include/utils/config_file_reader.hpp @@ -17,6 +17,11 @@ class ConfigFileReader ConfigFileReader(const std::string& arm_config_path); + YAML::Node getSubNode(const std::string& key) const + { + return node[key]; + } + template T getValue(const std::string& key) const { diff --git a/include/utils/robot_utils.hpp b/include/utils/robot_utils.hpp index b95ca6d..1a8bd33 100644 --- a/include/utils/robot_utils.hpp +++ b/include/utils/robot_utils.hpp @@ -1,3 +1,5 @@ +#pragma once + #include #include #include diff --git a/local_test.bash b/local_test.bash old mode 100755 new mode 100644 index 64fe9a7..3daa2c1 --- a/local_test.bash +++ b/local_test.bash @@ -1,4 +1,6 @@ +mkdir build cd build cmake -DLOCAL_TESTING=ON .. make -j8 -./proxy ../local_test.yaml \ No newline at end of file +cd .. +./build/proxy ./config/ProxyConfig.yaml \ No newline at end of file diff --git a/models/franka_emika_panda/panda_robotiq_85.urdf b/models/franka_emika_panda/panda_robotiq_85.urdf new file mode 100644 index 0000000..c8db7cf --- /dev/null +++ b/models/franka_emika_panda/panda_robotiq_85.urdf @@ -0,0 +1,524 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + diff --git a/real_robot_test.bash b/real_robot_test.bash new file mode 100644 index 0000000..95dcde9 --- /dev/null +++ b/real_robot_test.bash @@ -0,0 +1,6 @@ +mkdir build +cd build +cmake -DLOCAL_TESTING=OFF .. +make -j8 +cd .. +./build/proxy ./config/proxy/droid.yaml \ No newline at end of file diff --git a/scripts/compile.sh b/scripts/compile.sh old mode 100755 new mode 100644 index 6c44187..ce990d7 --- a/scripts/compile.sh +++ b/scripts/compile.sh @@ -1,5 +1,6 @@ cd build rm -rf ./* +echo "Compiling ZeroLanCom..." echo ${PWD}/../../ZeroLanCom/build -cmake .. +cmake .. -DCMAKE_PREFIX_PATH=${PWD}/../../ZeroLanCom/build make -j$(nproc) \ No newline at end of file diff --git a/scripts/format.sh b/scripts/format.sh old mode 100755 new mode 100644 diff --git a/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index a685ec6..e5d02e1 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -1,7 +1,16 @@ #include "control_mode/abstract_control_mode.hpp" +#include "motion_generator/cartesian_pose_motion_generator.hpp" +#include "motion_generator/joint_position_motion_generator.hpp" +#include +#include +#include #include +#include +#include +#include + void ControllerConfig::readBaseConfig(const ConfigFileReader& reader) { controller_name = reader.getValue("name"); @@ -57,7 +66,7 @@ void AbstractControlMode::stopControl() zlc::info("[{}] Stopping control thread...", getModeName()); control_thread_.join(); } - zlc::info("[{}] Stopped.", getModeName()); + zlc::info("{} Mode Stopped.", getModeName()); } const std::string AbstractControlMode::getModeName() @@ -79,18 +88,92 @@ void AbstractControlMode::controlTask() } catch (const std::exception& ex) { - zlc::error("[CartesianVelocityMode] Robot is unable to be controlled: {}", ex.what()); - break; + zlc::error("[{}] Robot is unable to be controlled: {}", getModeName(), ex.what()); } bool recovered = tryRecovery(); if (!recovered) { - zlc::error("[CartesianVelocityMode] Unable to recover robot. Exiting control loop."); + zlc::error("[{}] Unable to recover robot. Exiting control loop.", getModeName()); break; } } zlc::info("[{}] Control thread ended.", getModeName()); } +bool AbstractControlMode::moveToJointPosition(const std::array& target_q, + double max_velocity, double tolerance) +{ + stopControl(); + zlc::info("[{}] Moving to joint position...", getModeName()); + if (!robot_) + { + zlc::error("[{}] moveToJointPosition failed: robot not initialized.", getModeName()); + return false; + } + if (is_running_) + { + zlc::warn("[{}] moveToJointPosition rejected: control thread is running.", getModeName()); + return false; + } + // robot_->setCollisionBehavior( + // {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, + // {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + // {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + for (size_t i = 0; i < 5; i++) + { + try { + JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_, tolerance); + robot_->control(motion_generator); + }catch (const franka::Exception& e) { + zlc::error("Error when move joint position {}", e.what()); + } + bool recovered = tryRecovery(); + if (!recovered) + { + zlc::error("[{}] Unable to recover robot. Exiting control loop.", getModeName()); + break; + } + } + zlc::info("[{}] Reached target joint position.", getModeName()); + startControl(); + return true; +} + +bool AbstractControlMode::moveToCartesianPose(const Eigen::Vector3d& target_position, + const Eigen::Quaterniond& target_orientation, + double max_velocity, double tolerance) +{ +#if NO_ROBOT_TESTING + zlc::error("[{}] moveToCartesianPose is not supported in NO_ROBOT_TESTING mode.", + getModeName()); + return false; +#else + if (!robot_) + { + zlc::error("[{}] moveToCartesianPose failed: robot not initialized.", getModeName()); + return false; + } + if (is_running_) + { + zlc::warn("[{}] moveToCartesianPose rejected: control thread is running.", getModeName()); + return false; + } + try { + robot_->setCollisionBehavior( + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, + {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, {{20.0, 20.0, 20.0, 20.0, 20.0, 20.0}}, + {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}, {{10.0, 10.0, 10.0, 10.0, 10.0, 10.0}}); + CartesianPoseMotionGenerator motion_generator(max_velocity, target_position, target_orientation, *state_buffer_, tolerance); + robot_->control(motion_generator); + } catch (const franka::Exception& e) { + std::cout << e.what() << std::endl; + return false; + } + return true; +#endif +} + bool AbstractControlMode::tryRecovery(int max_attempts) { diff --git a/src/control_mode/gravity_comp_control.cpp b/src/control_mode/gravity_comp_control.cpp new file mode 100644 index 0000000..6352cc0 --- /dev/null +++ b/src/control_mode/gravity_comp_control.cpp @@ -0,0 +1,51 @@ +#include "control_mode/gravity_comp_control.hpp" + +#include + +void GravityCompControl::reset() +{ + hold_initialized_ = false; +} + +void GravityCompControl::initController(FrankaPanda& robot, PandaPinocchioModel& pinocchio_model, + AtomicDoubleBuffer& state_buffer) +{ + AbstractControlMode::initController(robot, pinocchio_model, state_buffer); + controller_name = "GravityComp"; + reset(); + zlc::info("[GravityComp] Initialized (spring + damping, no gravity compensation)."); +} + +franka::Torques GravityCompControl::controlLoop(const franka::RobotState& robot_state, + franka::Duration /*duration*/) +{ + // Keep publishing state for others (e.g. follower) even when leader is hand-guided. + state_buffer_->write(robot_state); + + // Map robot state to Eigen vectors + JointPosition q = Eigen::Map(robot_state.q.data()); + JointVelocity dq = Eigen::Map(robot_state.dq.data()); + + if (!hold_initialized_) + { + hold_q_ = q; + hold_initialized_ = true; + } + + // Simple spring + damping (no gravity compensation). + // Tune these two values to change hand-guiding feel. + const std::array k_hold = {0.08, 0.08, 0.08, 0.04, 0.04, 0.08, 0.08}; + const std::array damping = {0.5, 0.5, 0.5, 0.4, 0.3, 0.5, 0.5}; + + std::array tau_cmd{}; + for (int i = 0; i < 7; i++) + { + const double tau_hold = k_hold[i] * (hold_q_[i] - q[i]); + const double tau = tau_hold - damping[i] * dq[i]; + tau_cmd[i] = tau; + } + + franka::Torques out{tau_cmd}; + if (!is_running_) out.motion_finished = true; + return out; +} diff --git a/src/control_mode/hybrid_joint_impedance_control.cpp b/src/control_mode/hybrid_joint_impedance_control.cpp index 5c5e944..8ea1f7e 100644 --- a/src/control_mode/hybrid_joint_impedance_control.cpp +++ b/src/control_mode/hybrid_joint_impedance_control.cpp @@ -13,6 +13,9 @@ void HybridJointImpedanceControl::initController( controller_name = config_.controller_name; std::array current_pos = state_buffer.read().q; desired_positions_.write(JointPosition::Map(current_pos.data())); + auto pose = desired_positions_.read(); + std::cout << pose[0] << "," << pose[1] << "," << pose[2] << "," << pose[3] << "," + << pose[4] << "," << pose[5] << "," << pose[6] << std::endl; zlc::registerSubscriberHandler(config_.command_topic, &HybridJointImpedanceControl::writeCommand, this); } @@ -42,6 +45,8 @@ franka::Torques HybridJointImpedanceControl::controlLoop(const franka::RobotStat JointTorque torque_forward = pinocchio_model_->inverseDynamics(current_pos, current_vel, JointAcceleration::Zero()); + torque_forward -= pinocchio_model_->inverseDynamics( + current_pos, JointVelocity::Zero(), JointAcceleration::Zero()); // remove gravity compensation if needed std::array tau_cmd{}; for (size_t i = 0; i < 7; i++) { diff --git a/src/control_mode/idle_control_mode.cpp b/src/control_mode/idle_control_mode.cpp index 17d09a8..a2702e9 100644 --- a/src/control_mode/idle_control_mode.cpp +++ b/src/control_mode/idle_control_mode.cpp @@ -10,6 +10,17 @@ void IdleControlMode::initController(FrankaPanda& robot, PandaPinocchioModel& mo controller_name = "Idle"; } +void IdleControlMode::controlTask() +{ + while (is_running_) + { + auto robot_state = robot_->readOnce(); + state_buffer_->write(robot_state); + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + zlc::info("[{}] Control thread ended.", getModeName()); +} + franka::Torques IdleControlMode::controlLoop(const franka::RobotState& robot_state, franka::Duration /*duration*/) { diff --git a/src/debugger/state_debug.cpp b/src/debugger/state_debug.cpp deleted file mode 100644 index d2440c1..0000000 --- a/src/debugger/state_debug.cpp +++ /dev/null @@ -1,46 +0,0 @@ -// #include -// #include -// #include "protocol/msg_header.hpp" -// #include "debugger/state_debug.hpp" - -// namespace protocol{ - -// void debugPrintFrankaArmStateBuffer(const std::vector& buffer) { -// if (buffer.size() != 652) { -// std::cerr << "Buffer size incorrect: expected " << protocol::FrankaArmState::kSize -// << ", got " << buffer.size() << std::endl; -// return; -// } -// // Step 1: Decode header -// MsgHeader header = MsgHeader::decode(buffer.data()); -// std::cout << "\n[DEBUG] MessageHeader:\n"; -// std::cout << " ID = " << static_cast(header.id) << "\n"; -// std::cout << " len = " << header.len << "\n"; -// 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, 636); -// std::cout << std::fixed << std::setprecision(4); -// std::cout << "\n[DEBUG] FrankaArmState:\n"; -// std::cout << " timestamp_ms = " << decoded.timestamp_ms << "\n"; -// auto print_array = [](const std::string& label, const auto& arr) { -// std::cout << " " << label << " = ["; -// for (size_t i = 0; i < arr.size(); ++i) { -// std::cout << arr[i]; -// if (i < arr.size() - 1) std::cout << ", "; -// } -// std::cout << "]\n"; -// }; -// print_array("q", decoded.q); -// print_array("q_d", decoded.q_d); -// print_array("dq", decoded.dq); -// print_array("dq_d", decoded.dq_d); -// print_array("tau_ext_hat_filtered", decoded.tau_ext_hat_filtered); -// print_array("O_T_EE", decoded.O_T_EE); -// print_array("O_T_EE_d", decoded.O_T_EE_d); -// print_array("O_F_ext_hat_K", decoded.O_F_ext_hat_K); -// print_array("K_F_ext_hat_K", decoded.K_F_ext_hat_K); -// } -// } diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp deleted file mode 100644 index da4c88d..0000000 --- a/src/franka_arm_proxy.cpp +++ /dev/null @@ -1,157 +0,0 @@ - -#include "franka_arm_proxy.hpp" - -static std::atomic running_flag{true}; // let ctrl-c stop the server -static void signalHandler(int signum) -{ - zlc::info("Caught signal {}, shutting down...", signum); - running_flag = false; -} -// namespace { -franka::RobotState createDefaultState(const FrankaArmConfig& cfg) -{ - franka::RobotState state{}; - std::array q{}; - const auto& q_src = cfg.arm_default_state_q.size() == 7 - ? cfg.arm_default_state_q - : std::array{{0.0, -0.785, 0.0, -2.356, 0.0, 1.571, 0.785}}; - std::copy(q_src.begin(), q_src.begin() + 7, q.begin()); - state.q = q; - - std::array pose = cfg.arm_default_state_O_T_EE; - if (pose == std::array{}) - { - pose = {{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}}; - } - state.O_T_EE = pose; - return state; -} -// } - -FrankaArmProxy::FrankaArmProxy(const std::string& config_path) - : is_running(false), - config_(config_path), - current_state(AtomicDoubleBuffer(createDefaultState(config_))) -{ - // Register service handlers - initRobot(); - safety_config_.fromFile("./config/SafetyLimitConfig.cfg"); - ControlModeFactory::registerControlModes(control_modes_, *robot_, *model_, current_state, - safety_config_); - setControlMode("Idle"); - initializeService(); -} - -void FrankaArmProxy::initializeService() -{ - std::string service_namespace = fmt::format("{}/set_franka_arm_control_mode", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::setControlMode, this); - service_namespace = fmt::format("{}/get_franka_arm_state", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::getFrankaArmState, this); - service_namespace = fmt::format("{}/get_franka_arm_control_mode", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::getFrankaArmControlMode, this); - // zlc::registerServiceHandler("MOVE_FRANKA_ARM_TO_JOINT_POSITION", &FrankaArmProxy::moveFrankaArmToJointPosition, this); - // zlc::registerServiceHandler("MOVE_FRANKA_ARM_TO_CARTESIAN_POSITION", &FrankaArmProxy::moveFrankaArmToCartesianPosition, this); -} - -FrankaArmProxy::~FrankaArmProxy() -{ - stop(); -} - -void FrankaArmProxy::initRobot() -{ - //initialize franka robot - try - { - robot_ = std::make_unique(config_.robot_ip); - model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", - "panda_link8"); - } - catch (const franka::NetworkException& e) - { - zlc::error("{}", e.what()); - this->stop(); - } - current_state.write(robot_->readOnce()); - state_pub_thread = std::thread(&FrankaArmProxy::statePublishThread, this); - zlc::info("FrankaArmProxy started successfully."); - is_running = true; -} - -void FrankaArmProxy::stop() -{ - zlc::info("Stopping FrankaArmProxy..."); - is_running = false; - if (current_control_mode_) - current_control_mode_->stopControl(); - if (state_pub_thread.joinable()) - state_pub_thread.join(); - // wait for closing - robot_.reset(); - model_.reset(); - current_control_mode_ = nullptr; // reset current control mode - zlc::info("FrankaArmProxy stopped successfully."); -} - -// Main loop for processing requests, ctrl-c to stop the server -void FrankaArmProxy::spin() -{ - std::signal(SIGINT, signalHandler); // Catch Ctrl+C to stop the server - zlc::info("Entering main spin loop. Press Ctrl+C to exit."); - zlc::info("Current running flag: {}", running_flag.load()); - while (running_flag) - { - std::this_thread::sleep_for(std::chrono::milliseconds(100)); - } - stop(); - zlc::info("Shutdown complete."); -} - -// publish threads -void FrankaArmProxy::statePublishThread() -{ - const std::string topic_name = fmt::format("{}/franka_arm_state", config_.name); - zlc::Publisher state_pub(topic_name); - int dt = int(1000.0 / config_.arm_state_pub_rate_hz); - while (is_running) - { - const franka::RobotState rs = current_state.read(); - FrankaArmState frs(rs); - state_pub.publish(frs); - std::this_thread::sleep_for(std::chrono::milliseconds(dt)); - } -} - -zlc::Empty FrankaArmProxy::setControlMode(const std::string& mode) -{ - if (control_modes_.find(mode) == control_modes_.end()) - { - zlc::warn("Control mode '{}' not found!", mode); - return zlc::empty; - } - if (current_control_mode_ != nullptr) - { - zlc::info("Stopping previous control mode..."); - current_control_mode_->stopControl(); - } - zlc::info("Switching to control mode: {}", mode); - current_control_mode_ = control_modes_.at(mode).get(); - current_control_mode_->startControl(); - return zlc::empty; -} - -FrankaArmState FrankaArmProxy::getFrankaArmState(const zlc::Empty&) -{ - return FrankaArmState(current_state.read()); -} - -std::string FrankaArmProxy::getFrankaArmControlMode(const zlc::Empty&) -{ - if (!current_control_mode_) - { - zlc::warn("No active control mode"); - return "None"; - } - return current_control_mode_->getModeName(); -} diff --git a/src/main.cpp b/src/main.cpp index b7d7151..8702446 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,7 +3,10 @@ #include #include -#include "franka_arm_proxy.hpp" +#include "robots/panda_arm.hpp" +#include "robots/panda_gripper.hpp" +#include "robots/robotiq_gripper.hpp" + int main(int argc, char** argv) { @@ -19,20 +22,68 @@ int main(int argc, char** argv) ConfigFileReader proxy_reader(proxy_config_path); std::string node_name = proxy_reader.getValue("node_name"); std::string proxy_ip = proxy_reader.getValue("proxy_ip"); - zlc::info("Starting Franka Control Proxy with node name: {}", node_name); - zlc::info("Using proxy IP address: {}", proxy_ip); - zlc::init(node_name, proxy_ip); - - std::string arm_config_path = proxy_reader.getValue("arm_config_path"); - if (arm_config_path.empty()) - { - zlc::error("Arm config path is empty in proxy config file."); - return 1; + std::string group = proxy_reader.getValue("group"); + int group_port = proxy_reader.getValue("group_port"); + std::string group_name = proxy_reader.getValue("group_name"); + zlc::init(node_name, proxy_ip, group, group_port, group_name); + YAML::Node robot_node = proxy_reader.getSubNode("robot"); + std::vector> arms; + if (robot_node && robot_node.IsSequence()) { + for (size_t i = 0; i < robot_node.size(); ++i) { + std::string type = robot_node[i]["type"].as(); + std::string cfg = robot_node[i]["config_path"].as(); + zlc::info("Robot [{}]: type={}, path={}", i, type, cfg); + try + { + if (type == "panda") + { + arms.push_back(std::make_unique(cfg)); + } + else + { + zlc::warn("Unknown robot type: {}", type); + } + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + return 1; + } + + } } - FrankaArmProxy robot_proxy(arm_config_path); - // FrankaGripperProxy gripper_proxy(gripper_cfg); - // gripper_proxy.start(); + YAML::Node gripper_node = proxy_reader.getSubNode("grippers"); + std::vector> franka_grippers; + std::vector> robotiq_grippers; + if (gripper_node && gripper_node.IsSequence()) { + for (const auto& item : gripper_node) { + std::string type = item["type"].as(); + std::string cfg = item["config_path"].as(); + zlc::info("Gripper: type={}, path={}", type, cfg); + try + { + if (type == "franka_gripper") + { + franka_grippers.push_back(std::make_unique(cfg)); + } + else if (type == "robotiq_gripper") + { + robotiq_grippers.push_back(std::make_unique(cfg)); + } + else + { + zlc::warn("Unknown gripper type: {}", type); + } + } + catch(const std::exception& e) + { + std::cerr << e.what() << '\n'; + return 1; + } + + } + } zlc::spin(); return 0; } diff --git a/src/motion_generator/cartesian_pose_motion_generator.cpp b/src/motion_generator/cartesian_pose_motion_generator.cpp new file mode 100644 index 0000000..fc2a329 --- /dev/null +++ b/src/motion_generator/cartesian_pose_motion_generator.cpp @@ -0,0 +1,300 @@ +#include "motion_generator/cartesian_pose_motion_generator.hpp" + +#include +#include + +CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( + double speed_factor, + const Eigen::Vector3d& goal_position, + const Eigen::Quaterniond& goal_orientation, + AtomicDoubleBuffer& state_buffer, + double tolerance, + double dx_max, + double ddx_max_start, + double ddx_max_goal, + double omega_max, + double domega_max_start, + double domega_max_goal) + : pos_goal_(goal_position), + q_goal_(goal_orientation.normalized()), + dx_max_(Vector3d::Constant(dx_max * speed_factor)), + ddx_max_start_(Vector3d::Constant(ddx_max_start * speed_factor)), + ddx_max_goal_(Vector3d::Constant(ddx_max_goal * speed_factor)), + omega_max_(omega_max * speed_factor), + domega_max_start_(domega_max_start * speed_factor), + domega_max_goal_(domega_max_goal * speed_factor), + state_buffer_(&state_buffer), + tolerance_(tolerance) { + dx_max_sync_.setZero(); + pos_start_.setZero(); + delta_pos_.setZero(); + t_1_sync_pos_.setZero(); + t_2_sync_pos_.setZero(); + t_f_sync_pos_.setZero(); + pos_1_.setZero(); + + omega_max_sync_ = 0.0; + t_1_sync_rot_ = 0.0; + t_2_sync_rot_ = 0.0; + t_f_sync_rot_ = 0.0; + rot_1_ = 0.0; + delta_rot_ = 0.0; + t_f_sync_ = 0.0; +} + +franka::CartesianPose CartesianPoseMotionGenerator::operator()( + const franka::RobotState& robot_state, + franka::Duration period) { + state_buffer_->write(robot_state); + time_ += period.toSec(); + + if (time_ == 0.0) { + // Extract start position from O_T_EE (column-major 4x4 matrix) + pos_start_ = Vector3d(robot_state.O_T_EE[12], robot_state.O_T_EE[13], robot_state.O_T_EE[14]); + + // Extract start orientation from O_T_EE rotation matrix + Eigen::Matrix3d rot_matrix; + rot_matrix << robot_state.O_T_EE[0], robot_state.O_T_EE[4], robot_state.O_T_EE[8], + robot_state.O_T_EE[1], robot_state.O_T_EE[5], robot_state.O_T_EE[9], + robot_state.O_T_EE[2], robot_state.O_T_EE[6], robot_state.O_T_EE[10]; + q_start_ = Eigen::Quaterniond(rot_matrix).normalized(); + + // Ensure shortest path for quaternion SLERP + if (q_start_.dot(q_goal_) < 0.0) { + q_goal_.coeffs() *= -1.0; + } + + delta_pos_ = pos_goal_ - pos_start_; + + // Calculate total rotation angle + Eigen::Quaterniond q_diff = q_start_.inverse() * q_goal_; + delta_rot_ = 2.0 * std::acos(std::clamp(std::abs(q_diff.w()), 0.0, 1.0)); + + // Check if already within tolerance - skip motion if so + if (delta_pos_.norm() < tolerance_ && delta_rot_ < tolerance_) { + franka::CartesianPose output(robot_state.O_T_EE); + output.motion_finished = true; + return output; + } + + calculateSynchronizedValues(); + } + + Vector3d delta_pos_d; + double alpha; + bool motion_finished = calculateDesiredValues(time_, &delta_pos_d, &alpha); + + // Interpolate position + Vector3d current_pos = pos_start_ + delta_pos_d; + + // Interpolate orientation using SLERP + Eigen::Quaterniond current_quat = q_start_.slerp(alpha, q_goal_); + + // Convert to 4x4 matrix + std::array pose_matrix = poseToMatrix(current_pos, current_quat); + + franka::CartesianPose output(pose_matrix); + output.motion_finished = motion_finished; + return output; +} + +bool CartesianPoseMotionGenerator::calculateDesiredValues( + double time, + Vector3d* delta_pos_d, + double* alpha) const { + + // Position interpolation (same S-curve as joint version, but for 3 axes) + Eigen::Vector3i sign_delta_pos; + for (int i = 0; i < 3; i++) { + sign_delta_pos[i] = (delta_pos_[i] >= 0) ? 1 : -1; + } + Vector3d t_d_pos = t_2_sync_pos_ - t_1_sync_pos_; + Vector3d delta_t_2_sync_pos = t_f_sync_pos_ - t_2_sync_pos_; + + std::array pos_motion_finished{}; + + for (int i = 0; i < 3; i++) { + if (std::abs(delta_pos_[i]) < kDeltaPosMotionFinished) { + (*delta_pos_d)[i] = 0; + pos_motion_finished[i] = true; + } else { + if (time < t_1_sync_pos_[i]) { + (*delta_pos_d)[i] = -1.0 / std::pow(t_1_sync_pos_[i], 3.0) * dx_max_sync_[i] * sign_delta_pos[i] * + (0.5 * time - t_1_sync_pos_[i]) * std::pow(time, 3.0); + } else if (time >= t_1_sync_pos_[i] && time < t_2_sync_pos_[i]) { + (*delta_pos_d)[i] = pos_1_[i] + (time - t_1_sync_pos_[i]) * dx_max_sync_[i] * sign_delta_pos[i]; + } else if (time >= t_2_sync_pos_[i] && time < t_f_sync_pos_[i]) { + (*delta_pos_d)[i] = + delta_pos_[i] + + 0.5 * + (1.0 / std::pow(delta_t_2_sync_pos[i], 3.0) * + (time - t_1_sync_pos_[i] - 2.0 * delta_t_2_sync_pos[i] - t_d_pos[i]) * + std::pow((time - t_1_sync_pos_[i] - t_d_pos[i]), 3.0) + + (2.0 * time - 2.0 * t_1_sync_pos_[i] - delta_t_2_sync_pos[i] - 2.0 * t_d_pos[i])) * + dx_max_sync_[i] * sign_delta_pos[i]; + } else { + (*delta_pos_d)[i] = delta_pos_[i]; + pos_motion_finished[i] = true; + } + } + } + + // Rotation interpolation (S-curve for alpha parameter) + bool rot_motion_finished = false; + if (delta_rot_ < kDeltaRotMotionFinished) { + *alpha = 1.0; + rot_motion_finished = true; + } else { + int sign_delta_rot = 1; // Always positive since delta_rot_ is absolute angle + double t_d_rot = t_2_sync_rot_ - t_1_sync_rot_; + double delta_t_2_sync_rot = t_f_sync_rot_ - t_2_sync_rot_; + double delta_rot_d; + + if (time < t_1_sync_rot_) { + delta_rot_d = -1.0 / std::pow(t_1_sync_rot_, 3.0) * omega_max_sync_ * sign_delta_rot * + (0.5 * time - t_1_sync_rot_) * std::pow(time, 3.0); + } else if (time >= t_1_sync_rot_ && time < t_2_sync_rot_) { + delta_rot_d = rot_1_ + (time - t_1_sync_rot_) * omega_max_sync_ * sign_delta_rot; + } else if (time >= t_2_sync_rot_ && time < t_f_sync_rot_) { + delta_rot_d = + delta_rot_ + + 0.5 * + (1.0 / std::pow(delta_t_2_sync_rot, 3.0) * + (time - t_1_sync_rot_ - 2.0 * delta_t_2_sync_rot - t_d_rot) * + std::pow((time - t_1_sync_rot_ - t_d_rot), 3.0) + + (2.0 * time - 2.0 * t_1_sync_rot_ - delta_t_2_sync_rot - 2.0 * t_d_rot)) * + omega_max_sync_ * sign_delta_rot; + } else { + delta_rot_d = delta_rot_; + rot_motion_finished = true; + } + + // Convert to alpha [0, 1] for SLERP + *alpha = std::clamp(delta_rot_d / delta_rot_, 0.0, 1.0); + } + + return std::all_of(pos_motion_finished.cbegin(), pos_motion_finished.cend(), + [](bool is_finished) { return is_finished; }) && rot_motion_finished; +} + +void CartesianPoseMotionGenerator::calculateSynchronizedValues() { + // === Position synchronization (for 3 axes) === + Vector3d dx_max_reach(dx_max_); + Vector3d t_f_pos = Vector3d::Zero(); + Vector3d delta_t_2_pos = Vector3d::Zero(); + Vector3d t_1_pos = Vector3d::Zero(); + Vector3d delta_t_2_sync_pos = Vector3d::Zero(); + Eigen::Vector3i sign_delta_pos; + for (int i = 0; i < 3; i++) { + sign_delta_pos[i] = (delta_pos_[i] >= 0) ? 1 : -1; + } + + for (int i = 0; i < 3; i++) { + if (std::abs(delta_pos_[i]) > kDeltaPosMotionFinished) { + if (std::abs(delta_pos_[i]) < (3.0 / 4.0 * (std::pow(dx_max_[i], 2.0) / ddx_max_start_[i]) + + 3.0 / 4.0 * (std::pow(dx_max_[i], 2.0) / ddx_max_goal_[i]))) { + dx_max_reach[i] = std::sqrt(4.0 / 3.0 * std::abs(delta_pos_[i]) * + (ddx_max_start_[i] * ddx_max_goal_[i]) / + (ddx_max_start_[i] + ddx_max_goal_[i])); + } + t_1_pos[i] = 1.5 * dx_max_reach[i] / ddx_max_start_[i]; + delta_t_2_pos[i] = 1.5 * dx_max_reach[i] / ddx_max_goal_[i]; + t_f_pos[i] = t_1_pos[i] / 2.0 + delta_t_2_pos[i] / 2.0 + std::abs(delta_pos_[i]) / dx_max_reach[i]; + } + } + + double max_t_f_pos = t_f_pos.maxCoeff(); + + // === Rotation synchronization === + double omega_max_reach = omega_max_; + double t_f_rot = 0.0; + double delta_t_2_rot = 0.0; + double t_1_rot = 0.0; + double delta_t_2_sync_rot = 0.0; + + if (delta_rot_ > kDeltaRotMotionFinished) { + if (delta_rot_ < (3.0 / 4.0 * (std::pow(omega_max_, 2.0) / domega_max_start_) + + 3.0 / 4.0 * (std::pow(omega_max_, 2.0) / domega_max_goal_))) { + omega_max_reach = std::sqrt(4.0 / 3.0 * delta_rot_ * + (domega_max_start_ * domega_max_goal_) / + (domega_max_start_ + domega_max_goal_)); + } + t_1_rot = 1.5 * omega_max_reach / domega_max_start_; + delta_t_2_rot = 1.5 * omega_max_reach / domega_max_goal_; + t_f_rot = t_1_rot / 2.0 + delta_t_2_rot / 2.0 + delta_rot_ / omega_max_reach; + } + + // === Synchronize position and rotation to same finish time === + t_f_sync_ = std::max(max_t_f_pos, t_f_rot); + + // Re-compute position timing for synchronized finish + for (int i = 0; i < 3; i++) { + if (std::abs(delta_pos_[i]) > kDeltaPosMotionFinished) { + double a = 1.5 / 2.0 * (ddx_max_goal_[i] + ddx_max_start_[i]); + double b = -1.0 * t_f_sync_ * ddx_max_goal_[i] * ddx_max_start_[i]; + double c = std::abs(delta_pos_[i]) * ddx_max_goal_[i] * ddx_max_start_[i]; + double delta = b * b - 4.0 * a * c; + if (delta < 0.0) { + delta = 0.0; + } + dx_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); + t_1_sync_pos_[i] = 1.5 * dx_max_sync_[i] / ddx_max_start_[i]; + delta_t_2_sync_pos[i] = 1.5 * dx_max_sync_[i] / ddx_max_goal_[i]; + t_f_sync_pos_[i] = t_1_sync_pos_[i] / 2.0 + delta_t_2_sync_pos[i] / 2.0 + + std::abs(delta_pos_[i] / dx_max_sync_[i]); + t_2_sync_pos_[i] = t_f_sync_pos_[i] - delta_t_2_sync_pos[i]; + pos_1_[i] = dx_max_sync_[i] * sign_delta_pos[i] * (0.5 * t_1_sync_pos_[i]); + } + } + + // Re-compute rotation timing for synchronized finish + if (delta_rot_ > kDeltaRotMotionFinished) { + double a = 1.5 / 2.0 * (domega_max_goal_ + domega_max_start_); + double b = -1.0 * t_f_sync_ * domega_max_goal_ * domega_max_start_; + double c = delta_rot_ * domega_max_goal_ * domega_max_start_; + double delta = b * b - 4.0 * a * c; + if (delta < 0.0) { + delta = 0.0; + } + omega_max_sync_ = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); + t_1_sync_rot_ = 1.5 * omega_max_sync_ / domega_max_start_; + delta_t_2_sync_rot = 1.5 * omega_max_sync_ / domega_max_goal_; + t_f_sync_rot_ = t_1_sync_rot_ / 2.0 + delta_t_2_sync_rot / 2.0 + delta_rot_ / omega_max_sync_; + t_2_sync_rot_ = t_f_sync_rot_ - delta_t_2_sync_rot; + rot_1_ = omega_max_sync_ * (0.5 * t_1_sync_rot_); + } +} + +std::array CartesianPoseMotionGenerator::poseToMatrix( + const Vector3d& position, + const Eigen::Quaterniond& orientation) const { + + Eigen::Matrix3d rot_matrix = orientation.normalized().toRotationMatrix(); + + // Column-major 4x4 homogeneous transformation matrix + std::array matrix{}; + + // Rotation part (columns 0-2) + matrix[0] = rot_matrix(0, 0); + matrix[1] = rot_matrix(1, 0); + matrix[2] = rot_matrix(2, 0); + matrix[3] = 0.0; + + matrix[4] = rot_matrix(0, 1); + matrix[5] = rot_matrix(1, 1); + matrix[6] = rot_matrix(2, 1); + matrix[7] = 0.0; + + matrix[8] = rot_matrix(0, 2); + matrix[9] = rot_matrix(1, 2); + matrix[10] = rot_matrix(2, 2); + matrix[11] = 0.0; + + // Translation part (column 3) + matrix[12] = position[0]; + matrix[13] = position[1]; + matrix[14] = position[2]; + matrix[15] = 1.0; + + return matrix; +} diff --git a/src/motion_generator/joint_position_motion_generator.cpp b/src/motion_generator/joint_position_motion_generator.cpp new file mode 100644 index 0000000..e658355 --- /dev/null +++ b/src/motion_generator/joint_position_motion_generator.cpp @@ -0,0 +1,127 @@ +#include "motion_generator/joint_position_motion_generator.hpp" + +#include +#include + +JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, const std::array& q_goal, + AtomicDoubleBuffer& state_buffer, + double tolerance) + : q_goal_(q_goal.data()), state_buffer_(&state_buffer), tolerance_(tolerance) { + dq_max_ *= speed_factor; + ddq_max_start_ *= speed_factor; + ddq_max_goal_ *= speed_factor; + dq_max_sync_.setZero(); + q_start_.setZero(); + delta_q_.setZero(); + t_1_sync_.setZero(); + t_2_sync_.setZero(); + t_f_sync_.setZero(); + q_1_.setZero(); +} + +franka::JointPositions JointPositionMotionGenerator::operator()(const franka::RobotState& robot_state, + franka::Duration period) { + state_buffer_->write(robot_state); + time_ += period.toSec(); + + if (time_ == 0.0) { + q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q.data()); + delta_q_ = q_goal_ - q_start_; + + // Check if already within tolerance - skip motion if so + if (delta_q_.cwiseAbs().maxCoeff() < tolerance_) { + franka::JointPositions output(robot_state.q); + output.motion_finished = true; + return output; + } + + calculateSynchronizedValues(); + } + + JointPositionMotionGenerator::Vector7d delta_q_d; + bool motion_finished = calculateDesiredValues(time_, &delta_q_d); + + std::array joint_positions; + Eigen::VectorXd::Map(joint_positions.data(), 7) = (q_start_ + delta_q_d); + franka::JointPositions output(joint_positions); + output.motion_finished = motion_finished; + return output; +} +bool JointPositionMotionGenerator::calculateDesiredValues(double time, JointPositionMotionGenerator::Vector7d* delta_q_d) const { + JointPositionMotionGenerator::Vector7i sign_delta_q; + sign_delta_q << delta_q_.cwiseSign().cast(); + JointPositionMotionGenerator::Vector7d t_d = t_2_sync_ - t_1_sync_; + JointPositionMotionGenerator::Vector7d delta_t_2_sync = t_f_sync_ - t_2_sync_; + std::array joint_motion_finished{}; + + for (Eigen::Index i = 0; i < 7; i++) { + if (std::abs(delta_q_[i]) < kDeltaQMotionFinished) { + (*delta_q_d)[i] = 0; + joint_motion_finished[i] = true; + } else { + if (time < t_1_sync_[i]) { + (*delta_q_d)[i] = -1.0 / std::pow(t_1_sync_[i], 3.0) * dq_max_sync_[i] * sign_delta_q[i] * + (0.5 * time - t_1_sync_[i]) * std::pow(time, 3.0); + } else if (time >= t_1_sync_[i] && time < t_2_sync_[i]) { + (*delta_q_d)[i] = q_1_[i] + (time - t_1_sync_[i]) * dq_max_sync_[i] * sign_delta_q[i]; + } else if (time >= t_2_sync_[i] && time < t_f_sync_[i]) { + (*delta_q_d)[i] = + delta_q_[i] + + 0.5 * + (1.0 / std::pow(delta_t_2_sync[i], 3.0) * + (time - t_1_sync_[i] - 2.0 * delta_t_2_sync[i] - t_d[i]) * + std::pow((time - t_1_sync_[i] - t_d[i]), 3.0) + + (2.0 * time - 2.0 * t_1_sync_[i] - delta_t_2_sync[i] - 2.0 * t_d[i])) * + dq_max_sync_[i] * sign_delta_q[i]; + } else { + (*delta_q_d)[i] = delta_q_[i]; + joint_motion_finished[i] = true; + } + } + } + return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), + [](bool is_finished) { return is_finished; }); +} + +void JointPositionMotionGenerator::calculateSynchronizedValues() { + JointPositionMotionGenerator::Vector7d dq_max_reach(dq_max_); + JointPositionMotionGenerator::Vector7d t_f = JointPositionMotionGenerator::Vector7d::Zero(); + JointPositionMotionGenerator::Vector7d delta_t_2 = JointPositionMotionGenerator::Vector7d::Zero(); + JointPositionMotionGenerator::Vector7d t_1 = JointPositionMotionGenerator::Vector7d::Zero(); + JointPositionMotionGenerator::Vector7d delta_t_2_sync = JointPositionMotionGenerator::Vector7d::Zero(); + JointPositionMotionGenerator::Vector7i sign_delta_q; + sign_delta_q << delta_q_.cwiseSign().cast(); + + for (Eigen::Index i = 0; i < 7U; i++) { + if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { + if (std::abs(delta_q_[i]) < (3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_start_[i]) + + 3.0 / 4.0 * (std::pow(dq_max_[i], 2.0) / ddq_max_goal_[i]))) { + dq_max_reach[i] = std::sqrt(4.0 / 3.0 * delta_q_[i] * sign_delta_q[i] * + (ddq_max_start_[i] * ddq_max_goal_[i]) / + (ddq_max_start_[i] + ddq_max_goal_[i])); + } + t_1[i] = 1.5 * dq_max_reach[i] / ddq_max_start_[i]; + delta_t_2[i] = 1.5 * dq_max_reach[i] / ddq_max_goal_[i]; + t_f[i] = t_1[i] / 2.0 + delta_t_2[i] / 2.0 + std::abs(delta_q_[i]) / dq_max_reach[i]; + } + } + double max_t_f = t_f.maxCoeff(); + for (Eigen::Index i = 0; i < 7; i++) { + if (std::abs(delta_q_[i]) > kDeltaQMotionFinished) { + double a = 1.5 / 2.0 * (ddq_max_goal_[i] + ddq_max_start_[i]); // NOLINT + double b = -1.0 * max_t_f * ddq_max_goal_[i] * ddq_max_start_[i]; // NOLINT + double c = std::abs(delta_q_[i]) * ddq_max_goal_[i] * ddq_max_start_[i]; // NOLINT + double delta = b * b - 4.0 * a * c; + if (delta < 0.0) { + delta = 0.0; + } + dq_max_sync_[i] = (-1.0 * b - std::sqrt(delta)) / (2.0 * a); + t_1_sync_[i] = 1.5 * dq_max_sync_[i] / ddq_max_start_[i]; + delta_t_2_sync[i] = 1.5 * dq_max_sync_[i] / ddq_max_goal_[i]; + t_f_sync_[i] = + (t_1_sync_)[i] / 2.0 + delta_t_2_sync[i] / 2.0 + std::abs(delta_q_[i] / dq_max_sync_[i]); + t_2_sync_[i] = (t_f_sync_)[i] - delta_t_2_sync[i]; + q_1_[i] = (dq_max_sync_)[i] * sign_delta_q[i] * (0.5 * (t_1_sync_)[i]); + } + } +} diff --git a/src/mujoco_sim/mujoco_panda_env.cpp b/src/mujoco_sim/mujoco_panda_env.cpp index 2b5fee0..8d0195d 100644 --- a/src/mujoco_sim/mujoco_panda_env.cpp +++ b/src/mujoco_sim/mujoco_panda_env.cpp @@ -4,6 +4,7 @@ #include #include #include + #include MujocoPandaEnv::MujocoPandaEnv(const std::string& model_path) : model_path_(model_path) {} diff --git a/src/mujoco_sim/mujoco_robot.cpp b/src/mujoco_sim/mujoco_robot.cpp index bbb75ff..6710893 100644 --- a/src/mujoco_sim/mujoco_robot.cpp +++ b/src/mujoco_sim/mujoco_robot.cpp @@ -1,12 +1,14 @@ #include "mujoco_sim/mujoco_robot.hpp" -#include -#include - #include #include +#include #include +#include +#include +#include + #include "mujoco_sim/mujoco_panda_env.hpp" namespace @@ -21,7 +23,8 @@ namespace MujocoRobot::MujocoRobot(const std::string&) : env_(std::make_unique("./models/franka_emika_panda/scene.xml")), - viewer_(std::make_unique(env_.get())) + viewer_(std::make_unique(env_.get())), + model_(std::make_unique("./models/franka_emika_panda/panda_arm.urdf", "panda_link8")) { env_->start(); env_->refreshRobotState(current_state_); @@ -139,3 +142,337 @@ void MujocoRobot::stop() env_->stop(); viewer_->stop(); } + +// ============================================================================ +// Control conversion helpers +// ============================================================================ + +franka::Torques MujocoRobot::jointPositionToTorque(const franka::JointPositions& desired_positions, + const franka::RobotState& state) +{ + std::array tau_J{}; + // Get gravity compensation from MuJoCo + const auto* data = env_->getData(); + + for (size_t i = 0; i < 7; i++) + { + double pos_error = desired_positions.q[i] - state.q[i]; + double vel_error = 0.0 - state.dq[i]; // Target velocity is zero for position control + tau_J[i] = kDefaultStiffness[i] * pos_error + kDefaultDamping[i] * vel_error; + // Add gravity compensation + tau_J[i] += data->qfrc_bias[i]; + } + franka::Torques torques(tau_J); + torques.motion_finished = desired_positions.motion_finished; + return torques; +} + +franka::Torques MujocoRobot::jointVelocityToTorque(const franka::JointVelocities& desired_velocities, + const franka::RobotState& state) +{ + std::array tau_J{}; + // Get gravity compensation from MuJoCo + const auto* data = env_->getData(); + + for (size_t i = 0; i < 7; i++) + { + // Pure velocity damping control + double vel_error = desired_velocities.dq[i] - state.dq[i]; + tau_J[i] = kDefaultDamping[i] * vel_error; + // Add gravity compensation + tau_J[i] += data->qfrc_bias[i]; + } + franka::Torques torques(tau_J); + torques.motion_finished = desired_velocities.motion_finished; + return torques; +} + +std::array MujocoRobot::cartesianPoseToJointPosition(const franka::CartesianPose& desired_pose, + const franka::RobotState& state) +{ + // Extract target position and orientation from 4x4 matrix (column-major) + Eigen::Vector3d target_pos(desired_pose.O_T_EE[12], desired_pose.O_T_EE[13], desired_pose.O_T_EE[14]); + Eigen::Matrix3d target_rot; + target_rot << desired_pose.O_T_EE[0], desired_pose.O_T_EE[4], desired_pose.O_T_EE[8], + desired_pose.O_T_EE[1], desired_pose.O_T_EE[5], desired_pose.O_T_EE[9], + desired_pose.O_T_EE[2], desired_pose.O_T_EE[6], desired_pose.O_T_EE[10]; + Eigen::Quaterniond target_quat(target_rot); + + // Start from current joint positions + JointPosition q; + for (size_t i = 0; i < 7; i++) + { + q[i] = state.q[i]; + } + + // Iterative IK using Jacobian pseudoinverse + for (int iter = 0; iter < kIKMaxIterations; iter++) + { + // Compute current FK + PoseQuat current_pose = model_->forwardKinematics(q); + Eigen::Vector3d current_pos(current_pose[0], current_pose[1], current_pose[2]); + Eigen::Quaterniond current_quat(current_pose[6], current_pose[3], current_pose[4], current_pose[5]); + + // Compute position error + Eigen::Vector3d pos_error = target_pos - current_pos; + + // Compute orientation error (using quaternion error) + Eigen::Quaterniond quat_error = target_quat * current_quat.inverse(); + if (quat_error.w() < 0.0) + { + quat_error.coeffs() *= -1.0; // Ensure shortest path + } + Eigen::Vector3d rot_error = 2.0 * quat_error.vec(); // Approximate axis-angle + + // Check convergence + double error_norm = std::sqrt(pos_error.squaredNorm() + rot_error.squaredNorm()); + if (error_norm < kIKErrorThreshold) + { + break; + } + + // Compute Jacobian + JocobianMatrix J = model_->computeJacobian(q); + + // Compute 6D error vector + Eigen::Matrix dx; + dx << pos_error, rot_error; + + // Compute Jacobian pseudoinverse: J† = J^T (J J^T)^{-1} + Eigen::Matrix JJt = J * J.transpose(); + Eigen::Matrix JJt_damped = JJt + 1e-6 * Eigen::Matrix::Identity(); + Eigen::Matrix J_pinv = J.transpose() * JJt_damped.inverse(); + + // Compute joint delta + JointPosition dq = J_pinv * dx; + + // Update joint positions + q += kIKStepSize * dq; + } + + std::array result; + for (size_t i = 0; i < 7; i++) + { + result[i] = q[i]; + } + return result; +} + +std::array MujocoRobot::cartesianVelocityToJointVelocity( + const franka::CartesianVelocities& desired_velocities, + const franka::RobotState& state) +{ + // Get current joint positions + JointPosition q; + for (size_t i = 0; i < 7; i++) + { + q[i] = state.q[i]; + } + + // Compute Jacobian at current configuration + JocobianMatrix J = model_->computeJacobian(q); + + // Compute 6D Cartesian velocity vector + Eigen::Matrix dx; + dx << desired_velocities.O_dP_EE[0], desired_velocities.O_dP_EE[1], desired_velocities.O_dP_EE[2], + desired_velocities.O_dP_EE[3], desired_velocities.O_dP_EE[4], desired_velocities.O_dP_EE[5]; + + // Compute Jacobian pseudoinverse: J† = J^T (J J^T)^{-1} + Eigen::Matrix JJt = J * J.transpose(); + Eigen::Matrix JJt_damped = JJt + 1e-6 * Eigen::Matrix::Identity(); + Eigen::Matrix J_pinv = J.transpose() * JJt_damped.inverse(); + + // Compute joint velocities: dq = J† * dx + JointVelocity dq = J_pinv * dx; + + std::array result; + for (size_t i = 0; i < 7; i++) + { + result[i] = dq[i]; + } + return result; +} + +// ============================================================================ +// Control overloads for different command types +// ============================================================================ + +void MujocoRobot::control( + std::function motion_generator_callback, + bool /*limit_rate*/, double /*cutoff_frequency*/) +{ + std::lock_guard guard(control_mutex_); + if (running_) + { + throw franka::InvalidOperationException( + "MujocoRobot::control: A control loop is already running."); + } + + running_ = true; + using clock = std::chrono::steady_clock; + auto next_tick = clock::now(); + auto last_tick = next_tick; + + while (running_) + { + const auto now = clock::now(); + auto dt_ns = std::chrono::duration_cast(now - last_tick); + last_tick = now; + + franka::JointPositions desired_positions = motion_generator_callback(current_state_, toDuration(dt_ns)); + franka::Torques torques = jointPositionToTorque(desired_positions, current_state_); + + { + std::lock_guard lock(state_mutex_); + current_state_.tau_J_d = torques.tau_J; + } + env_->nextStep(torques, current_state_); + + if (desired_positions.motion_finished) + { + running_ = false; + break; + } + + next_tick += kControlPeriod; + std::this_thread::sleep_until(next_tick); + } +} + +void MujocoRobot::control( + std::function motion_generator_callback, + bool /*limit_rate*/, double /*cutoff_frequency*/) +{ + std::lock_guard guard(control_mutex_); + if (running_) + { + throw franka::InvalidOperationException( + "MujocoRobot::control: A control loop is already running."); + } + + running_ = true; + using clock = std::chrono::steady_clock; + auto next_tick = clock::now(); + auto last_tick = next_tick; + + while (running_) + { + const auto now = clock::now(); + auto dt_ns = std::chrono::duration_cast(now - last_tick); + last_tick = now; + + franka::JointVelocities desired_velocities = motion_generator_callback(current_state_, toDuration(dt_ns)); + franka::Torques torques = jointVelocityToTorque(desired_velocities, current_state_); + + { + std::lock_guard lock(state_mutex_); + current_state_.tau_J_d = torques.tau_J; + } + env_->nextStep(torques, current_state_); + + if (desired_velocities.motion_finished) + { + running_ = false; + break; + } + + next_tick += kControlPeriod; + std::this_thread::sleep_until(next_tick); + } +} + +void MujocoRobot::control( + std::function motion_generator_callback, + bool /*limit_rate*/, double /*cutoff_frequency*/) +{ + std::lock_guard guard(control_mutex_); + if (running_) + { + throw franka::InvalidOperationException( + "MujocoRobot::control: A control loop is already running."); + } + + running_ = true; + using clock = std::chrono::steady_clock; + auto next_tick = clock::now(); + auto last_tick = next_tick; + + while (running_) + { + const auto now = clock::now(); + auto dt_ns = std::chrono::duration_cast(now - last_tick); + last_tick = now; + + franka::CartesianPose desired_pose = motion_generator_callback(current_state_, toDuration(dt_ns)); + + // Convert Cartesian pose to joint positions via IK + std::array target_q = cartesianPoseToJointPosition(desired_pose, current_state_); + franka::JointPositions joint_positions{target_q}; + joint_positions.motion_finished = desired_pose.motion_finished; + + franka::Torques torques = jointPositionToTorque(joint_positions, current_state_); + + { + std::lock_guard lock(state_mutex_); + current_state_.tau_J_d = torques.tau_J; + } + env_->nextStep(torques, current_state_); + + if (desired_pose.motion_finished) + { + running_ = false; + break; + } + + next_tick += kControlPeriod; + std::this_thread::sleep_until(next_tick); + } +} + +void MujocoRobot::control( + std::function motion_generator_callback, + bool /*limit_rate*/, double /*cutoff_frequency*/) +{ + std::lock_guard guard(control_mutex_); + if (running_) + { + throw franka::InvalidOperationException( + "MujocoRobot::control: A control loop is already running."); + } + + running_ = true; + using clock = std::chrono::steady_clock; + auto next_tick = clock::now(); + auto last_tick = next_tick; + + while (running_) + { + const auto now = clock::now(); + auto dt_ns = std::chrono::duration_cast(now - last_tick); + last_tick = now; + + franka::CartesianVelocities desired_velocities = motion_generator_callback(current_state_, toDuration(dt_ns)); + + // Convert Cartesian velocities to joint velocities + std::array target_dq = cartesianVelocityToJointVelocity(desired_velocities, current_state_); + franka::JointVelocities joint_velocities{target_dq}; + joint_velocities.motion_finished = desired_velocities.motion_finished; + + franka::Torques torques = jointVelocityToTorque(joint_velocities, current_state_); + + { + std::lock_guard lock(state_mutex_); + current_state_.tau_J_d = torques.tau_J; + } + env_->nextStep(torques, current_state_); + + if (desired_velocities.motion_finished) + { + running_ = false; + break; + } + + next_tick += kControlPeriod; + std::this_thread::sleep_until(next_tick); + } +} diff --git a/src/robots/panda_arm.cpp b/src/robots/panda_arm.cpp new file mode 100644 index 0000000..88949c3 --- /dev/null +++ b/src/robots/panda_arm.cpp @@ -0,0 +1,193 @@ +#include "robots/panda_arm.hpp" + +#include + +static std::atomic running_flag{true}; // let ctrl-c stop the server +static void signalHandler(int signum) +{ + zlc::info("Caught signal {}, shutting down...", signum); + running_flag = false; +} + +PandaArm::PandaArm(const std::string& config_path) + : is_running(false), + config_(config_path), + current_state(AtomicDoubleBuffer(franka::RobotState{})) +{ + // Register service handlers + initRobot(); + + // Check if robot and model initialization was successful + if (!robot_ || !model_) + { + zlc::error("Robot or model initialization failed. Proxy cannot continue."); + is_running = false; + return; + } + + safety_config_.fromFile("./config/SafetyLimitConfig.cfg"); + ControlModeFactory::registerControlModes(control_modes_, *robot_, *model_, current_state, + safety_config_); + // Safety: don't start any control loop by default. If we start "Idle" here (0 torque), + // the arm can sag under gravity before a client switches to an active mode. + // current_control_mode_ = control_modes_.at("GravityComp").get(); + // zlc::info("Default control mode set to 'GravityComp' (not started). Waiting for client service call: {}/set_franka_arm_control_mode", + // config_.name); + setControlMode("Idle"); + initializeService(); +} + +void PandaArm::initializeService() +{ + std::string service_namespace = fmt::format("{}/set_franka_arm_control_mode", config_.name); + zlc::registerServiceHandler(service_namespace, &PandaArm::setControlMode, this); + service_namespace = fmt::format("{}/get_franka_arm_state", config_.name); + zlc::registerServiceHandler(service_namespace, &PandaArm::getFrankaArmState, this); + service_namespace = fmt::format("{}/get_franka_arm_control_mode", config_.name); + zlc::registerServiceHandler(service_namespace, &PandaArm::getFrankaArmControlMode, this); + service_namespace = + fmt::format("{}/move_franka_arm_to_joint_position", config_.name); + zlc::registerServiceHandler(service_namespace, &PandaArm::moveFrankaArmToJointPosition, + this); + service_namespace = + fmt::format("{}/move_franka_arm_to_cartesian_position", config_.name); + // zlc::registerServiceHandler(service_namespace, + // &PandaArm::moveFrankaArmToCartesianPosition, this); +} + +PandaArm::~PandaArm() +{ + stop(); +} + +void PandaArm::initRobot() +{ + //initialize franka robot + try + { + robot_ = std::make_unique(config_.robot_ip); + model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", + "panda_link8"); + } + catch (const franka::NetworkException& e) + { + zlc::error("Franka Network Exception: {}", e.what()); + this->stop(); + return; + } + catch (const std::exception& e) + { + zlc::error("Exception during robot/model initialization: {}", e.what()); + this->stop(); + return; + } + catch (...) + { + zlc::error("Unknown exception during robot/model initialization"); + this->stop(); + return; + } + current_state.write(robot_->readOnce()); + state_pub_thread = std::thread(&PandaArm::statePublishThread, this); + zlc::info("PandaArm started successfully."); + is_running = true; +} + +void PandaArm::stop() +{ + zlc::info("Stopping PandaArm..."); + is_running = false; + if (current_control_mode_) + current_control_mode_->stopControl(); + if (state_pub_thread.joinable()) + state_pub_thread.join(); + // wait for closing + robot_.reset(); + model_.reset(); + current_control_mode_ = nullptr; // reset current control mode + zlc::info("PandaArm stopped successfully."); +} + +// Main loop for processing requests, ctrl-c to stop the server +void PandaArm::spin() +{ + std::signal(SIGINT, signalHandler); // Catch Ctrl+C to stop the server + zlc::info("Entering main spin loop. Press Ctrl+C to exit."); + zlc::info("Current running flag: {}", running_flag.load()); + while (running_flag) + { + std::this_thread::sleep_for(std::chrono::milliseconds(100)); + } + stop(); + zlc::info("Shutdown complete."); +} + +// publish threads +void PandaArm::statePublishThread() +{ + const std::string topic_name = fmt::format("{}/franka_arm_state", config_.name); + zlc::Publisher state_pub(topic_name); + int dt = int(1000.0 / config_.arm_state_pub_rate_hz); + while (is_running) + { + const franka::RobotState rs = current_state.read(); + FrankaArmState frs(rs); + state_pub.publish(frs); + std::this_thread::sleep_for(std::chrono::milliseconds(dt)); + } +} + +zlc::Empty PandaArm::setControlMode(const std::string& mode) +{ + if (control_modes_.find(mode) == control_modes_.end()) + { + zlc::warn("Control mode '{}' not found!", mode); + return zlc::empty; + } + if (current_control_mode_ != nullptr) + { + zlc::info("Stopping previous control mode..."); + current_control_mode_->stopControl(); + } + zlc::info("Switching to control mode: {}", mode); + current_control_mode_ = control_modes_.at(mode).get(); + current_control_mode_->startControl(); + return zlc::empty; +} + +FrankaArmState PandaArm::getFrankaArmState(const zlc::Empty&) +{ + return FrankaArmState(current_state.read()); +} + +std::string PandaArm::getFrankaArmControlMode(const zlc::Empty&) +{ + if (!current_control_mode_) + { + zlc::warn("No active control mode"); + return "None"; + } + return current_control_mode_->getModeName(); +} + +std::pair> PandaArm::moveFrankaArmToJointPosition( + const std::vector& target_q) +{ + if (!current_control_mode_) + { + return {std::string(protocol::FrankaResponseCode::FAIL), {}}; + } + if (target_q.size() != 7) + { + zlc::warn("moveFrankaArmToJointPosition: invalid payload size {} (expected 7)", + target_q.size()); + return {std::string(protocol::FrankaResponseCode::INVALID_ARG), {}}; + } + std::array target_q_array{}; + std::copy(target_q.begin(), target_q.end(), target_q_array.begin()); + // current_control_mode_->stopControl(); + const bool ok = current_control_mode_->moveToJointPosition(target_q_array); + return {std::string(ok ? protocol::FrankaResponseCode::SUCCESS + : protocol::FrankaResponseCode::FAIL), + {}}; +} diff --git a/src/robots/panda_gripper.cpp b/src/robots/panda_gripper.cpp new file mode 100644 index 0000000..6834d03 --- /dev/null +++ b/src/robots/panda_gripper.cpp @@ -0,0 +1,82 @@ +#include "robots/panda_gripper.hpp" + +PandaGripper::PandaGripper(const std::string& config_path) + : is_running_(false), + command_(AtomicDoubleBuffer(GraspCommand{})), + config_(config_path) +{ + gripper_ = std::make_shared(config_.gripper_ip); + gripper_->homing(); + // Initialize the command to the current width so the gripper doesn't immediately + // try to close by default on startup. + const franka::GripperState gs0 = gripper_->readOnce(); + command_.write(GraspCommand{static_cast(gs0.width), 0.1}); + is_running_ = true; + zlc::info("Gripper running flag set to {}", is_running_.load()); + state_pub_thread_ = std::thread(&PandaGripper::statePubThread, this); + if (config_.enable_control) + { + zlc::registerSubscriberHandler(config_.command_topic, &PandaGripper::updateCommand, this); + control_thread_ = std::thread(&PandaGripper::controlLoopThread, this); + zlc::info("Gripper control enabled: subscribing to '{}'", config_.command_topic); + } + else + { + zlc::info("Gripper control disabled (monitor-only): publishing '{}' only", config_.state_topic); + } +} + +PandaGripper::~PandaGripper() +{ + stop(); +} + +void PandaGripper::stop() +{ + zlc::info("Stopping PandaGripper..."); + is_running_ = false; + if (state_pub_thread_.joinable()) state_pub_thread_.join(); + if (control_thread_.joinable()) control_thread_.join(); + gripper_.reset(); + zlc::info("PandaGripper stopped successfully."); +} + +void PandaGripper::controlLoopThread() +{ + while (is_running_) { + GraspCommand cmd = command_.read(); + try { + gripper_->move(cmd.width, cmd.speed); + } + catch (const std::exception& e) { + zlc::error("PandaGripper control loop exception: {}", e.what()); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +void PandaGripper::statePubThread() +{ + zlc::info("PandaGripper state publishing thread started."); + const std::string topic_name = fmt::format("{}/{}", config_.name, config_.state_topic); + zlc::Publisher state_publisher(topic_name); + while (is_running_) { + franka::GripperState gs = gripper_->readOnce(); + GripperStateMsg gsm{ + gs.width, + gs.max_width, + gs.is_grasped, + gs.temperature, + gs.time.toMSec() + }; + state_publisher.publish(gsm); + std::this_thread::sleep_until( + std::chrono::steady_clock::now() + std::chrono::milliseconds(1000 / GRIPPER_PUB_RATE_HZ) + ); + } +} + +void PandaGripper::updateCommand(const GraspCommand& cmd) +{ + command_.write(cmd); +} diff --git a/src/robots/robotiq_gripper.cpp b/src/robots/robotiq_gripper.cpp new file mode 100644 index 0000000..f1aa34d --- /dev/null +++ b/src/robots/robotiq_gripper.cpp @@ -0,0 +1,109 @@ +#include "robots/robotiq_gripper.hpp" + +RobotiqGripper::RobotiqGripper(const std::string& config_path) + : is_running_(false), + command_(AtomicDoubleBuffer(RobotiqGraspCommand{})), + config_(config_path) +{ + gripper_ = std::make_shared(); + bool connected = gripper_->connect(config_.port, config_.baud, config_.scale_alpha, + config_.scale_beta); + if (!connected) { + zlc::error("RobotiqGripper failed to connect on port {}", config_.port); + } else { + gripper_->set_timeout(config_.receive_timeout_ms); + if (!gripper_->is_activated()) { + zlc::info("RobotiqGripper activating gripper..."); + gripper_->activate(true); + } + } + command_.write(RobotiqGraspCommand{}); // initialize command buffer + bool reset_success = gripper_->reset(true); + zlc::info("RobotiqGripper reset result: {}", reset_success); + is_running_ = true; + zlc::info("RobotiqGripper running flag set to {}", is_running_.load()); + zlc::registerSubscriberHandler(config_.command_topic, &RobotiqGripper::updateCommand, this); + command_.write(RobotiqGraspCommand{}); // initialize command buffer + state_pub_thread_ = std::thread(&RobotiqGripper::statePubThread, this); + control_thread_ = std::thread(&RobotiqGripper::controlLoopThread, this); + robotiq::GripperFeedback gs = gripper_->get_feedback(); + zlc::info("RobotiqFeedback after init: Pos {:.2f}, CmdPos {}, Curr {:.2f}", + gs.position, gs.raw_commanded_position, gs.current); +} + +RobotiqGripper::~RobotiqGripper() +{ + stop(); +} + +void RobotiqGripper::stop() +{ + zlc::info("Stopping RobotiqGripper..."); + is_running_ = false; + if (state_pub_thread_.joinable()) state_pub_thread_.join(); + if (control_thread_.joinable()) control_thread_.join(); + gripper_->reset(true); + zlc::info("RobotiqGripper stopped successfully."); +} + +bool RobotiqGripper::commandChanged(const RobotiqGraspCommand& a, const RobotiqGraspCommand& b) const +{ + return a.position != b.position || a.speed != b.speed || a.force != b.force || + a.blocking != b.blocking; +} + +void RobotiqGripper::controlLoopThread() +{ + RobotiqGraspCommand last_cmd = command_.read(); + while (is_running_) { + RobotiqGraspCommand cmd = command_.read(); + if (gripper_) { + try { + if (commandChanged(cmd, last_cmd)) { + // zlc::info("RobotiqGripper executing new command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + // cmd.position, cmd.speed, cmd.force, cmd.blocking); + gripper_->set_gripper_position(cmd.position, cmd.speed, cmd.force, cmd.blocking); + last_cmd = cmd; + } + } + catch (const std::exception& e) { + zlc::error("RobotiqGripper control loop exception: {}", e.what()); + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } +} + +void RobotiqGripper::statePubThread() +{ + zlc::info("RobotiqGripper state publishing thread started."); + const std::string topic_name = fmt::format("{}/{}", config_.name, config_.state_topic); + zlc::Publisher state_publisher(topic_name); + while (is_running_) { + if (gripper_) { + robotiq::GripperFeedback gs = gripper_->get_feedback(); + RobotiqGraspCommand last_cmd = command_.read(); + RobotiqGripperStateMsg gsm{ + gs.commanded_position, + last_cmd.speed, + last_cmd.force, + gs.position, + gs.current, + gs.raw_commanded_position, + gs.raw_position + }; + state_publisher.publish(gsm); + } + std::this_thread::sleep_until( + std::chrono::steady_clock::now() + + std::chrono::milliseconds(1000 / config_.gripper_pub_rate_hz) + ); + } +} + +void RobotiqGripper::updateCommand(const RobotiqGraspCommand& cmd) +{ + command_.write(cmd); + // zlc::info("RobotiqGripper received new command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + // cmd.position, cmd.speed, cmd.force, cmd.blocking); +} diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index 4029029..56221bb 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -1,13 +1,12 @@ - #include "utils/robot_model.hpp" -#include "pinocchio/algorithm/frames.hpp" -#include "pinocchio/algorithm/jacobian.hpp" -#include "pinocchio/algorithm/joint-configuration.hpp" -#include "pinocchio/algorithm/kinematics.hpp" -#include "pinocchio/algorithm/rnea.hpp" -#include "pinocchio/parsers/sample-models.hpp" -#include "pinocchio/parsers/urdf.hpp" +#include +#include +#include +#include +#include +#include +#include PandaPinocchioModel::PandaPinocchioModel(std::string urdf_filename, std::string ee_joint_name) { @@ -128,4 +127,4 @@ JointPosition PandaPinocchioModel::inverseDynamics(JointPosition joint_position, // JointPosition result{}; // result = g; // return result; -// } +// } \ No newline at end of file diff --git a/test.bash b/test.bash old mode 100755 new mode 100644 diff --git a/test/follower_client.py b/test/follower_client.py deleted file mode 100644 index cf3b4ae..0000000 --- a/test/follower_client.py +++ /dev/null @@ -1,97 +0,0 @@ -import zmq -import struct -import time -import threading -import signal -import sys -#ARM &GRIPPER P1 follower -# === MSGID&ControlModeID === -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 ControlModeID: - 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([ControlModeID.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 deleted file mode 100644 index ab15934..0000000 --- a/test/follower_interact_client.py +++ /dev/null @@ -1,115 +0,0 @@ -import zmq -import struct -import time -import threading -import signal -import sys -#P2 follower interact -# === MSGID & ControlModeID === -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 ControlModeID: - 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([ControlModeID.PD_TEST])) - - elif cmd == "set_mode idle": - send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ControlModeID.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 deleted file mode 100644 index f2d8f83..0000000 --- a/test/leader_client.py +++ /dev/null @@ -1,99 +0,0 @@ -import zmq -import struct -import time -import threading -import signal -import sys -#ARM &GRIPPER P2 leader -# === MSGID&ControlModeID === -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 ControlModeID: - 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([ControlModeID.HUMAN_MODE])) - -# print("\n--- Set Arm ControlMode to JOINT_VELOCITY (Follower) ---") -# send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ControlModeID.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 deleted file mode 100644 index 012dea9..0000000 --- a/test/leader_interact_client.py +++ /dev/null @@ -1,115 +0,0 @@ -import zmq -import struct -import time -import threading -import signal -import sys -#P1 leader interact -# === MSGID & ControlModeID === -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 ControlModeID: - 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([ControlModeID.HUMAN_MODE])) - - elif cmd == "set_mode idle": - send_arm_request(MsgID.SET_CONTROL_MODE_REQ, bytes([ControlModeID.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 diff --git a/test/request.py b/test/request.py deleted file mode 100755 index 56caab8..0000000 --- a/test/request.py +++ /dev/null @@ -1,20 +0,0 @@ -import zmq -import json - -# Create a context -context = zmq.Context() - -# Create a SUB (subscriber) socket -client = context.socket(zmq.REQ) - -# Connect to the publisher (replace with the correct IP and port) -client.connect("tcp://127.0.0.1:5556") -# Receive and print messages - -pi = 3.1415926 - -# _ = input() -target_joint_pose = [0, -pi/4, 0, -3 * pi / 4, 0, pi / 2, pi / 4] -client.send_string(json.dumps(target_joint_pose)) -message = client.recv() -print(message) diff --git a/test/subscriber.py b/test/subscriber.py deleted file mode 100755 index 5dc3edc..0000000 --- a/test/subscriber.py +++ /dev/null @@ -1,25 +0,0 @@ -import zmq -import json - -# Create a context -context = zmq.Context() - -# Create a SUB (subscriber) socket -subscriber = context.socket(zmq.SUB) - -# Connect to the publisher (replace with the correct IP and port) -subscriber.connect("tcp://141.3.53.152:5555") -subscriber.setsockopt_string(zmq.SUBSCRIBE, "") -# Receive and print messages -try: - while True: - print("waiting for message") - message = subscriber.recv_string() # Receive a message - data = json.loads(message) - print(f"Received message: {data}") - -except KeyboardInterrupt: - print("Subscriber interrupted.") -finally: - subscriber.close() - context.term() diff --git a/third_party/robotiq-gripper-interface/.clang-format b/third_party/robotiq-gripper-interface/.clang-format new file mode 100644 index 0000000..7bb1f50 --- /dev/null +++ b/third_party/robotiq-gripper-interface/.clang-format @@ -0,0 +1,116 @@ +--- +Language: Cpp +BasedOnStyle: Google +AccessModifierOffset: -1 +AlignAfterOpenBracket: Align +AlignConsecutiveAssignments: false +AlignConsecutiveDeclarations: false +AlignEscapedNewlines: Left +AlignOperands: true +AlignTrailingComments: true +AllowAllParametersOfDeclarationOnNextLine: true +AllowShortBlocksOnASingleLine: false +AllowShortCaseLabelsOnASingleLine: false +AllowShortFunctionsOnASingleLine: All +AllowShortIfStatementsOnASingleLine: true +AllowShortLoopsOnASingleLine: true +AlwaysBreakAfterDefinitionReturnType: None +AlwaysBreakAfterReturnType: None +AlwaysBreakBeforeMultilineStrings: true +AlwaysBreakTemplateDeclarations: true +BinPackArguments: true +BinPackParameters: true +BraceWrapping: + AfterClass: false + AfterControlStatement: false + AfterEnum: false + AfterFunction: false + AfterNamespace: false + AfterObjCDeclaration: false + AfterStruct: false + AfterUnion: false + AfterExternBlock: false + BeforeCatch: false + BeforeElse: false + IndentBraces: false + SplitEmptyFunction: true + SplitEmptyRecord: true + SplitEmptyNamespace: true +BreakBeforeBinaryOperators: None +BreakBeforeBraces: Attach +BreakBeforeInheritanceComma: false +BreakBeforeTernaryOperators: true +BreakConstructorInitializersBeforeComma: false +BreakConstructorInitializers: BeforeColon +BreakAfterJavaFieldAnnotations: false +BreakStringLiterals: true +ColumnLimit: 90 +CommentPragmas: '^ IWYU pragma:' +CompactNamespaces: false +ConstructorInitializerAllOnOneLineOrOnePerLine: true +ConstructorInitializerIndentWidth: 4 +ContinuationIndentWidth: 4 +Cpp11BracedListStyle: true +DerivePointerAlignment: false +DisableFormat: false +ExperimentalAutoDetectBinPacking: false +FixNamespaceComments: true +ForEachMacros: + - foreach + - Q_FOREACH + - BOOST_FOREACH +IncludeBlocks: Preserve +IncludeCategories: + - Regex: '^' + Priority: 2 + - Regex: '^<.*\.h>' + Priority: 1 + - Regex: '^<.*' + Priority: 2 + - Regex: '.*' + Priority: 3 +IncludeIsMainRegex: '([-_](test|unittest))?$' +IndentCaseLabels: true +IndentPPDirectives: None +IndentWidth: 2 +IndentWrappedFunctionNames: false +JavaScriptQuotes: Leave +JavaScriptWrapImports: true +KeepEmptyLinesAtTheStartOfBlocks: false +MacroBlockBegin: '' +MacroBlockEnd: '' +MaxEmptyLinesToKeep: 1 +NamespaceIndentation: None +ObjCBlockIndentWidth: 2 +ObjCSpaceAfterProperty: false +ObjCSpaceBeforeProtocolList: false +PenaltyBreakAssignment: 2 +PenaltyBreakBeforeFirstCallParameter: 1 +PenaltyBreakComment: 300 +PenaltyBreakFirstLessLess: 120 +PenaltyBreakString: 1000 +PenaltyExcessCharacter: 1000000 +PenaltyReturnTypeOnItsOwnLine: 200 +PointerAlignment: Left +ReflowComments: true +SortIncludes: true +SortUsingDeclarations: true +SpaceAfterCStyleCast: false +SpaceAfterTemplateKeyword: true +SpaceBeforeAssignmentOperators: true +SpaceBeforeParens: ControlStatements +SpaceInEmptyParentheses: false +SpacesBeforeTrailingComments: 2 +SpacesInAngles: false +SpacesInContainerLiterals: true +SpacesInCStyleCastParentheses: false +SpacesInParentheses: false +SpacesInSquareBrackets: false +Standard: Auto +TabWidth: 8 +UseTab: Never +--- +Language: Proto +BasedOnStyle: Google +ColumnLimit: 90 +... diff --git a/third_party/robotiq-gripper-interface/.gitignore b/third_party/robotiq-gripper-interface/.gitignore new file mode 100644 index 0000000..78debd9 --- /dev/null +++ b/third_party/robotiq-gripper-interface/.gitignore @@ -0,0 +1,51 @@ +# temp & build folders +DerivedData +obj +build +html +bin +lib +*.dir +*.tmp + +# Mac files +.DS_Store + +# Python files +__pycache__ + +# IDE +.vscode + +# Jupyter notebooks +.ipynb_checkpoints + +# Compiled objects +*.slo +*.lo +*.o +*.obj + +# Precompiled headers +*.gch +*.pch + +# Dynamic libraries +# *.so # This is commented to commit the shlib needed for IK unit tests +*.dylib +*.dll +*.pyd + +# Static libraries +*.lai +*.la +*.a +*.lib + +# Exectuables +*.exe +*.out +*.app + +# Data +*.csv diff --git a/third_party/robotiq-gripper-interface/CMakeLists.txt b/third_party/robotiq-gripper-interface/CMakeLists.txt new file mode 100644 index 0000000..839e0d9 --- /dev/null +++ b/third_party/robotiq-gripper-interface/CMakeLists.txt @@ -0,0 +1,130 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +cmake_minimum_required(VERSION 3.10.2) + +project(RobotiqGripperInterface) + +set(CMAKE_CXX_STANDARD 17) +set(CMAKE_MODULE_PATH "${CMAKE_SOURCE_DIR}/cmake") + +set(TARGET_NAME robotiq-gripper-interface) + +# Include custom functions +include(CMakePackageConfigHelpers) +include(GNUInstallDirs) + +include(CTest) +enable_testing() + +add_compile_options(-Wall -Wextra -O3 -g) + +if(NOT CMAKE_BUILD_TYPE OR CMAKE_BUILD_TYPE STREQUAL "") + set(CMAKE_BUILD_TYPE "Release" CACHE STRING "" FORCE) +endif() + +# ----------------------------------------------------------------------------- +# Set dependencies +# ----------------------------------------------------------------------------- +find_package(Boost 1.65.1 REQUIRED COMPONENTS system) + +include_directories( + ${PROJECT_SOURCE_DIR}/include + ${PROJECT_SOURCE_DIR} + ${Boost_INCLUDE_DIRS} +) + +# ----------------------------------------------------------------------------- +# Library +# ----------------------------------------------------------------------------- + +# Set the public header names +set(library_public_hdrs + ${PROJECT_SOURCE_DIR}/include/robotiq/constants.h + ${PROJECT_SOURCE_DIR}/include/robotiq/types.h + ${PROJECT_SOURCE_DIR}/include/robotiq/robotiq_gripper_interface.h +) + +# Set the header names +set(library_private_hdrs + ${PROJECT_SOURCE_DIR}/src/helpers.h + ${PROJECT_SOURCE_DIR}/src/timeout_reader.h +) + +# Set the source file names +set(library_srcs + ${PROJECT_SOURCE_DIR}/src/robotiq_gripper_interface.cc + ${PROJECT_SOURCE_DIR}/src/helpers.cc + ${PROJECT_SOURCE_DIR}/src/timeout_reader.cc +) + +add_library(${TARGET_NAME} SHARED ${library_srcs}) + +set_target_properties(${TARGET_NAME} PROPERTIES + LIBRARY_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib + ARCHIVE_OUTPUT_DIRECTORY ${PROJECT_SOURCE_DIR}/lib + PUBLIC_HEADER "${library_public_hdrs}" + PRIVATE_HEADER "${library_private_hdrs}" +) + +target_link_libraries(${TARGET_NAME} PRIVATE + ${Boost_LIBRARIES} + pthread +) + +configure_package_config_file( + ${CMAKE_SOURCE_DIR}/cmake/RobotiqGripperInterfaceConfig.cmake.in + ${CMAKE_BINARY_DIR}/cmake/RobotiqGripperInterfaceConfig.cmake + INSTALL_DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +install( + TARGETS ${TARGET_NAME} + EXPORT ${PROJECT_NAME}Targets + INCLUDES DESTINATION ${CMAKE_INSTALL_INCLUDEDIR} + RUNTIME DESTINATION ${CMAKE_INSTALL_BINDIR} + ARCHIVE DESTINATION ${CMAKE_INSTALL_LIBDIR} + LIBRARY DESTINATION ${CMAKE_INSTALL_LIBDIR} + PUBLIC_HEADER DESTINATION ${CMAKE_INSTALL_INCLUDEDIR}/robotiq +) + +install( + EXPORT ${PROJECT_NAME}Targets + FILE ${PROJECT_NAME}Targets.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +install(FILES + ${CMAKE_BINARY_DIR}/cmake/RobotiqGripperInterfaceConfig.cmake + DESTINATION ${CMAKE_INSTALL_LIBDIR}/cmake/${PROJECT_NAME} +) + +# ----------------------------------------------------------------------------- +# Tests +# ----------------------------------------------------------------------------- +if(BUILD_TESTING) + find_package(GTest REQUIRED) + include_directories(${GTEST_INCLUDE_DIR}) + add_subdirectory(tests) +endif() + +# ----------------------------------------------------------------------------- +# Examples +# ----------------------------------------------------------------------------- +add_subdirectory(examples/activate_gripper) +add_subdirectory(examples/open_gripper) +add_subdirectory(examples/close_gripper) +add_subdirectory(examples/position_gripper) +add_subdirectory(examples/speed_force_setting) +add_subdirectory(examples/read_write_simultaneous) diff --git a/third_party/robotiq-gripper-interface/CODE_OF_CONDUCT.md b/third_party/robotiq-gripper-interface/CODE_OF_CONDUCT.md new file mode 100644 index 0000000..5b627cf --- /dev/null +++ b/third_party/robotiq-gripper-interface/CODE_OF_CONDUCT.md @@ -0,0 +1,4 @@ +## Code of Conduct +This project has adopted the [Amazon Open Source Code of Conduct](https://aws.github.io/code-of-conduct). +For more information see the [Code of Conduct FAQ](https://aws.github.io/code-of-conduct-faq) or contact +opensource-codeofconduct@amazon.com with any additional questions or comments. diff --git a/third_party/robotiq-gripper-interface/CONTRIBUTING.md b/third_party/robotiq-gripper-interface/CONTRIBUTING.md new file mode 100644 index 0000000..c4b6a1c --- /dev/null +++ b/third_party/robotiq-gripper-interface/CONTRIBUTING.md @@ -0,0 +1,59 @@ +# Contributing Guidelines + +Thank you for your interest in contributing to our project. Whether it's a bug report, new feature, correction, or additional +documentation, we greatly value feedback and contributions from our community. + +Please read through this document before submitting any issues or pull requests to ensure we have all the necessary +information to effectively respond to your bug report or contribution. + + +## Reporting Bugs/Feature Requests + +We welcome you to use the GitHub issue tracker to report bugs or suggest features. + +When filing an issue, please check existing open, or recently closed, issues to make sure somebody else hasn't already +reported the issue. Please try to include as much information as you can. Details like these are incredibly useful: + +* A reproducible test case or series of steps +* The version of our code being used +* Any modifications you've made relevant to the bug +* Anything unusual about your environment or deployment + + +## Contributing via Pull Requests +Contributions via pull requests are much appreciated. Before sending us a pull request, please ensure that: + +1. You are working against the latest source on the *main* branch. +2. You check existing open, and recently merged, pull requests to make sure someone else hasn't addressed the problem already. +3. You open an issue to discuss any significant work - we would hate for your time to be wasted. + +To send us a pull request, please: + +1. Fork the repository. +2. Modify the source; please focus on the specific change you are contributing. If you also reformat all the code, it will be hard for us to focus on your change. +3. Ensure local tests pass. +4. Commit to your fork using clear commit messages. +5. Send us a pull request, answering any default questions in the pull request interface. +6. Pay attention to any automated CI failures reported in the pull request, and stay involved in the conversation. + +GitHub provides additional document on [forking a repository](https://help.github.com/articles/fork-a-repo/) and +[creating a pull request](https://help.github.com/articles/creating-a-pull-request/). + + +## Finding contributions to work on +Looking at the existing issues is a great way to find something to contribute on. As our projects, by default, use the default GitHub issue labels (enhancement/bug/duplicate/help wanted/invalid/question/wontfix), looking at any 'help wanted' issues is a great place to start. + + +## Code of Conduct +This project has adopted the [Amazon Open Source Code of Conduct](https://aws.github.io/code-of-conduct). +For more information see the [Code of Conduct FAQ](https://aws.github.io/code-of-conduct-faq) or contact +opensource-codeofconduct@amazon.com with any additional questions or comments. + + +## Security issue notifications +If you discover a potential security issue in this project we ask that you notify AWS/Amazon Security via our [vulnerability reporting page](http://aws.amazon.com/security/vulnerability-reporting/). Please do **not** create a public github issue. + + +## Licensing + +See the [LICENSE](LICENSE) file for our project's licensing. We will ask you to confirm the licensing of your contribution. diff --git a/third_party/robotiq-gripper-interface/Dockerfile b/third_party/robotiq-gripper-interface/Dockerfile new file mode 100644 index 0000000..5d45460 --- /dev/null +++ b/third_party/robotiq-gripper-interface/Dockerfile @@ -0,0 +1,18 @@ +FROM ubuntu:20.04 + +# Avoid interactive prompts +ENV DEBIAN_FRONTEND=noninteractive + +# Install G++, CMake, Boost, and GTest +RUN apt-get clean \ + && apt-get update \ + && apt-get -y install --no-install-recommends \ + g++ \ + cmake \ + make \ + libboost-all-dev \ + libgtest-dev \ + && rm -rf /var/lib/apt/lists/* + +# Copy package in +COPY . /robotiq-gripper-interface diff --git a/third_party/robotiq-gripper-interface/LICENSE b/third_party/robotiq-gripper-interface/LICENSE new file mode 100644 index 0000000..67db858 --- /dev/null +++ b/third_party/robotiq-gripper-interface/LICENSE @@ -0,0 +1,175 @@ + + Apache License + Version 2.0, January 2004 + http://www.apache.org/licenses/ + + TERMS AND CONDITIONS FOR USE, REPRODUCTION, AND DISTRIBUTION + + 1. Definitions. + + "License" shall mean the terms and conditions for use, reproduction, + and distribution as defined by Sections 1 through 9 of this document. + + "Licensor" shall mean the copyright owner or entity authorized by + the copyright owner that is granting the License. + + "Legal Entity" shall mean the union of the acting entity and all + other entities that control, are controlled by, or are under common + control with that entity. For the purposes of this definition, + "control" means (i) the power, direct or indirect, to cause the + direction or management of such entity, whether by contract or + otherwise, or (ii) ownership of fifty percent (50%) or more of the + outstanding shares, or (iii) beneficial ownership of such entity. + + "You" (or "Your") shall mean an individual or Legal Entity + exercising permissions granted by this License. + + "Source" form shall mean the preferred form for making modifications, + including but not limited to software source code, documentation + source, and configuration files. + + "Object" form shall mean any form resulting from mechanical + transformation or translation of a Source form, including but + not limited to compiled object code, generated documentation, + and conversions to other media types. + + "Work" shall mean the work of authorship, whether in Source or + Object form, made available under the License, as indicated by a + copyright notice that is included in or attached to the work + (an example is provided in the Appendix below). + + "Derivative Works" shall mean any work, whether in Source or Object + form, that is based on (or derived from) the Work and for which the + editorial revisions, annotations, elaborations, or other modifications + represent, as a whole, an original work of authorship. For the purposes + of this License, Derivative Works shall not include works that remain + separable from, or merely link (or bind by name) to the interfaces of, + the Work and Derivative Works thereof. + + "Contribution" shall mean any work of authorship, including + the original version of the Work and any modifications or additions + to that Work or Derivative Works thereof, that is intentionally + submitted to Licensor for inclusion in the Work by the copyright owner + or by an individual or Legal Entity authorized to submit on behalf of + the copyright owner. For the purposes of this definition, "submitted" + means any form of electronic, verbal, or written communication sent + to the Licensor or its representatives, including but not limited to + communication on electronic mailing lists, source code control systems, + and issue tracking systems that are managed by, or on behalf of, the + Licensor for the purpose of discussing and improving the Work, but + excluding communication that is conspicuously marked or otherwise + designated in writing by the copyright owner as "Not a Contribution." + + "Contributor" shall mean Licensor and any individual or Legal Entity + on behalf of whom a Contribution has been received by Licensor and + subsequently incorporated within the Work. + + 2. Grant of Copyright License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + copyright license to reproduce, prepare Derivative Works of, + publicly display, publicly perform, sublicense, and distribute the + Work and such Derivative Works in Source or Object form. + + 3. Grant of Patent License. Subject to the terms and conditions of + this License, each Contributor hereby grants to You a perpetual, + worldwide, non-exclusive, no-charge, royalty-free, irrevocable + (except as stated in this section) patent license to make, have made, + use, offer to sell, sell, import, and otherwise transfer the Work, + where such license applies only to those patent claims licensable + by such Contributor that are necessarily infringed by their + Contribution(s) alone or by combination of their Contribution(s) + with the Work to which such Contribution(s) was submitted. If You + institute patent litigation against any entity (including a + cross-claim or counterclaim in a lawsuit) alleging that the Work + or a Contribution incorporated within the Work constitutes direct + or contributory patent infringement, then any patent licenses + granted to You under this License for that Work shall terminate + as of the date such litigation is filed. + + 4. Redistribution. You may reproduce and distribute copies of the + Work or Derivative Works thereof in any medium, with or without + modifications, and in Source or Object form, provided that You + meet the following conditions: + + (a) You must give any other recipients of the Work or + Derivative Works a copy of this License; and + + (b) You must cause any modified files to carry prominent notices + stating that You changed the files; and + + (c) You must retain, in the Source form of any Derivative Works + that You distribute, all copyright, patent, trademark, and + attribution notices from the Source form of the Work, + excluding those notices that do not pertain to any part of + the Derivative Works; and + + (d) If the Work includes a "NOTICE" text file as part of its + distribution, then any Derivative Works that You distribute must + include a readable copy of the attribution notices contained + within such NOTICE file, excluding those notices that do not + pertain to any part of the Derivative Works, in at least one + of the following places: within a NOTICE text file distributed + as part of the Derivative Works; within the Source form or + documentation, if provided along with the Derivative Works; or, + within a display generated by the Derivative Works, if and + wherever such third-party notices normally appear. The contents + of the NOTICE file are for informational purposes only and + do not modify the License. You may add Your own attribution + notices within Derivative Works that You distribute, alongside + or as an addendum to the NOTICE text from the Work, provided + that such additional attribution notices cannot be construed + as modifying the License. + + You may add Your own copyright statement to Your modifications and + may provide additional or different license terms and conditions + for use, reproduction, or distribution of Your modifications, or + for any such Derivative Works as a whole, provided Your use, + reproduction, and distribution of the Work otherwise complies with + the conditions stated in this License. + + 5. Submission of Contributions. Unless You explicitly state otherwise, + any Contribution intentionally submitted for inclusion in the Work + by You to the Licensor shall be under the terms and conditions of + this License, without any additional terms or conditions. + Notwithstanding the above, nothing herein shall supersede or modify + the terms of any separate license agreement you may have executed + with Licensor regarding such Contributions. + + 6. Trademarks. This License does not grant permission to use the trade + names, trademarks, service marks, or product names of the Licensor, + except as required for reasonable and customary use in describing the + origin of the Work and reproducing the content of the NOTICE file. + + 7. Disclaimer of Warranty. Unless required by applicable law or + agreed to in writing, Licensor provides the Work (and each + Contributor provides its Contributions) on an "AS IS" BASIS, + WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or + implied, including, without limitation, any warranties or conditions + of TITLE, NON-INFRINGEMENT, MERCHANTABILITY, or FITNESS FOR A + PARTICULAR PURPOSE. You are solely responsible for determining the + appropriateness of using or redistributing the Work and assume any + risks associated with Your exercise of permissions under this License. + + 8. Limitation of Liability. In no event and under no legal theory, + whether in tort (including negligence), contract, or otherwise, + unless required by applicable law (such as deliberate and grossly + negligent acts) or agreed to in writing, shall any Contributor be + liable to You for damages, including any direct, indirect, special, + incidental, or consequential damages of any character arising as a + result of this License or out of the use or inability to use the + Work (including but not limited to damages for loss of goodwill, + work stoppage, computer failure or malfunction, or any and all + other commercial damages or losses), even if such Contributor + has been advised of the possibility of such damages. + + 9. Accepting Warranty or Additional Liability. While redistributing + the Work or Derivative Works thereof, You may choose to offer, + and charge a fee for, acceptance of support, warranty, indemnity, + or other liability obligations and/or rights consistent with this + License. However, in accepting such obligations, You may act only + on Your own behalf and on Your sole responsibility, not on behalf + of any other Contributor, and only if You agree to indemnify, + defend, and hold each Contributor harmless for any liability + incurred by, or claims asserted against, such Contributor by reason + of your accepting any such warranty or additional liability. diff --git a/third_party/robotiq-gripper-interface/NOTICE b/third_party/robotiq-gripper-interface/NOTICE new file mode 100644 index 0000000..616fc58 --- /dev/null +++ b/third_party/robotiq-gripper-interface/NOTICE @@ -0,0 +1 @@ +Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. diff --git a/third_party/robotiq-gripper-interface/README.md b/third_party/robotiq-gripper-interface/README.md new file mode 100644 index 0000000..9a8886c --- /dev/null +++ b/third_party/robotiq-gripper-interface/README.md @@ -0,0 +1,105 @@ +# Robotiq Gripper Interface + +This package provides a simplified interface to work with Robotiq grippers via the [MODBUS RTU serial protocol (Section 4.7)](https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf). + +## Limitations + +- Tested on the Robotiq 2F-85, untested on other grippers. +- Tested on Ubuntu 18.04 with a USB serial connection at `/dev/ttyUSB0`. + +## Organization + +- `examples` provides examples for using the interface. +- `include/robotiq` provides the interface headers for use with the compiled library. +- `src` is the source code that packs/unpacks the serial messages to the gripper in a friendly manner. + +## Needed hardware + +This method of interface requires the following hardware +- A Robotiq 2F-85 gripper +- An RS-485/USB connector interface, e.g. ACC-ADT-USB-RS485 (for more info see [Section 8 of the manual](https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf)). + +## Power on and connection + +- **IMPORTANT! BEFORE YOU BEGIN** Make sure the gripper is mounted to a solid surface and the nothing will impede the gripper motion as it will fully open and close when activated. Be sure to spend time understanding the robot behavior in an lab setting before using it to grasp items. +- The Robotiq 2F-85 ships with the following default serial configuration: +``` +Baud: 115200 +Byte size: 8 +Stop bits: 1 +Parity: None +Id: 09 +``` +- Power on the gripper. Plug in the USB cable to your computer. You can use the [Robotiq User Interface](https://robotiq.com/support) (currently Windows only) to make sure the connection is working before using this package. + +## Prerequisites + +[Option 1: Local] Install CMake, Boost >=1.65, and GTest +``` +# Install G++, CMake, Boost, and GTest +apt-get update +apt-get install \ + g++ \ + cmake \ + libboost-all-dev \ + libgtest-dev + +# Build and link GTest +cd /usr/src/gtest \ + && cmake . \ + && make \ + && cp *.a /usr/lib +``` + +[Option 2: Container] As an alternative, a Dockerfile is provided to test the package out without modifying your local environment. Assuming you have installed [Docker](https://www.docker.com/get-started), build and run the Dockerfile from the package source. **NOTE**: Change the `--device` argument to point to your device if different. +``` +docker build -t robotiq-gripper-interface . +docker run --rm -it --device=/dev/ttyUSB0 robotiq-gripper-interface:latest /bin/bash +cd robotiq-gripper-interface/ +``` + +## Build with Cmake + +C++ libraries are built using CMake. +``` +mkdir build && cd build +cmake .. +make && make test +``` +Run make install to make the library and header available to other packages. +``` +make install +``` +Return to the package root to run the examples +``` +cd .. +``` + +## Run the examples + +With the project built, and the gripper connection verified, you are now ready to run the examples. Ensure the gripper is mounted to a solid surface, the reach around the gripper is clear, and the power is within your immediate reach in case something goes wrong. +- The first demo illustrates gripper reset and activation. The gripper will open and close, after which it is ready for use. If the gripper is not activated it will not accept other commands +``` +bin/activate_gripper --port /dev/ttyUSB0 +``` +- The next demo opens the gripper. Note, it must be activated for this command to run succesfully. +``` +bin/open_gripper --port /dev/ttyUSB0 +``` +- The next demo closes the gripper. Note, it must be activated for this command to run succesfully. +``` +bin/close_gripper --port /dev/ttyUSB0 +``` +- The next demo commands the gripper position to a few values and displays the position feedback. +``` +bin/position_gripper --port /dev/ttyUSB0 +``` + +## Security + +See [CONTRIBUTING](CONTRIBUTING.md#security-issue-notifications) for more information. + +## License + +This project is licensed under the Apache-2.0 License. + diff --git a/third_party/robotiq-gripper-interface/cmake/RobotiqGripperInterfaceConfig.cmake.in b/third_party/robotiq-gripper-interface/cmake/RobotiqGripperInterfaceConfig.cmake.in new file mode 100644 index 0000000..54d8b2d --- /dev/null +++ b/third_party/robotiq-gripper-interface/cmake/RobotiqGripperInterfaceConfig.cmake.in @@ -0,0 +1,8 @@ +@PACKAGE_INIT@ + +include(CMakeFindDependencyMacro) + +# Dependencies (same syntax as find_package) +find_dependency(Boost 1.65.1 COMPONENTS system) + +include("${CMAKE_CURRENT_LIST_DIR}/RobotiqGripperInterfaceTargets.cmake") diff --git a/third_party/robotiq-gripper-interface/examples/activate_gripper/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/activate_gripper/CMakeLists.txt new file mode 100644 index 0000000..ec996ae --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/activate_gripper/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example activate_gripper) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/activate_gripper.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/activate_gripper/activate_gripper.cc b/third_party/robotiq-gripper-interface/examples/activate_gripper/activate_gripper.cc new file mode 100644 index 0000000..6409ac6 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/activate_gripper/activate_gripper.cc @@ -0,0 +1,54 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" + +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud) << "\n"; + + // Activate the gripper. That the gripper will not run activation if it is already + // reset, so we reset first. + std::cout << "Reset: " << gripper.reset() << "\n"; + std::cout << "Activate: " << gripper.activate() << "\n"; + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/examples/close_gripper/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/close_gripper/CMakeLists.txt new file mode 100644 index 0000000..e32a3f2 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/close_gripper/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example close_gripper) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/close_gripper.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/close_gripper/close_gripper.cc b/third_party/robotiq-gripper-interface/examples/close_gripper/close_gripper.cc new file mode 100644 index 0000000..ea02833 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/close_gripper/close_gripper.cc @@ -0,0 +1,59 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" + +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud) << "\n"; + + // Check if gripper is activated and activate otherwise + if (not gripper.is_activated()) { + std::cout << "Gripper is not activated... Activating..."; + gripper.activate(); + } + std::cout << "Gripper is activated!\n"; + + // Close the gripper + std::cout << "Close gripper: " << gripper.close_gripper() << "\n"; + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/examples/open_gripper/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/open_gripper/CMakeLists.txt new file mode 100644 index 0000000..332a04f --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/open_gripper/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example open_gripper) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/open_gripper.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/open_gripper/open_gripper.cc b/third_party/robotiq-gripper-interface/examples/open_gripper/open_gripper.cc new file mode 100644 index 0000000..df97c0e --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/open_gripper/open_gripper.cc @@ -0,0 +1,59 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" + +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud) << "\n"; + + // Check if gripper is activated and activate otherwise + if (not gripper.is_activated()) { + std::cout << "Gripper is not activated... Activating..."; + gripper.activate(); + } + std::cout << "Gripper is activated!\n"; + + // Open the gripper + std::cout << "Open gripper: " << gripper.open_gripper() << "\n"; + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/examples/position_gripper/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/position_gripper/CMakeLists.txt new file mode 100644 index 0000000..950bfd5 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/position_gripper/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example position_gripper) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/position_gripper.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/position_gripper/position_gripper.cc b/third_party/robotiq-gripper-interface/examples/position_gripper/position_gripper.cc new file mode 100644 index 0000000..f17d720 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/position_gripper/position_gripper.cc @@ -0,0 +1,78 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" + +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +// Define the scale factors. Here we map the position such that 0.086 is +// fully open and 0 is fully closed. +const double alpha = -0.086; +const double beta = 0.086; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +void print_feedback(const robotiq::GripperFeedback& feedback) { + std::cout << "Gripper feedback:\n"; + std::cout << " commanded_position: " << feedback.commanded_position << "\n"; + std::cout << " position: " << feedback.position << "\n"; + std::cout << " current: " << feedback.current << "\n"; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud, alpha, beta) << "\n"; + + // Check if gripper is activated and activate otherwise + if (not gripper.is_activated()) { + std::cout << "Gripper is not activated... Activating..."; + gripper.activate(); + } + std::cout << "Gripper is activated!\n"; + + // Set the gripper to several positions + std::cout << "Move to 0.043: " << gripper.set_gripper_position(0.043) << "\n"; + print_feedback(gripper.get_feedback()); + + std::cout << "Move to 0: " << gripper.set_gripper_position(0) << "\n"; + print_feedback(gripper.get_feedback()); + + std::cout << "Move to 0.086: " << gripper.set_gripper_position(0.086) << "\n"; + print_feedback(gripper.get_feedback()); + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/CMakeLists.txt new file mode 100644 index 0000000..a1144cb --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example read_write_simultaneous) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/read_write_simultaneous.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/read_write_simultaneous.cc b/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/read_write_simultaneous.cc new file mode 100644 index 0000000..9862f26 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/read_write_simultaneous/read_write_simultaneous.cc @@ -0,0 +1,101 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +// Define the scale factors. Here we map the position such that 100 is +// fully open and 0 is fully closed. +const double alpha = 100.0; +const double beta = 0.0; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +void print_feedback(const std::optional & feedback) { + if (!feedback) { + std::cout << "Feedback is empty, something went wrong!\n"; + return; + } + + std::cout << "\n\n"; + std::cout << "===============================\n"; + std::cout << "Gripper feedback:\n"; + std::cout << " commanded_position: " << feedback->commanded_position << "\n"; + std::cout << " position: " << feedback->position << "\n"; + std::cout << " current: " << feedback->current << "\n"; + std::cout << "===============================\n"; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud, alpha, beta) << "\n"; + + // Check if gripper is activated and activate otherwise + if (not gripper.is_activated()) { + std::cout << "Gripper is not activated... Activating..."; + gripper.activate(); + } + std::cout << "Gripper is activated!\n"; + + // Close gripper with no force + std::cout << "Move to 60% with no force\n"; + print_feedback(gripper.set_and_read_gripper(60.0, 50.0, 100)); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Grip at 50% + std::cout << "Grip 50% tighter\n"; + print_feedback(gripper.set_and_read_gripper(61.0, 50.0, 50)); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Grip tighter at 100% + std::cout << "Grip tightly\n"; + print_feedback(gripper.set_and_read_gripper(62.0, 50.0, 0.0)); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Relax it to 0% + std::cout << "Relax grip\n"; + print_feedback(gripper.set_and_read_gripper(61.0, 50.0, 100.0)); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + std::cout << "Open gripper" << gripper.set_gripper_position(0.0, 50.0, 100.0) << "\n"; + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/examples/speed_force_setting/CMakeLists.txt b/third_party/robotiq-gripper-interface/examples/speed_force_setting/CMakeLists.txt new file mode 100644 index 0000000..eb70744 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/speed_force_setting/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# ----------------------------------------------------------------------------- +# Demo target +# ----------------------------------------------------------------------------- +set(example speed_force_setting) + +include_directories( + ${CMAKE_CURRENT_SOURCE_DIR} +) + +set(srcs + ${CMAKE_CURRENT_SOURCE_DIR}/speed_force_setting.cc +) + +add_executable(${example} ${srcs}) + +add_dependencies(${example} + "robotiq-gripper-interface" +) + +target_link_libraries(${example} PRIVATE + "robotiq-gripper-interface" +) + +set_target_properties(${example} PROPERTIES + RUNTIME_OUTPUT_DIRECTORY ${CMAKE_SOURCE_DIR}/bin +) diff --git a/third_party/robotiq-gripper-interface/examples/speed_force_setting/speed_force_setting.cc b/third_party/robotiq-gripper-interface/examples/speed_force_setting/speed_force_setting.cc new file mode 100644 index 0000000..09ed687 --- /dev/null +++ b/third_party/robotiq-gripper-interface/examples/speed_force_setting/speed_force_setting.cc @@ -0,0 +1,98 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" +#include + +// Define the communication parameters +std::string port = robotiq::DEFAULT_PORT; +std::size_t baud = robotiq::DEFAULT_BAUD; + +// Define the scale factors. Here we map the position such that 100 is +// fully open and 0 is fully closed. +const double alpha = 100.0; +const double beta = 0.0; + +bool parse_args(int argc, char* argv[]) { + for (int i = 1; i < argc; i += 2) { + if (std::string(argv[i]) == "--help") { + std::cout << " --port Optional serial port ID\n"; + std::cout << " --baud Optional baud rate\n"; + return false; + } else if (std::string(argv[i]) == "--port") { + port = std::string(argv[i + 1]); + } else if (std::string(argv[i]) == "--baud") { + baud = std::atoi(argv[i + 1]); + } + } + return true; +} + +void print_feedback(const robotiq::GripperFeedback& feedback) { + std::cout << "\n\n"; + std::cout << "===============================\n"; + std::cout << "Gripper feedback:\n"; + std::cout << " commanded_position: " << feedback.commanded_position << "\n"; + std::cout << " position: " << feedback.position << "\n"; + std::cout << " current: " << feedback.current << "\n"; + std::cout << "===============================\n"; +} + +int main(int argc, char* argv[]) { + // Load the args + if (not parse_args(argc, argv)) { + return 0; + } + + // Open the serial port + robotiq::RobotiqGripperInterface gripper; + std::cout << "Connected: " << gripper.connect(port, baud, alpha, beta) << "\n"; + + // Check if gripper is activated and activate otherwise + if (not gripper.is_activated()) { + std::cout << "Gripper is not activated... Activating..."; + gripper.activate(); + } + std::cout << "Gripper is activated!\n"; + + // Close gripper with no force + std::cout << "Move to 60% with no force: " << gripper.set_gripper_position(60.0, 50.0, 100) << "\n"; + print_feedback(gripper.get_feedback()); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Grip at 50% + + std::cout << "Grip 50% tighter: " << gripper.set_gripper_position(61.0, 50.0, 50.0) << "\n"; + print_feedback(gripper.get_feedback()); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Grip tighter at 100% + std::cout << "Grip tightly " << gripper.set_gripper_position(62.0, 50.0, 0.0) << "\n"; + print_feedback(gripper.get_feedback()); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + // Hold the position but change the force. Relax it to 0% + std::cout << "Relax grip" << gripper.set_gripper_position(61.0, 50.0, 100.0) << "\n"; + print_feedback(gripper.get_feedback()); + std::cout << "\n\nPress Enter to continue...\n\n" << std::endl; + std::cin.get(); + + std::cout << "Open gripper" << gripper.set_gripper_position(0.0, 50.0, 100.0) << "\n"; + print_feedback(gripper.get_feedback()); + + return 0; +} diff --git a/third_party/robotiq-gripper-interface/include/robotiq/constants.h b/third_party/robotiq-gripper-interface/include/robotiq/constants.h new file mode 100644 index 0000000..62fd43e --- /dev/null +++ b/third_party/robotiq-gripper-interface/include/robotiq/constants.h @@ -0,0 +1,36 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +namespace robotiq { + +/** \brief Default port for MODBUS RTU (serial) communication, assumes Ubuntu */ +const std::string DEFAULT_PORT = "/dev/ttyUSB0"; + +/** \brief Default port for MODBUS RTU (serial) communication */ +const std::size_t DEFAULT_BAUD = 115200; + +/** \brief Default inactivity timeout*/ +const std::size_t DEFAULT_RECEIVE_TIMEOUT_MS = 200; + +/** \brief Default slope scale factor */ +const double DEFAULT_SCALE_ALPHA = 1; + +/** \brief Default zero crossing scale factor */ +const double DEFAULT_SCALE_BETA = 0; + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/include/robotiq/robotiq_gripper_interface.h b/third_party/robotiq-gripper-interface/include/robotiq/robotiq_gripper_interface.h new file mode 100644 index 0000000..967484e --- /dev/null +++ b/third_party/robotiq-gripper-interface/include/robotiq/robotiq_gripper_interface.h @@ -0,0 +1,183 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include +#include +#include +#include + +#include "robotiq/constants.h" +#include "robotiq/types.h" + +namespace robotiq { + +/** + * @brief This class is the simplified interface to the Robotiq Adaptive gripper. The + * Robotiq manual is available here: + * https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf + * + * Note that the class was tested with a 2F-85 2-finger gripper, Robotiq pinout to RS-485 + * board, an RS-485 serial to usb converter, and a Z6 workstation running Ubuntu 16.04. + */ +class RobotiqGripperInterface { + public: + explicit RobotiqGripperInterface(); + RobotiqGripperInterface(const RobotiqGripperInterface&) = delete; + RobotiqGripperInterface(RobotiqGripperInterface&&) = delete; + RobotiqGripperInterface& operator=(const RobotiqGripperInterface& other) = delete; + RobotiqGripperInterface& operator=(RobotiqGripperInterface&& other) = delete; + virtual ~RobotiqGripperInterface(); + + /** + * @brief Connects to the gripper over MODBUS RTU. This connection uses a serial port + * with RS-485. Note that the default port value assumes a Linux box with a serial to + * usb converter. + * + * The gripper can be accessed independently on a Windows machine using the Robotiq User + * Interface application. + * + * Effect of scale factors: + * $$y = (alpha / 255) * u + beta$$ + * $$u = (255 / alpha) * (y - beta)$$ + * where: + * $u$ is the commanded position message (uint8), range: [0, 255] + * $y$ is the scaled position value (double), range based on alpha, beta. + * + * Example 1: Default, y = [0: opened, 1: closed] + * alpha = 1, beta = 0 + * + * Example 2: y = [0.086: opened, 0: closed] + * alpha = -0.086, beta = 0.086 + * + * @param[in] port Serial port for communication (Ubuntu default: /dev/ttyUSB0) + * @param[in] baud Baud rate (default: 115200) + * @param[in] scale_alpha Linear slope factor for position scaling + * @param[in] scale_beta Linear zero crossing factor for position scaling + * @return True if succeeded. + */ + bool connect(const std::string& port = DEFAULT_PORT, std::size_t baud = DEFAULT_BAUD, + double scale_alpha = DEFAULT_SCALE_ALPHA, + double scale_beta = DEFAULT_SCALE_BETA); + + /** + * @brief Resets (deactivates) the gripper. + * + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool reset(bool blocking = true); + + /** + * @brief Activates the gripper, which will cause the gripper to move. + * + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool activate(bool blocking = true); + + /** + * @brief Checks if the gripper is activated. + * + * @return True if activated. + */ + bool is_activated(); + + /** + * @brief Closes the gripper until position reached or obstacle encountered. + * + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool close_gripper(bool blocking = true); + + /** + * @brief Opens the gripper until position reached or obstacle encountered. + * + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool open_gripper(bool blocking = true); + + /** + * @brief Sets the gripper position. + * + * @param[in] position Desired position, scaled by the scale factors + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool set_gripper_position(double position, bool blocking = true); + + /** + * @brief Sets the gripper position with speed and force + * + * @param[in] position Desired position, scaled by the scale factors + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + bool set_gripper_position(double position, double speed, double force, bool blocking = true); + + /** + * @brief Sets the gripper position with speed and force and immediate;y reads out the input registers. + * + * @param[in] position Desired position, scaled by the scale factors + * @param[in] speed Desired max speed, scaled by the scale factors + * @param[in] force Desired max force, scaled by the scale factors + * @param[in] blocking Waits to return until the gripper has completed the action. + * @return True if succeeded. + */ + std::optional set_and_read_gripper(double position, double speed, double force); + + /** + * @brief Returns the gripper feedback. + */ + GripperFeedback get_feedback(); + + /** + * @brief Sets the time out in ms for receiving messages from the gripper. + */ + void set_timeout(std::size_t timeout_ms); + + /** + * @brief Returns the time out in ms for receiving messages from the gripper. + */ + std::size_t get_timeout() const; + + private: + /** Writes the raw word (unscaled) to position at highest speed and force */ + bool set_raw_gripper_position(uint8_t position, bool blocking=true); + + /** Writes the raw word (unscaled) to position, speed and force*/ + bool set_raw_gripper_position(uint8_t position, uint8_t speed, uint8_t force, bool blocking); + + /** Transforms a sgtring from the input registers into a GripperFeedback */ + std::optional string_to_feedback(std::string const & r); + + /** Writes the raw word (unscaled) to position, speed and force and immediately return the GripperFeedback*/ + std::optional set_raw_gripper_position(uint8_t position, uint8_t speed, uint8_t force); + + /** Scales the raw word to position */ + double word_to_position(uint8_t word) const; + + /** Scales the position to raw word */ + uint8_t position_to_word(double position) const; + + // Pointer to implementation idiom is used to hide implementation from consumers + struct Implementation; + std::unique_ptr m_impl; +}; + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/include/robotiq/types.h b/third_party/robotiq-gripper-interface/include/robotiq/types.h new file mode 100644 index 0000000..8e2b9a6 --- /dev/null +++ b/third_party/robotiq-gripper-interface/include/robotiq/types.h @@ -0,0 +1,97 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include "constants.h" + +namespace robotiq { + +/* + * The Robotiq manual is available here: + * https://assets.robotiq.com/website-assets/support_documents/document/2F-85_2F-140_Instruction_Manual_e-Series_PDF_20190206.pdf + */ + +/** Basic status based on gACT, gGTO, and gSTA bits defined in 4.4 of the manual */ +enum BasicStatus { + NOT_CONNECTED, /** Returned if the gripper is not connected */ + RESET, /** Activation needs to be run */ + ACTIVATING, /** Activation is in progress */ + READY, /** Ready to receive a command */ + MOVING, /** In motion */ +}; + +/** Corresponds to gACT defined in 4.4 of the manual */ +enum ActivationStatus { + NOT_ACTIVATED, + ACTIVATED, +}; + +/** Corresponds to gGTO defined in 4.4 of the manual */ +enum ActionStatus { + STOPPED, + GOTO_POSITION, +}; + +/** Corresponds to gSTA defined in 4.4 of the manual */ +enum FingerStatus { + IN_RESET, + ACTIVATION_IN_PROGRESS, + ACTIVATION_COMPLETE, +}; + +/** Corresponds to gOBJ defined in 4.4 of the manual */ +enum ObjectStatus { + IN_MOTION, + STOPPED_WHILE_OPENING, /** Encountered an obstacle while opening */ + STOPPED_WHILE_CLOSING, /** Encountered an obstacle while closing */ + AT_REQUESTED_POSITION, /** Reach requested position - object may not be grasped */ +}; + +/** Corresponds to gFLT defined in 4.4 of the manual */ +enum FaultStatus { + NONE, + ACTION_DELAYED, + ACTIVATION_NEEDED, + MAX_TEMP_EXCEEDED, + COMM_TIMEOUT, + UNDER_VOLTAGE, + AUTOMATIC_RELEASE_IN_PROGRESS, + INTERNAL_FAULT, + ACTIVATION_FAULT, + OVERCURRENT, + AUTOMATIC_RELEASE_COMPLETED, + UNKNOWN, +}; + +/** Detailed status containing gACT, gGTO, gSTA, gOBJ, and gFLT */ +struct DetailedStatus { + ActivationStatus gact; + ActionStatus ggto; + FingerStatus gsta; + ObjectStatus gobj; + FaultStatus gflt; +}; + +/** Holds the gripper feedback */ +struct GripperFeedback { + double commanded_position{0}; /** Range determined by alpha, beta */ + double position{0}; /** Range determined by alpha, beta */ + double current{0}; /** Between 0 (min) and 1 (max) */ + uint8_t raw_commanded_position{0}; /** Between 0 (open) and 255 (closed) */ + uint8_t raw_position{0}; /** Between 0 (open) and 255 (closed) */ + DetailedStatus status; /** Detailed status returned by the gripper*/ +}; + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/src/helpers.cc b/third_party/robotiq-gripper-interface/src/helpers.cc new file mode 100644 index 0000000..3f4fad2 --- /dev/null +++ b/third_party/robotiq-gripper-interface/src/helpers.cc @@ -0,0 +1,103 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include + +#include "helpers.h" +#include "timeout_reader.h" + +namespace robotiq { + +std::string write_read(boost::asio::serial_port& serial, boost::asio::io_context& io_context, const std::string& message, + std::size_t timeout_ms) { + // write + write(serial, message); + + // read + TimeoutReader reader(serial, io_context, timeout_ms); + char c; + std::string result; + while (reader.read_char(c) && c != '\n') { + result += c; + } + return bin_to_hex(result); +} + +void write(boost::asio::serial_port& serial, const std::string& message) { + std::string binary_msg = hex_to_bin(message); + asio::write(serial, asio::buffer(binary_msg.c_str(), binary_msg.size())); +} + +std::string bin_to_hex(const std::string& input) { + const static std::string hex_codes = "0123456789ABCDEF"; + std::string hex_string; + for (std::size_t i = 0; i < input.length(); ++i) { + unsigned char bin_value = input[i]; + hex_string += hex_codes[(bin_value >> 4) & 0x0F]; + hex_string += hex_codes[bin_value & 0x0F]; + } + return hex_string; +} + +std::string hex_to_bin(const std::string& input) { + std::string bin_string; + for (std::size_t i = 0; i < input.length() - 1; i += 2) { + bin_string += (input[i] >= 'A' ? input[i] - 'A' + 10 : input[i] - '0') * 16 + + (input[i + 1] >= 'A' ? input[i + 1] - 'A' + 10 : input[i + 1] - '0'); + } + return bin_string; +} + +std::string uint8_to_hex(uint8_t value) { + std::stringstream stream; + stream << std::setfill('0') << std::setw(2) << std::hex << unsigned(value); + std::string str = stream.str(); + std::transform(str.begin(), str.end(), str.begin(), ::toupper); + return str; +} + +std::string crc16_modbus(const std::string& input) { + // Pad the input with zeros every other character to be able to use stringstream + std::string msg = ""; + for (std::size_t i = 0; i < input.size() / 2; ++i) { + msg += input.substr(2 * i, 2) + " "; + } + + // Convert the string to an array of bytes + std::istringstream istream(msg); + std::vector bytes; + unsigned int c; + while (istream >> std::hex >> c) { + bytes.push_back(c); + } + std::size_t const buffer_len = input.size() / 2; + unsigned char buffer[buffer_len]; + std::copy(bytes.begin(), bytes.end(), buffer); + + // Create the modbus crc computer and compute the crc + typedef boost::crc_optimal<16, 0x8005, 0xFFFF, 0, true, true> crc_modbus_type; + crc_modbus_type crc; + crc.process_bytes(buffer, buffer_len); + + // Return the hex string of the crc code + std::stringstream stream; + stream << std::setfill('0') << std::setw(4) << std::hex << ntohs(crc.checksum()); + std::string str = stream.str(); + std::transform(str.begin(), str.end(), str.begin(), ::toupper); + return str; +} + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/src/helpers.h b/third_party/robotiq-gripper-interface/src/helpers.h new file mode 100644 index 0000000..0a3a400 --- /dev/null +++ b/third_party/robotiq-gripper-interface/src/helpers.h @@ -0,0 +1,43 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include +#include + +#include + +namespace robotiq { + +/** Writes a message to the serial port and returns the received message */ +std::string write_read(boost::asio::serial_port& serial, boost::asio::io_context& io_context, const std::string& message, + std::size_t timeout_ms); + +/** Writes a message to the serial port and does not wait for a response*/ +void write(boost::asio::serial_port& serial, const std::string& message); + +/** Converts a binary string to a hexidecimal string */ +std::string bin_to_hex(const std::string& input); + +/** Converts a hexidecimal string to a binary string*/ +std::string hex_to_bin(const std::string& input); + +/** Converts an integer value to a fixed width hex string*/ +std::string uint8_to_hex(uint8_t value); + +/** Computes the modbus CRC (cyclic redundancy check) for a hexidecimal string*/ +std::string crc16_modbus(const std::string& input); + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc b/third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc new file mode 100644 index 0000000..eed3918 --- /dev/null +++ b/third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc @@ -0,0 +1,377 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "robotiq/robotiq_gripper_interface.h" +#include "helpers.h" + +#include +#include +#include +#include +#include + +#include +#include + +using namespace boost; + +namespace robotiq { + +// Messages for reading holding registers (FC03 from the manual) +//static std::string READ_FEEDBACK = "090307D00003040E"; + +/** + * From 2021 on, FC03 has been removed and only FC04 remains to read all inputs. + * Comment this line and uncomment the above one if you are using a model from before 2021. + */ +static std::string READ_FEEDBACK = "090407D00003B1CE"; + +// Messages for preseting multiple registers (FC16 from the manual) +static std::string PRESET_RESET = "091003E80003060000000000007330"; +static std::string PRESET_ACTIVATE = "091003E800030601000000000072E1"; + +// Position commands are prefixed by preset for multiple registers (FC16 from the manual) +// and a postfix to set max current and velocity. The word is ended with a CRC check on +// the unique message. +static std::string PRESET_POSITION_PREFIX = "091003E8000306090000"; + +// Preset for simultaneous read/write operations (F23) +static std::string PRESET_READ_WRITE_PREFIX = "091707D0000303E8000306090000"; + +// Expected response for preset messages +static std::string PRESET_RESPONSE = "091003E800030130"; + +struct RobotiqGripperInterface::Implementation { + Implementation(); + bool is_connected{false}; + boost::asio::io_context m_io_context; + asio::serial_port m_serial; + std::mutex m_serial_mutex; + std::size_t m_timeout_ms; + double m_scale_alpha; + double m_scale_beta; +}; + +RobotiqGripperInterface::Implementation::Implementation() + : m_io_context(), m_serial(m_io_context), m_timeout_ms{DEFAULT_RECEIVE_TIMEOUT_MS} {} + +RobotiqGripperInterface::RobotiqGripperInterface() + : m_impl{std::make_unique()} {} + +RobotiqGripperInterface::~RobotiqGripperInterface() { + if (m_impl->is_connected) { + m_impl->m_serial.close(); + } +} + +bool RobotiqGripperInterface::connect(const std::string& port, + std::size_t baud, + double scale_alpha, + double scale_beta) { + m_impl->m_scale_alpha = scale_alpha; + m_impl->m_scale_beta = scale_beta; + + if (m_impl->is_connected) { + m_impl->m_serial.close(); + } + + system::error_code error; + m_impl->m_serial.open(port, error); + if (error) { + std::cout << "RobotiqGripperInterface::connect failed with error: " << error.message() + << "\n"; + return m_impl->is_connected; + } + + m_impl->is_connected = true; + m_impl->m_serial.set_option(asio::serial_port_base::baud_rate(baud)); + m_impl->m_serial.set_option(asio::serial_port_base::character_size(8)); + m_impl->m_serial.set_option( + asio::serial_port_base::stop_bits(asio::serial_port_base::stop_bits::one)); + m_impl->m_serial.set_option( + asio::serial_port_base::parity(asio::serial_port_base::parity::none)); + return m_impl->is_connected; +} + +bool RobotiqGripperInterface::reset(bool blocking) { + if (not m_impl->is_connected) { + std::cout << "[RobotiqGripperInterface] Warning: reset() ignored since the gripper " + "is not connected\n"; + return m_impl->is_connected; + } + + std::string r; + { + std::lock_guard lock(m_impl->m_serial_mutex); + r = write_read(m_impl->m_serial, m_impl->m_io_context, PRESET_RESET, m_impl->m_timeout_ms); + } + if (r.compare(PRESET_RESPONSE) != 0) { + return false; + } + + if (blocking) { + bool done = false; + while (not done) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + GripperFeedback y = get_feedback(); + done = y.status.gact == ActivationStatus::NOT_ACTIVATED ? true : false; + } + } + + return true; +} + +bool RobotiqGripperInterface::is_activated() { + if (not m_impl->is_connected) { + std::cout + << "[RobotiqGripperInterface] Warning: activate() ignored since the gripper " + "is not connected\n"; + return m_impl->is_connected; + } + GripperFeedback y = get_feedback(); + return (y.status.gact == ActivationStatus::ACTIVATED); +} + +bool RobotiqGripperInterface::activate(bool blocking) { + if (not m_impl->is_connected) { + std::cout + << "[RobotiqGripperInterface] Warning: activate() ignored since the gripper " + "is not connected\n"; + return m_impl->is_connected; + } + + std::string r; + { + std::lock_guard lock(m_impl->m_serial_mutex); + r = write_read(m_impl->m_serial, m_impl->m_io_context, PRESET_ACTIVATE, m_impl->m_timeout_ms); + } + if (r.compare(PRESET_RESPONSE) != 0) { + return false; + } + + if (blocking) { + bool done = false; + while (not done) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + GripperFeedback y = get_feedback(); + done = y.status.gact == ActivationStatus::ACTIVATED ? true : false; + } + } + + // the activated flag seems to go high early + std::this_thread::sleep_for(std::chrono::milliseconds(2000)); + + return true; +} + +bool RobotiqGripperInterface::close_gripper(bool blocking) { + return set_raw_gripper_position(255, blocking); +} + +bool RobotiqGripperInterface::open_gripper(bool blocking) { + return set_raw_gripper_position(0, blocking); +} + +bool RobotiqGripperInterface::set_gripper_position(double position, bool blocking) { + return set_raw_gripper_position(position_to_word(position), blocking); +} + +bool RobotiqGripperInterface::set_gripper_position(double position, double speed, double force, bool blocking) { + return set_raw_gripper_position( + position_to_word(position), + position_to_word(speed), + position_to_word(force), + blocking + ); +} + +std::optional RobotiqGripperInterface::set_and_read_gripper(double position, double speed, double force) { + return set_raw_gripper_position( + position_to_word(position), + position_to_word(speed), + position_to_word(force) + ); +} + +std::optional RobotiqGripperInterface::string_to_feedback(std::string const & r) { + if (r.size() != 22) { + // std::cout << "[RobotiqGripperInterface] Warning: get_feedback() returned an " + // "unexpected number of bytes, consider increasing the timeout setting\n"; + return std::nullopt; + } + + GripperFeedback feedback; + // Note: bit masking is derived from the tables in Section 4.4 of the manual. + unsigned char byte0 = *hex_to_bin(r.substr(6, 2)).c_str(); + feedback.status.gobj = static_cast((byte0 & 0xC0) >> 6); // bits 7-6 + feedback.status.gsta = static_cast((byte0 & 0x30) >> 4); // bits 5-4 + feedback.status.ggto = static_cast((byte0 & 0x08) >> 3); // bits 3 + feedback.status.gact = static_cast((byte0 & 0x01) >> 0); // bits 0 + + unsigned byte2 = *hex_to_bin(r.substr(10, 2)).c_str(); + unsigned gflt = static_cast(byte2 & 0x0F); // bits 3-0 + switch (gflt) { + case 0: + feedback.status.gflt = FaultStatus::NONE; + break; + case 5: + feedback.status.gflt = FaultStatus::ACTION_DELAYED; + break; + case 7: + feedback.status.gflt = FaultStatus::ACTIVATION_NEEDED; + break; + case 8: + feedback.status.gflt = FaultStatus::MAX_TEMP_EXCEEDED; + break; + case 9: + feedback.status.gflt = FaultStatus::COMM_TIMEOUT; + break; + case 10: + feedback.status.gflt = FaultStatus::UNDER_VOLTAGE; + break; + case 11: + feedback.status.gflt = FaultStatus::AUTOMATIC_RELEASE_IN_PROGRESS; + break; + case 12: + feedback.status.gflt = FaultStatus::INTERNAL_FAULT; + break; + case 13: + feedback.status.gflt = FaultStatus::ACTIVATION_FAULT; + break; + case 14: + feedback.status.gflt = FaultStatus::OVERCURRENT; + break; + case 15: + feedback.status.gflt = FaultStatus::AUTOMATIC_RELEASE_COMPLETED; + break; + default: + feedback.status.gflt = FaultStatus::UNKNOWN; + } + + // Access the streaming feedback values + unsigned byte3 = *hex_to_bin(r.substr(12, 2)).c_str(); + uint8_t pos_cmd = static_cast(byte3 & 0xFF); + feedback.raw_commanded_position = pos_cmd; + feedback.commanded_position = word_to_position(pos_cmd); + + unsigned byte4 = *hex_to_bin(r.substr(14, 2)).c_str(); + uint8_t pos_fbk = static_cast(byte4 & 0xFF); + feedback.raw_position = pos_fbk; + feedback.position = word_to_position(pos_fbk); + + unsigned byte5 = *hex_to_bin(r.substr(16, 2)).c_str(); + unsigned cur_fbk = static_cast(byte5 & 0xFF); + feedback.current = static_cast(cur_fbk) / 255.0; + + return feedback; +} + +GripperFeedback RobotiqGripperInterface::get_feedback() { + if (not m_impl->is_connected) { + std::cout + << "[RobotiqGripperInterface] Warning: get_feedback() ignored since the gripper " + "is not connected\n"; + return GripperFeedback(); + } + + std::string response; + { + std::lock_guard lock(m_impl->m_serial_mutex); + response = write_read(m_impl->m_serial, m_impl->m_io_context, READ_FEEDBACK, m_impl->m_timeout_ms); + } + std::optional feedback = string_to_feedback(response); + if (!feedback) { + return GripperFeedback(); + } + + return *feedback; +} + +void RobotiqGripperInterface::set_timeout(std::size_t timeout_ms) { + m_impl->m_timeout_ms = timeout_ms; +} + +std::size_t RobotiqGripperInterface::get_timeout() const { return m_impl->m_timeout_ms; } + +bool RobotiqGripperInterface::set_raw_gripper_position(uint8_t position, bool blocking) { + return set_raw_gripper_position(position, 0xFF, 0xFF, blocking); +} + +bool RobotiqGripperInterface::set_raw_gripper_position(uint8_t position, uint8_t speed, uint8_t force, bool blocking) { + if (not m_impl->is_connected) { + std::cout << "[RobotiqGripperInterface] Warning: set_raw_gripper_position() ignored " + "since the gripper is not connected\n"; + return m_impl->is_connected; + } + + // Create the message and append the modbus CRC check + std::string message = + PRESET_POSITION_PREFIX + uint8_to_hex(position) + uint8_to_hex(speed) + uint8_to_hex(force); + message += crc16_modbus(message); + + if (blocking) { + std::string r; + { + std::lock_guard lock(m_impl->m_serial_mutex); + r = write_read(m_impl->m_serial, m_impl->m_io_context, message, m_impl->m_timeout_ms); + } + if (r.compare(PRESET_RESPONSE) != 0) { + return false; + } + + bool done = false; + while (not done) { + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + GripperFeedback y = get_feedback(); + done = y.status.gobj == ObjectStatus::IN_MOTION ? false : true; + } + } else { + std::lock_guard lock(m_impl->m_serial_mutex); + write(m_impl->m_serial, message); + } + + return true; +} + +std::optional RobotiqGripperInterface::set_raw_gripper_position(uint8_t position, uint8_t speed, uint8_t force) { + if (not m_impl->is_connected) { + std::cout << "[RobotiqGripperInterface] Warning: set_raw_gripper_position() ignored " + "since the gripper is not connected\n"; + return std::nullopt; + } + + // Create the message and append the modbus CRC check + std::string message = PRESET_READ_WRITE_PREFIX + uint8_to_hex(position) + uint8_to_hex(speed) + uint8_to_hex(force); + message += crc16_modbus(message); + + std::string response; + { + std::lock_guard lock(m_impl->m_serial_mutex); + response = write_read(m_impl->m_serial, m_impl->m_io_context, message, m_impl->m_timeout_ms); + } + return string_to_feedback(response); +} + +double RobotiqGripperInterface::word_to_position(uint8_t word) const { + return (m_impl->m_scale_alpha / 255.0) * static_cast(word) + m_impl->m_scale_beta; +} + +uint8_t RobotiqGripperInterface::position_to_word(double position) const { + double scaled_position = 255.0 / m_impl->m_scale_alpha * (position - m_impl->m_scale_beta); + scaled_position = std::max(scaled_position, 0.0); + scaled_position = std::min(scaled_position, 255.0); + return static_cast(scaled_position); +} + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/src/timeout_reader.cc b/third_party/robotiq-gripper-interface/src/timeout_reader.cc new file mode 100644 index 0000000..61845fd --- /dev/null +++ b/third_party/robotiq-gripper-interface/src/timeout_reader.cc @@ -0,0 +1,69 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include "timeout_reader.h" + +#include + +namespace robotiq { + +TimeoutReader::TimeoutReader(asio::serial_port& serial, boost::asio::io_context& io_context, std::size_t timeout_ms) + : + m_io_context(io_context), + m_serial(serial), + m_timeout_ms(timeout_ms), + m_timer(io_context), + m_read_error{true} {} + +bool TimeoutReader::read_char(char& c) { + c = '\0'; + + // After a timeout & cancel it seems we need + // to do a reset for subsequent reads to work. + m_io_context.reset(); + + // Asynchronously read 1 character. + boost::asio::async_read( + m_serial, asio::buffer(&c, 1), + bind(&TimeoutReader::read_complete, this, asio::placeholders::error, + asio::placeholders::bytes_transferred)); + + // Setup a deadline time to implement our timeout. + m_timer.expires_from_now(posix_time::milliseconds(m_timeout_ms)); + m_timer.async_wait(bind(&TimeoutReader::timeout, this, asio::placeholders::error)); + + // This will block until a character is read + // or until it is cancelled. + m_io_context.run(); + + if (m_read_error) { + c = '\0'; + } + + return not m_read_error; +} + +void TimeoutReader::read_complete(const system::error_code& error, std::size_t bytes) { + m_read_error = (error || bytes == 0); + m_timer.cancel(); +} + +void TimeoutReader::timeout(const system::error_code& error) { + if (error) { + return; + } + m_serial.cancel(); +} + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/src/timeout_reader.h b/third_party/robotiq-gripper-interface/src/timeout_reader.h new file mode 100644 index 0000000..f30725a --- /dev/null +++ b/third_party/robotiq-gripper-interface/src/timeout_reader.h @@ -0,0 +1,41 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#pragma once + +#include + +#include + +using namespace boost; + +namespace robotiq { + +/** Reads serial messages with a dedicated timeout */ +class TimeoutReader { + public: + TimeoutReader(asio::serial_port& serial, boost::asio::io_context& io_context, std::size_t timeout_ms); + bool read_char(char& c); + + private: + void read_complete(const system::error_code& error, std::size_t bytes); + void timeout(const system::error_code& error); + asio::io_context& m_io_context; + asio::serial_port& m_serial; + std::size_t m_timeout_ms; + asio::deadline_timer m_timer; + bool m_read_error; +}; + +} // namespace robotiq diff --git a/third_party/robotiq-gripper-interface/tests/CMakeLists.txt b/third_party/robotiq-gripper-interface/tests/CMakeLists.txt new file mode 100644 index 0000000..a170130 --- /dev/null +++ b/third_party/robotiq-gripper-interface/tests/CMakeLists.txt @@ -0,0 +1,40 @@ +# Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +# +# Licensed under the Apache License, Version 2.0 (the "License"). +# You may not use this file except in compliance with the License. +# You may obtain a copy of the License at +# +# http://www.apache.org/licenses/LICENSE-2.0 +# +# Unless required by applicable law or agreed to in writing, software +# distributed under the License is distributed on an "AS IS" BASIS, +# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +# See the License for the specific language governing permissions and +# limitations under the License. + +# Set the library name +set(test rai_robotiq_tests) + +# Set the test file names +set(test_srcs + ${CMAKE_CURRENT_SOURCE_DIR}/test_helpers.cc +) + +# ----------------------------------------------------------------------------- +# Test target +# ----------------------------------------------------------------------------- +add_executable(${test} ${test_srcs}) + +add_test(${test} ${test}) + +add_dependencies(${test} + "robotiq-gripper-interface" +) + +target_link_libraries(${test} PRIVATE + "robotiq-gripper-interface" + ${GTEST_LIBRARIES} + ${GTEST_MAIN_LIBRARIES} + ${Boost_LIBRARIES} + pthread # This needs to come after gtest libs +) diff --git a/third_party/robotiq-gripper-interface/tests/test_helpers.cc b/third_party/robotiq-gripper-interface/tests/test_helpers.cc new file mode 100644 index 0000000..7975e36 --- /dev/null +++ b/third_party/robotiq-gripper-interface/tests/test_helpers.cc @@ -0,0 +1,27 @@ +// Copyright Amazon.com, Inc. or its affiliates. All Rights Reserved. +// +// Licensed under the Apache License, Version 2.0 (the "License"). +// You may not use this file except in compliance with the License. +// You may obtain a copy of the License at +// +// http://www.apache.org/licenses/LICENSE-2.0 +// +// Unless required by applicable law or agreed to in writing, software +// distributed under the License is distributed on an "AS IS" BASIS, +// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied. +// See the License for the specific language governing permissions and +// limitations under the License. + +#include + +#include "src/helpers.h" + +TEST(helpers, uint8_to_hex) { + EXPECT_EQ(robotiq::uint8_to_hex(0), "00"); + EXPECT_EQ(robotiq::uint8_to_hex(255), "FF"); +} + +TEST(helpers, crc_checks) { + EXPECT_EQ(robotiq::crc16_modbus("091003E8000306090000FFFFFF"), "4229"); + EXPECT_EQ(robotiq::crc16_modbus("091003E800030609000000FFFF"), "7219"); +}