From add307e1e010fee9aac6823c483826fb23eedc73 Mon Sep 17 00:00:00 2001 From: irl Date: Thu, 29 Jan 2026 12:48:50 +0100 Subject: [PATCH 01/19] add move to joint pose --- CMakeLists.txt | 2 +- config/ProxyConfig.yaml | 2 +- config/RobotArmConfig.yaml | 2 +- config/SafetyLimitConfig.cfg | 4 +- .../hybrid_joint_impedance_controller.cfg | 20 +- .../control_mode/abstract_control_mode.hpp | 49 ++- include/franka_arm_proxy.hpp | 5 +- local_test.bash | 4 +- models/franka_emika_panda/panda_arm.urdf | 16 + .../robotiq_arg2f_85_model.urdf | 328 ++++++++++++++++++ real_robot_test.bash | 6 + scripts/compile.sh | 3 +- src/control_mode/abstract_control_mode.cpp | 153 ++++++++ src/franka_arm_proxy.cpp | 34 +- 14 files changed, 612 insertions(+), 16 deletions(-) create mode 100644 models/franka_emika_panda/robotiq_arg2f_85_model.urdf create mode 100755 real_robot_test.bash diff --git a/CMakeLists.txt b/CMakeLists.txt index 6fdaffa..1cf338c 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") diff --git a/config/ProxyConfig.yaml b/config/ProxyConfig.yaml index 7c766eb..7aad1d8 100644 --- a/config/ProxyConfig.yaml +++ b/config/ProxyConfig.yaml @@ -1,7 +1,7 @@ # Franka Arm Configuration for P10 node_name: FrankaPanda robot_ip: 10.10.10.10 -proxy_ip: 127.0.0.1 +proxy_ip: 192.168.0.117 arm_config_path: config/RobotArmConfig.yaml gripper_config_path: config/GripperConfig.yaml \ No newline at end of file diff --git a/config/RobotArmConfig.yaml b/config/RobotArmConfig.yaml index 94b473c..c425ff4 100644 --- a/config/RobotArmConfig.yaml +++ b/config/RobotArmConfig.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/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/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index ce6d269..45a1a54 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,9 +1,19 @@ name: HybridJointImpedance command_topic: FrankaPanda/joint_position_command -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 +kq_gains: [40, 50, 50, 40, 35, 20, 10] +kqd_gains: [1.0, 1.5, 1.0, 1.5, 0.5, 0.5, 0.5] +kx_gains: [35, 20, 35, 14, 14, 14] +kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] + +ignore_gravity: true # no use for now +# 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] + +# 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 \ No newline at end of file diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 6f715c0..618c222 100644 --- a/include/control_mode/abstract_control_mode.hpp +++ b/include/control_mode/abstract_control_mode.hpp @@ -67,7 +67,8 @@ class AbstractControlMode void stopControl(); const std::string getModeName(); void controlTask(); - + bool moveToJointPosition(const std::array& target_q, + double max_velocity = 0.1, double tolerance = 1e-2); protected: AbstractControlMode(const SafetyLimitConfig& safety_config) : safety_config_(safety_config) {} FrankaPanda* robot_; @@ -94,3 +95,49 @@ class AbstractControlMode double margin, double k); std::unordered_map active_constraints_map_; }; +class MotionGenerator +{ + public: + /** + * Creates a new MotionGenerator instance for a target q. + * + * @param[in] speed_factor General speed factor in range [0, 1]. + * @param[in] q_goal Target joint positions. + */ + MotionGenerator(double speed_factor, const std::array& q_goal); + + /** + * 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(); +}; diff --git a/include/franka_arm_proxy.hpp b/include/franka_arm_proxy.hpp index 153d3bf..5656542 100644 --- a/include/franka_arm_proxy.hpp +++ b/include/franka_arm_proxy.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" @@ -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/local_test.bash b/local_test.bash index 64fe9a7..3daa2c1 100755 --- 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_arm.urdf b/models/franka_emika_panda/panda_arm.urdf index db9e366..85d834a 100644 --- a/models/franka_emika_panda/panda_arm.urdf +++ b/models/franka_emika_panda/panda_arm.urdf @@ -228,4 +228,20 @@ + + + + + + + + + + + + + diff --git a/models/franka_emika_panda/robotiq_arg2f_85_model.urdf b/models/franka_emika_panda/robotiq_arg2f_85_model.urdf new file mode 100644 index 0000000..69027d2 --- /dev/null +++ b/models/franka_emika_panda/robotiq_arg2f_85_model.urdf @@ -0,0 +1,328 @@ + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + transmission_interface/SimpleTransmission + + PositionJointInterface + + + 1 + + + + diff --git a/real_robot_test.bash b/real_robot_test.bash new file mode 100755 index 0000000..8094b96 --- /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/ProxyConfig.yaml \ No newline at end of file diff --git a/scripts/compile.sh b/scripts/compile.sh index 6c44187..ce990d7 100755 --- 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/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index a685ec6..c7907b5 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -1,5 +1,14 @@ #include "control_mode/abstract_control_mode.hpp" +#include +#include +#include + +#include +#include + +#include +#include #include void ControllerConfig::readBaseConfig(const ConfigFileReader& reader) @@ -91,6 +100,150 @@ void AbstractControlMode::controlTask() } zlc::info("[{}] Control thread ended.", getModeName()); } +MotionGenerator::MotionGenerator(double speed_factor, const std::array& q_goal) + : q_goal_(q_goal.data()) { + 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 MotionGenerator::operator()(const franka::RobotState& robot_state, + franka::Duration period) { + time_ += period.toSec(); + + if (time_ == 0.0) { + q_start_ = MotionGenerator::Vector7d(robot_state.q.data()); + delta_q_ = q_goal_ - q_start_; + calculateSynchronizedValues(); + } + + MotionGenerator::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 MotionGenerator::calculateDesiredValues(double time, MotionGenerator::Vector7d* delta_q_d) const { + MotionGenerator::Vector7i sign_delta_q; + sign_delta_q << delta_q_.cwiseSign().cast(); + MotionGenerator::Vector7d t_d = t_2_sync_ - t_1_sync_; + MotionGenerator::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 MotionGenerator::calculateSynchronizedValues() { + MotionGenerator::Vector7d dq_max_reach(dq_max_); + MotionGenerator::Vector7d t_f = MotionGenerator::Vector7d::Zero(); + MotionGenerator::Vector7d delta_t_2 = MotionGenerator::Vector7d::Zero(); + MotionGenerator::Vector7d t_1 = MotionGenerator::Vector7d::Zero(); + MotionGenerator::Vector7d delta_t_2_sync = MotionGenerator::Vector7d::Zero(); + MotionGenerator::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]); + } + } +} +bool AbstractControlMode::moveToJointPosition(const std::array& target_q, + double max_velocity, double tolerance) +{ +#if NO_ROBOT_TESTING + zlc::error("[{}] moveToJointPosition is not supported in NO_ROBOT_TESTING mode.", + getModeName()); + return false; +#else + 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; + } + 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}}); + MotionGenerator motion_generator(max_velocity, target_q); + robot_->control(motion_generator); + }catch (const franka::Exception& e) { + std::cout << e.what() << std::endl; + return -1; + } + return true; +#endif +} + bool AbstractControlMode::tryRecovery(int max_attempts) { diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index da4c88d..ec398aa 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -1,6 +1,8 @@ #include "franka_arm_proxy.hpp" +#include + static std::atomic running_flag{true}; // let ctrl-c stop the server static void signalHandler(int signum) { @@ -50,8 +52,14 @@ void FrankaArmProxy::initializeService() 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); + service_namespace = + fmt::format("{}/move_franka_arm_to_joint_position", config_.name); + zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::moveFrankaArmToJointPosition, + this); + service_namespace = + fmt::format("{}/move_franka_arm_to_cartesian_position", config_.name); + // zlc::registerServiceHandler(service_namespace, + // &FrankaArmProxy::moveFrankaArmToCartesianPosition, this); } FrankaArmProxy::~FrankaArmProxy() @@ -155,3 +163,25 @@ std::string FrankaArmProxy::getFrankaArmControlMode(const zlc::Empty&) } return current_control_mode_->getModeName(); } + +std::pair> FrankaArmProxy::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), + {}}; +} From 2f827d9cb41831f774e8b1fc0352e67809cce9c8 Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Thu, 29 Jan 2026 14:57:01 +0100 Subject: [PATCH 02/19] add exception during robot init&add franka robotiq urdf issue:need to fix computeJacobian --- .../hybrid_joint_impedance_controller.cfg | 14 +- include/protocol/msg.hpp | 11 +- .../franka_emika_panda/panda_robotiq_85.urdf | 524 ++++++++++++++++++ src/franka_arm_proxy.cpp | 30 +- src/utils/robot_model.cpp | 40 +- 5 files changed, 606 insertions(+), 13 deletions(-) create mode 100644 models/franka_emika_panda/panda_robotiq_85.urdf diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 45a1a54..6dde1cc 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,12 +1,16 @@ 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: [40, 50, 50, 40, 35, 20, 10] +# kqd_gains: [1.0, 1.5, 1.0, 1.5, 0.5, 0.5, 0.5] +# kx_gains: [35, 20, 35, 14, 14, 14] +# kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] -kq_gains: [40, 50, 50, 40, 35, 20, 10] -kqd_gains: [1.0, 1.5, 1.0, 1.5, 0.5, 0.5, 0.5] -kx_gains: [35, 20, 35, 14, 14, 14] -kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] - ignore_gravity: true # no use for now # 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] diff --git a/include/protocol/msg.hpp b/include/protocol/msg.hpp index 359bd99..f738aed 100644 --- a/include/protocol/msg.hpp +++ b/include/protocol/msg.hpp @@ -20,12 +20,17 @@ 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 @@ -57,4 +62,4 @@ struct FrankaResponseMsg std::string code; std::vector payload; MSGPACK_DEFINE_MAP(code, payload); -}; +}; \ 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/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index ec398aa..a55825e 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -37,6 +37,15 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) { // 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_); @@ -73,13 +82,28 @@ void FrankaArmProxy::initRobot() try { robot_ = std::make_unique(config_.robot_ip); - model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", - "panda_link8"); + // model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", + // "panda_link8"); + model_ = std::make_unique("./models/franka_emika_panda/panda_robotiq_85.urdf", + "panda_joint_ee"); } catch (const franka::NetworkException& e) { - zlc::error("{}", e.what()); + 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(&FrankaArmProxy::statePublishThread, this); diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index 4029029..47a43ff 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -1,4 +1,3 @@ - #include "utils/robot_model.hpp" #include "pinocchio/algorithm/frames.hpp" @@ -8,16 +7,33 @@ #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/parsers/urdf.hpp" +#include +#include PandaPinocchioModel::PandaPinocchioModel(std::string urdf_filename, std::string ee_joint_name) { ee_joint_name_ = std::move(ee_joint_name); std::ifstream stream(urdf_filename); + if (!stream.is_open()) { + throw std::runtime_error("Cannot open URDF file: " + urdf_filename); + } + xml_buffer_ = std::string((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); pinocchio::urdf::buildModelFromXML(xml_buffer_, model_); model_data_ = pinocchio::Data(model_); + + // Check if the frame exists + if (!model_.existFrame(ee_joint_name_)) { + std::cerr << "Frame '" << ee_joint_name_ << "' not found in model!" << std::endl; + std::cerr << "Available frames:" << std::endl; + for (size_t i = 0; i < model_.nframes; ++i) { + std::cerr << " " << i << ": " << model_.frames[i].name << std::endl; + } + throw std::runtime_error("Frame '" + ee_joint_name_ + "' not found in URDF model"); + } + ee_link_idx_ = model_.getFrameId(ee_joint_name_); } @@ -110,7 +126,27 @@ JointPosition PandaPinocchioModel::inverseDynamics(JointPosition joint_position, JointVelocity joint_velocity, JointAcceleration joint_acceleration) { - return pinocchio::rnea(model_, model_data_, joint_position, joint_velocity, joint_acceleration); + // If model has more than 7 DOF (e.g., with gripper), use only first 7 + if (model_.nv > 7) + { + // Pad joint position, velocity, and acceleration to match model DOF + Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_.nv); + Eigen::VectorXd dq_full = Eigen::VectorXd::Zero(model_.nv); + Eigen::VectorXd ddq_full = Eigen::VectorXd::Zero(model_.nv); + + q_full.head(7) = joint_position; + dq_full.head(7) = joint_velocity; + ddq_full.head(7) = joint_acceleration; + + Eigen::VectorXd tau_full = pinocchio::rnea(model_, model_data_, q_full, dq_full, ddq_full); + + // Return only first 7 elements + return tau_full.head(7); + } + else + { + return pinocchio::rnea(model_, model_data_, joint_position, joint_velocity, joint_acceleration); + } } // JointPosition PandaPinocchioModel::coriolis(JointPosition joint_position, From 7ae366b5f240de585747f892852ee5e2a469e0e7 Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Thu, 29 Jan 2026 18:01:14 +0100 Subject: [PATCH 03/19] add gravity ignore & change back to official urdf --- models/franka_emika_panda/panda_arm.urdf | 16 - .../robotiq_arg2f_85_model.urdf | 328 ------------------ .../hybrid_joint_impedance_control.cpp | 2 + src/franka_arm_proxy.cpp | 6 +- src/utils/robot_model.cpp | 42 +-- 5 files changed, 7 insertions(+), 387 deletions(-) delete mode 100644 models/franka_emika_panda/robotiq_arg2f_85_model.urdf diff --git a/models/franka_emika_panda/panda_arm.urdf b/models/franka_emika_panda/panda_arm.urdf index 85d834a..db9e366 100644 --- a/models/franka_emika_panda/panda_arm.urdf +++ b/models/franka_emika_panda/panda_arm.urdf @@ -228,20 +228,4 @@ - - - - - - - - - - - - - diff --git a/models/franka_emika_panda/robotiq_arg2f_85_model.urdf b/models/franka_emika_panda/robotiq_arg2f_85_model.urdf deleted file mode 100644 index 69027d2..0000000 --- a/models/franka_emika_panda/robotiq_arg2f_85_model.urdf +++ /dev/null @@ -1,328 +0,0 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - transmission_interface/SimpleTransmission - - PositionJointInterface - - - 1 - - - - diff --git a/src/control_mode/hybrid_joint_impedance_control.cpp b/src/control_mode/hybrid_joint_impedance_control.cpp index 5c5e944..a3cbe73 100644 --- a/src/control_mode/hybrid_joint_impedance_control.cpp +++ b/src/control_mode/hybrid_joint_impedance_control.cpp @@ -42,6 +42,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/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index a55825e..f7680a8 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -82,10 +82,8 @@ void FrankaArmProxy::initRobot() try { robot_ = std::make_unique(config_.robot_ip); - // model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", - // "panda_link8"); - model_ = std::make_unique("./models/franka_emika_panda/panda_robotiq_85.urdf", - "panda_joint_ee"); + model_ = std::make_unique("./models/franka_emika_panda/panda_arm.urdf", + "panda_link8"); } catch (const franka::NetworkException& e) { diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index 47a43ff..c3bced0 100644 --- a/src/utils/robot_model.cpp +++ b/src/utils/robot_model.cpp @@ -1,3 +1,4 @@ + #include "utils/robot_model.hpp" #include "pinocchio/algorithm/frames.hpp" @@ -7,33 +8,16 @@ #include "pinocchio/algorithm/rnea.hpp" #include "pinocchio/parsers/sample-models.hpp" #include "pinocchio/parsers/urdf.hpp" -#include -#include PandaPinocchioModel::PandaPinocchioModel(std::string urdf_filename, std::string ee_joint_name) { ee_joint_name_ = std::move(ee_joint_name); std::ifstream stream(urdf_filename); - if (!stream.is_open()) { - throw std::runtime_error("Cannot open URDF file: " + urdf_filename); - } - xml_buffer_ = std::string((std::istreambuf_iterator(stream)), std::istreambuf_iterator()); pinocchio::urdf::buildModelFromXML(xml_buffer_, model_); model_data_ = pinocchio::Data(model_); - - // Check if the frame exists - if (!model_.existFrame(ee_joint_name_)) { - std::cerr << "Frame '" << ee_joint_name_ << "' not found in model!" << std::endl; - std::cerr << "Available frames:" << std::endl; - for (size_t i = 0; i < model_.nframes; ++i) { - std::cerr << " " << i << ": " << model_.frames[i].name << std::endl; - } - throw std::runtime_error("Frame '" + ee_joint_name_ + "' not found in URDF model"); - } - ee_link_idx_ = model_.getFrameId(ee_joint_name_); } @@ -126,27 +110,7 @@ JointPosition PandaPinocchioModel::inverseDynamics(JointPosition joint_position, JointVelocity joint_velocity, JointAcceleration joint_acceleration) { - // If model has more than 7 DOF (e.g., with gripper), use only first 7 - if (model_.nv > 7) - { - // Pad joint position, velocity, and acceleration to match model DOF - Eigen::VectorXd q_full = Eigen::VectorXd::Zero(model_.nv); - Eigen::VectorXd dq_full = Eigen::VectorXd::Zero(model_.nv); - Eigen::VectorXd ddq_full = Eigen::VectorXd::Zero(model_.nv); - - q_full.head(7) = joint_position; - dq_full.head(7) = joint_velocity; - ddq_full.head(7) = joint_acceleration; - - Eigen::VectorXd tau_full = pinocchio::rnea(model_, model_data_, q_full, dq_full, ddq_full); - - // Return only first 7 elements - return tau_full.head(7); - } - else - { - return pinocchio::rnea(model_, model_data_, joint_position, joint_velocity, joint_acceleration); - } + return pinocchio::rnea(model_, model_data_, joint_position, joint_velocity, joint_acceleration); } // JointPosition PandaPinocchioModel::coriolis(JointPosition joint_position, @@ -164,4 +128,4 @@ JointPosition PandaPinocchioModel::inverseDynamics(JointPosition joint_position, // JointPosition result{}; // result = g; // return result; -// } +// } \ No newline at end of file From 56b62bb882bb8255160329d1b8c41052529be2e0 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Thu, 29 Jan 2026 18:28:02 +0100 Subject: [PATCH 04/19] using namespace for zlc and clean the motion generator --- .../control_mode/abstract_control_mode.hpp | 50 +-- .../cartesian_pose_motion_generator.hpp | 97 +++++ .../joint_position_motion_generator.hpp | 54 +++ include/debugger/fake_franka.hpp | 196 +++++----- include/debugger/state_debug.hpp | 18 +- include/mujoco_sim/mujoco_robot.hpp | 38 ++ src/control_mode/abstract_control_mode.cpp | 156 +++----- .../cartesian_pose_motion_generator.cpp | 288 +++++++++++++++ .../joint_position_motion_generator.cpp | 115 ++++++ src/franka_arm_proxy.cpp | 23 +- src/main.cpp | 2 +- src/mujoco_sim/mujoco_robot.cpp | 339 +++++++++++++++++- 12 files changed, 1084 insertions(+), 292 deletions(-) create mode 100644 include/control_mode/cartesian_pose_motion_generator.hpp create mode 100644 include/control_mode/joint_position_motion_generator.hpp create mode 100644 src/control_mode/cartesian_pose_motion_generator.cpp create mode 100644 src/control_mode/joint_position_motion_generator.cpp diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 618c222..7e89b49 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 @@ -69,6 +70,9 @@ class AbstractControlMode 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_; @@ -95,49 +99,3 @@ class AbstractControlMode double margin, double k); std::unordered_map active_constraints_map_; }; -class MotionGenerator -{ - public: - /** - * Creates a new MotionGenerator instance for a target q. - * - * @param[in] speed_factor General speed factor in range [0, 1]. - * @param[in] q_goal Target joint positions. - */ - MotionGenerator(double speed_factor, const std::array& q_goal); - - /** - * 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(); -}; diff --git a/include/control_mode/cartesian_pose_motion_generator.hpp b/include/control_mode/cartesian_pose_motion_generator.hpp new file mode 100644 index 0000000..53ec6ce --- /dev/null +++ b/include/control_mode/cartesian_pose_motion_generator.hpp @@ -0,0 +1,97 @@ +#pragma once + +#include +#include +#include +#include +#include + +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] 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, + 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_; +}; diff --git a/include/control_mode/joint_position_motion_generator.hpp b/include/control_mode/joint_position_motion_generator.hpp new file mode 100644 index 0000000..88af369 --- /dev/null +++ b/include/control_mode/joint_position_motion_generator.hpp @@ -0,0 +1,54 @@ +#pragma once + +#include +#include +#include +#include +#include + +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. + */ + JointPositionMotionGenerator(double speed_factor, const std::array& q_goal); + + /** + * 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(); +}; diff --git a/include/debugger/fake_franka.hpp b/include/debugger/fake_franka.hpp index df59c42..2d7f87e 100644 --- a/include/debugger/fake_franka.hpp +++ b/include/debugger/fake_franka.hpp @@ -1,109 +1,109 @@ -#include +// #include -#include -#include -#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 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."); - } +// class FakeFrankaRobot +// { +// public: +// FakeFrankaRobot() = delete; +// FakeFrankaRobot(const std::string&) +// { +// zlc::info("Initialized FakeFrankaRobot for testing purposes."); +// } - ~FakeFrankaRobot() - { - zlc::info("Destroyed FakeFrankaRobot."); - } +// ~FakeFrankaRobot() +// { +// zlc::info("Destroyed FakeFrankaRobot."); +// } - void automaticErrorRecovery() - { - zlc::info("FakeFrankaRobot: automaticErrorRecovery called."); - } +// 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, 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(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 +// 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_; - } +// franka::RobotState readOnce() +// { +// if (isOnControlLoop) +// { +// throw std::runtime_error("Cannot read state while on control loop."); +// } +// return current_state_; +// } - FakeFrankaModel loadModel() - { - return FakeFrankaModel(); - } +// FakeFrankaModel loadModel() +// { +// return FakeFrankaModel(); +// } - private: - std::atomic isOnControlLoop = false; - franka::RobotState current_state_; -}; \ No newline at end of file +// 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 index 8d2cd0b..24812ef 100644 --- a/include/debugger/state_debug.hpp +++ b/include/debugger/state_debug.hpp @@ -1,11 +1,11 @@ -#ifndef STATE_DEBUG_HPP -#define STATE_DEBUG_HPP -#include -#include +// #ifndef STATE_DEBUG_HPP +// #define STATE_DEBUG_HPP +// #include +// #include -namespace protocol -{ - void debugPrintFrankaArmStateBuffer(const std::vector& buffer); -} +// namespace protocol +// { +// void debugPrintFrankaArmStateBuffer(const std::vector& buffer); +// } -#endif // STATE_DEBUG_HPP \ No newline at end of file +// #endif // STATE_DEBUG_HPP \ No newline at end of file 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/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index c7907b5..60434b5 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -1,4 +1,6 @@ #include "control_mode/abstract_control_mode.hpp" +#include "control_mode/joint_position_motion_generator.hpp" +#include "control_mode/cartesian_pose_motion_generator.hpp" #include #include @@ -66,7 +68,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() @@ -100,132 +102,54 @@ void AbstractControlMode::controlTask() } zlc::info("[{}] Control thread ended.", getModeName()); } -MotionGenerator::MotionGenerator(double speed_factor, const std::array& q_goal) - : q_goal_(q_goal.data()) { - 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 MotionGenerator::operator()(const franka::RobotState& robot_state, - franka::Duration period) { - time_ += period.toSec(); - - if (time_ == 0.0) { - q_start_ = MotionGenerator::Vector7d(robot_state.q.data()); - delta_q_ = q_goal_ - q_start_; - calculateSynchronizedValues(); - } - - MotionGenerator::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 MotionGenerator::calculateDesiredValues(double time, MotionGenerator::Vector7d* delta_q_d) const { - MotionGenerator::Vector7i sign_delta_q; - sign_delta_q << delta_q_.cwiseSign().cast(); - MotionGenerator::Vector7d t_d = t_2_sync_ - t_1_sync_; - MotionGenerator::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; - } +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; } - } - return std::all_of(joint_motion_finished.cbegin(), joint_motion_finished.cend(), - [](bool is_finished) { return is_finished; }); -} - -void MotionGenerator::calculateSynchronizedValues() { - MotionGenerator::Vector7d dq_max_reach(dq_max_); - MotionGenerator::Vector7d t_f = MotionGenerator::Vector7d::Zero(); - MotionGenerator::Vector7d delta_t_2 = MotionGenerator::Vector7d::Zero(); - MotionGenerator::Vector7d t_1 = MotionGenerator::Vector7d::Zero(); - MotionGenerator::Vector7d delta_t_2_sync = MotionGenerator::Vector7d::Zero(); - MotionGenerator::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]; - } + if (is_running_) + { + zlc::warn("[{}] moveToJointPosition rejected: control thread is running.", getModeName()); + return false; } - 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]); + 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}}); + JointPositionMotionGenerator motion_generator(max_velocity, target_q); + robot_->control(motion_generator); + }catch (const franka::Exception& e) { + std::cout << e.what() << std::endl; + return false; } - } + zlc::info("[{}] Reached target joint position.", getModeName()); + return true; } -bool AbstractControlMode::moveToJointPosition(const std::array& target_q, + +bool AbstractControlMode::moveToCartesianPose(const Eigen::Vector3d& target_position, + const Eigen::Quaterniond& target_orientation, double max_velocity, double tolerance) { #if NO_ROBOT_TESTING - zlc::error("[{}] moveToJointPosition is not supported in NO_ROBOT_TESTING mode.", + zlc::error("[{}] moveToCartesianPose is not supported in NO_ROBOT_TESTING mode.", getModeName()); return false; #else if (!robot_) { - zlc::error("[{}] moveToJointPosition failed: robot not initialized.", getModeName()); + zlc::error("[{}] moveToCartesianPose failed: robot not initialized.", getModeName()); return false; } if (is_running_) { - zlc::warn("[{}] moveToJointPosition rejected: control thread is running.", getModeName()); + zlc::warn("[{}] moveToCartesianPose rejected: control thread is running.", getModeName()); return false; } try { @@ -234,12 +158,12 @@ bool AbstractControlMode::moveToJointPosition(const std::array {{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}}); - MotionGenerator motion_generator(max_velocity, target_q); + CartesianPoseMotionGenerator motion_generator(max_velocity, target_position, target_orientation); robot_->control(motion_generator); - }catch (const franka::Exception& e) { - std::cout << e.what() << std::endl; - return -1; - } + } catch (const franka::Exception& e) { + std::cout << e.what() << std::endl; + return false; + } return true; #endif } diff --git a/src/control_mode/cartesian_pose_motion_generator.cpp b/src/control_mode/cartesian_pose_motion_generator.cpp new file mode 100644 index 0000000..2ef2836 --- /dev/null +++ b/src/control_mode/cartesian_pose_motion_generator.cpp @@ -0,0 +1,288 @@ +#include "control_mode/cartesian_pose_motion_generator.hpp" + +#include +#include + +CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( + double speed_factor, + const Eigen::Vector3d& goal_position, + const Eigen::Quaterniond& goal_orientation, + 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) { + 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) { + 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)); + + 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/control_mode/joint_position_motion_generator.cpp b/src/control_mode/joint_position_motion_generator.cpp new file mode 100644 index 0000000..a57c33e --- /dev/null +++ b/src/control_mode/joint_position_motion_generator.cpp @@ -0,0 +1,115 @@ +#include "control_mode/joint_position_motion_generator.hpp" + +#include +#include + +JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, const std::array& q_goal) + : q_goal_(q_goal.data()) { + 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) { + time_ += period.toSec(); + + if (time_ == 0.0) { + q_start_ = JointPositionMotionGenerator::Vector7d(robot_state.q.data()); + delta_q_ = q_goal_ - q_start_; + 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/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index f7680a8..a60bcc0 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -9,31 +9,11 @@ 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_))) + current_state(AtomicDoubleBuffer(franka::RobotState{})) { // Register service handlers initRobot(); @@ -50,6 +30,7 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) ControlModeFactory::registerControlModes(control_modes_, *robot_, *model_, current_state, safety_config_); setControlMode("Idle"); + current_control_mode_->moveToJointPosition(config_.arm_default_state_q); initializeService(); } diff --git a/src/main.cpp b/src/main.cpp index b7d7151..16fa8c5 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -21,7 +21,7 @@ int main(int argc, char** argv) 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); + zlc::init(node_name, proxy_ip, "224.0.0.1", 7720, "localtest"); std::string arm_config_path = proxy_reader.getValue("arm_config_path"); if (arm_config_path.empty()) diff --git a/src/mujoco_sim/mujoco_robot.cpp b/src/mujoco_sim/mujoco_robot.cpp index bbb75ff..a0969ff 100644 --- a/src/mujoco_sim/mujoco_robot.cpp +++ b/src/mujoco_sim/mujoco_robot.cpp @@ -3,8 +3,10 @@ #include #include +#include #include #include +#include #include #include "mujoco_sim/mujoco_panda_env.hpp" @@ -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); + } +} From 8fd098fb8dfb384922d2d59513883ecbb2cd5120 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Thu, 29 Jan 2026 18:28:06 +0100 Subject: [PATCH 05/19] add zlc namespace init parameters for multicast group --- config/ProxyConfig.yaml | 6 +- include/debugger/fake_franka.hpp | 109 ------------------------------- include/debugger/state_debug.hpp | 11 ---- src/debugger/state_debug.cpp | 46 ------------- src/main.cpp | 7 +- 5 files changed, 9 insertions(+), 170 deletions(-) delete mode 100644 include/debugger/fake_franka.hpp delete mode 100644 include/debugger/state_debug.hpp delete mode 100644 src/debugger/state_debug.cpp diff --git a/config/ProxyConfig.yaml b/config/ProxyConfig.yaml index 7aad1d8..c816f6f 100644 --- a/config/ProxyConfig.yaml +++ b/config/ProxyConfig.yaml @@ -1,7 +1,9 @@ # Franka Arm Configuration for P10 node_name: FrankaPanda -robot_ip: 10.10.10.10 -proxy_ip: 192.168.0.117 +group: 224.0.0.1 +group_port: 7720 +group_name: "RobotControlGroup" +proxy_ip: 192.168.0.134 arm_config_path: config/RobotArmConfig.yaml gripper_config_path: config/GripperConfig.yaml \ No newline at end of file diff --git a/include/debugger/fake_franka.hpp b/include/debugger/fake_franka.hpp deleted file mode 100644 index 2d7f87e..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 24812ef..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/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/main.cpp b/src/main.cpp index 16fa8c5..ef1e0d2 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -19,9 +19,12 @@ 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"); + 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); 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, "224.0.0.1", 7720, "localtest"); + zlc::info("Using proxy IP address: {} at group {}:{} with group name {}", proxy_ip, group, group_port, group_name); std::string arm_config_path = proxy_reader.getValue("arm_config_path"); if (arm_config_path.empty()) From 95e2654d954d16fb765bb5cd71e54cb0fafec8d4 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Thu, 29 Jan 2026 19:39:03 +0100 Subject: [PATCH 06/19] add gripper to proxy --- config/GripperConfig.yaml | 14 +- include/franka_gripper_proxy.hpp | 413 +++++++++++------------------ include/protocol/grasp_command.hpp | 16 -- src/main.cpp | 12 +- 4 files changed, 172 insertions(+), 283 deletions(-) delete mode 100644 include/protocol/grasp_command.hpp diff --git a/config/GripperConfig.yaml b/config/GripperConfig.yaml index 92771f1..4520674 100644 --- a/config/GripperConfig.yaml +++ b/config/GripperConfig.yaml @@ -1,9 +1,9 @@ # Gripper settings -gripper_pub_rate_hz: 100 -gripper_command_rcvtimeo_ms: 500 +name: FrankaPandaGripper +gripper_ip: 10.10.10.10 + +command_topic: franka_gripper_command +state_topic: franka_gripper_state -# 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 +gripper_pub_rate_hz: 100 +gripper_command_rcvtimeo_ms: 500 \ No newline at end of file diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index cd26414..8b30dfd 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -1,258 +1,155 @@ -// #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; -// }; +#pragma once +#include +#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; + int64_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 FrankaGripperConfig +{ + // communication + std::string name; + std::string gripper_ip; + + std::string command_topic; + std::string state_topic; + + FrankaGripperConfig(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"); + } +}; + + + + +class FrankaGripperProxy { + +public: + // Constructor & Destructor + explicit FrankaGripperProxy(const std::string& config_path): + is_running(false), + command_(AtomicDoubleBuffer(GraspCommand{})), + config_(config_path) + { + gripper_ = std::make_shared(config_.gripper_ip); + gripper_->homing(); + command_.write(GraspCommand{0.0f, 0.1f}); + is_running = true; + zlc::info("Gripper proxy running flag set to {}", is_running.load()); + zlc::registerSubscriberHandler(config_.command_topic, &FrankaGripperProxy::updateCommand, this); + state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); + control_thread_ = std::thread(&FrankaGripperProxy::controlLoopThread, this); + + }; + ~FrankaGripperProxy() { + stop(); + }; + + + void stop() { + zlc::info("Stopping FrankaGripperProxy..."); + is_running = false; + if (state_pub_thread_.joinable()) state_pub_thread_.join(); + if (control_thread_.joinable()) control_thread_.join(); + gripper_.reset(); + zlc::info("FrankaGripperProxy stopped successfully."); + }; + +private: + + // Franka robot + std::shared_ptr gripper_; + + // Threading + std::thread state_pub_thread_; + std::thread control_thread_; + + // Threading Tasks + void controlLoopThread() { + while (is_running) { + GraspCommand cmd = command_.read(); + try { + gripper_->move(cmd.width, cmd.speed); + } + catch (const std::exception& e) { + zlc::error("FrankaGripperProxy control loop exception: {}", e.what()); + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + }; + + void statePubThread() { + zlc::info("FrankaGripperProxy 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 updateCommand(const GraspCommand& cmd) { + command_.write(cmd); + }; + + // Synchronization + std::atomic is_running; // for threads + + AtomicDoubleBuffer command_; + + FrankaGripperConfig config_; + + // TODO: put all the Constants to a config file + static constexpr int GRIPPER_PUB_RATE_HZ = 100; +}; 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/src/main.cpp b/src/main.cpp index ef1e0d2..796cc0d 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -4,6 +4,7 @@ #include #include "franka_arm_proxy.hpp" +#include "franka_gripper_proxy.hpp" int main(int argc, char** argv) { @@ -34,8 +35,15 @@ int main(int argc, char** argv) } FrankaArmProxy robot_proxy(arm_config_path); - // FrankaGripperProxy gripper_proxy(gripper_cfg); - // gripper_proxy.start(); + std::string gripper_config_path = proxy_reader.getValue("gripper_config_path"); + if (gripper_config_path.empty()) + { + zlc::error("Gripper config path is empty in proxy config file."); + return 1; + } + + FrankaGripperProxy gripper_proxy(proxy_reader.getValue("gripper_config_path")); + zlc::spin(); return 0; } From 0f6d41cd12c8252a109db5520c9332f2df70e510 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Sun, 1 Feb 2026 16:43:49 +0100 Subject: [PATCH 07/19] add state buffer --- .../cartesian_pose_motion_generator.hpp | 6 ++++++ .../joint_position_motion_generator.hpp | 8 +++++++- src/control_mode/abstract_control_mode.cpp | 16 +++++++--------- .../cartesian_pose_motion_generator.cpp | 7 +++++-- .../joint_position_motion_generator.cpp | 9 ++++++--- 5 files changed, 31 insertions(+), 15 deletions(-) rename include/{control_mode => motion_generator}/cartesian_pose_motion_generator.hpp (92%) rename include/{control_mode => motion_generator}/joint_position_motion_generator.hpp (84%) rename src/{control_mode => motion_generator}/cartesian_pose_motion_generator.cpp (97%) rename src/{control_mode => motion_generator}/joint_position_motion_generator.cpp (94%) diff --git a/include/control_mode/cartesian_pose_motion_generator.hpp b/include/motion_generator/cartesian_pose_motion_generator.hpp similarity index 92% rename from include/control_mode/cartesian_pose_motion_generator.hpp rename to include/motion_generator/cartesian_pose_motion_generator.hpp index 53ec6ce..b192fc9 100644 --- a/include/control_mode/cartesian_pose_motion_generator.hpp +++ b/include/motion_generator/cartesian_pose_motion_generator.hpp @@ -6,6 +6,8 @@ #include #include +#include "utils/atomic_double_buffer.hpp" + class CartesianPoseMotionGenerator { public: @@ -15,6 +17,7 @@ class CartesianPoseMotionGenerator * @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] 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). @@ -25,6 +28,7 @@ class CartesianPoseMotionGenerator CartesianPoseMotionGenerator(double speed_factor, const Eigen::Vector3d& goal_position, const Eigen::Quaterniond& goal_orientation, + AtomicDoubleBuffer& state_buffer, double dx_max = 0.3, double ddx_max_start = 1.0, double ddx_max_goal = 1.0, @@ -94,4 +98,6 @@ class CartesianPoseMotionGenerator double omega_max_; double domega_max_start_; double domega_max_goal_; + + AtomicDoubleBuffer* state_buffer_; }; diff --git a/include/control_mode/joint_position_motion_generator.hpp b/include/motion_generator/joint_position_motion_generator.hpp similarity index 84% rename from include/control_mode/joint_position_motion_generator.hpp rename to include/motion_generator/joint_position_motion_generator.hpp index 88af369..0fb47e1 100644 --- a/include/control_mode/joint_position_motion_generator.hpp +++ b/include/motion_generator/joint_position_motion_generator.hpp @@ -6,6 +6,8 @@ #include #include +#include "utils/atomic_double_buffer.hpp" + class JointPositionMotionGenerator { public: @@ -14,8 +16,10 @@ class JointPositionMotionGenerator * * @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. */ - JointPositionMotionGenerator(double speed_factor, const std::array& q_goal); + JointPositionMotionGenerator(double speed_factor, const std::array& q_goal, + AtomicDoubleBuffer& state_buffer); /** * Sends joint position calculations @@ -51,4 +55,6 @@ class JointPositionMotionGenerator 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_; }; diff --git a/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index 60434b5..a5a574a 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -1,18 +1,16 @@ #include "control_mode/abstract_control_mode.hpp" -#include "control_mode/joint_position_motion_generator.hpp" -#include "control_mode/cartesian_pose_motion_generator.hpp" +#include "motion_generator/cartesian_pose_motion_generator.hpp" +#include "motion_generator/joint_position_motion_generator.hpp" -#include +#include #include #include +#include +#include #include #include -#include -#include -#include - void ControllerConfig::readBaseConfig(const ConfigFileReader& reader) { controller_name = reader.getValue("name"); @@ -123,7 +121,7 @@ bool AbstractControlMode::moveToJointPosition(const std::array {{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}}); - JointPositionMotionGenerator motion_generator(max_velocity, target_q); + JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_); robot_->control(motion_generator); }catch (const franka::Exception& e) { std::cout << e.what() << std::endl; @@ -158,7 +156,7 @@ bool AbstractControlMode::moveToCartesianPose(const Eigen::Vector3d& target_posi {{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); + CartesianPoseMotionGenerator motion_generator(max_velocity, target_position, target_orientation, *state_buffer_); robot_->control(motion_generator); } catch (const franka::Exception& e) { std::cout << e.what() << std::endl; diff --git a/src/control_mode/cartesian_pose_motion_generator.cpp b/src/motion_generator/cartesian_pose_motion_generator.cpp similarity index 97% rename from src/control_mode/cartesian_pose_motion_generator.cpp rename to src/motion_generator/cartesian_pose_motion_generator.cpp index 2ef2836..4e548f8 100644 --- a/src/control_mode/cartesian_pose_motion_generator.cpp +++ b/src/motion_generator/cartesian_pose_motion_generator.cpp @@ -1,4 +1,4 @@ -#include "control_mode/cartesian_pose_motion_generator.hpp" +#include "motion_generator/cartesian_pose_motion_generator.hpp" #include #include @@ -7,6 +7,7 @@ CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( double speed_factor, const Eigen::Vector3d& goal_position, const Eigen::Quaterniond& goal_orientation, + AtomicDoubleBuffer& state_buffer, double dx_max, double ddx_max_start, double ddx_max_goal, @@ -20,7 +21,8 @@ CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( 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) { + domega_max_goal_(domega_max_goal * speed_factor), + state_buffer_(&state_buffer) { dx_max_sync_.setZero(); pos_start_.setZero(); delta_pos_.setZero(); @@ -41,6 +43,7 @@ CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( franka::CartesianPose CartesianPoseMotionGenerator::operator()( const franka::RobotState& robot_state, franka::Duration period) { + state_buffer_->write(robot_state); time_ += period.toSec(); if (time_ == 0.0) { diff --git a/src/control_mode/joint_position_motion_generator.cpp b/src/motion_generator/joint_position_motion_generator.cpp similarity index 94% rename from src/control_mode/joint_position_motion_generator.cpp rename to src/motion_generator/joint_position_motion_generator.cpp index a57c33e..fae22a4 100644 --- a/src/control_mode/joint_position_motion_generator.cpp +++ b/src/motion_generator/joint_position_motion_generator.cpp @@ -1,10 +1,11 @@ -#include "control_mode/joint_position_motion_generator.hpp" +#include "motion_generator/joint_position_motion_generator.hpp" #include #include -JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, const std::array& q_goal) - : q_goal_(q_goal.data()) { +JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, const std::array& q_goal, + AtomicDoubleBuffer& state_buffer) + : q_goal_(q_goal.data()), state_buffer_(&state_buffer) { dq_max_ *= speed_factor; ddq_max_start_ *= speed_factor; ddq_max_goal_ *= speed_factor; @@ -16,8 +17,10 @@ JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, 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) { From a26fe7a71eb0db446f0bb331a37136e59cb58136 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Sun, 1 Feb 2026 16:54:15 +0100 Subject: [PATCH 08/19] clean the include --- include/franka_gripper_proxy.hpp | 4 ++-- include/protocol/msg.hpp | 3 ++- include/utils/robot_utils.hpp | 2 ++ src/franka_arm_proxy.cpp | 1 - src/main.cpp | 1 + src/mujoco_sim/mujoco_panda_env.cpp | 1 + src/mujoco_sim/mujoco_robot.cpp | 8 ++++---- src/utils/robot_model.cpp | 15 +++++++-------- 8 files changed, 19 insertions(+), 16 deletions(-) diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index 8b30dfd..42c0863 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -18,9 +18,9 @@ struct GripperStateMsg { double max_width; bool is_grasped; uint16_t temperature; - int64_t time; + uint64_t time; - MSGPACK_DEFINE_MAP(width, max_width, is_grasped, temperature, time); + MSGPACK_DEFINE_MAP(width, max_width, is_grasped, temperature, time) }; struct GraspCommand diff --git a/include/protocol/msg.hpp b/include/protocol/msg.hpp index f738aed..cd3c04d 100644 --- a/include/protocol/msg.hpp +++ b/include/protocol/msg.hpp @@ -1,4 +1,5 @@ -// #include +#pragma once + #include #include 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/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index a60bcc0..eb2fe52 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -1,4 +1,3 @@ - #include "franka_arm_proxy.hpp" #include diff --git a/src/main.cpp b/src/main.cpp index 796cc0d..6eff8aa 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,6 +1,7 @@ #include #include #include + #include #include "franka_arm_proxy.hpp" 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 a0969ff..6710893 100644 --- a/src/mujoco_sim/mujoco_robot.cpp +++ b/src/mujoco_sim/mujoco_robot.cpp @@ -1,14 +1,14 @@ #include "mujoco_sim/mujoco_robot.hpp" -#include -#include - -#include #include #include #include #include +#include +#include +#include + #include "mujoco_sim/mujoco_panda_env.hpp" namespace diff --git a/src/utils/robot_model.cpp b/src/utils/robot_model.cpp index c3bced0..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) { From fe6e52b4ec5c2b23473b7a36bead805437af05bb Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Sun, 1 Feb 2026 17:01:41 +0100 Subject: [PATCH 09/19] add tolerance --- .../cartesian_pose_motion_generator.hpp | 3 +++ .../joint_position_motion_generator.hpp | 5 ++++- src/control_mode/abstract_control_mode.cpp | 4 ++-- .../cartesian_pose_motion_generator.cpp | 11 ++++++++++- .../joint_position_motion_generator.cpp | 13 +++++++++++-- 5 files changed, 30 insertions(+), 6 deletions(-) diff --git a/include/motion_generator/cartesian_pose_motion_generator.hpp b/include/motion_generator/cartesian_pose_motion_generator.hpp index b192fc9..d94d7c6 100644 --- a/include/motion_generator/cartesian_pose_motion_generator.hpp +++ b/include/motion_generator/cartesian_pose_motion_generator.hpp @@ -18,6 +18,7 @@ class CartesianPoseMotionGenerator * @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). @@ -29,6 +30,7 @@ class CartesianPoseMotionGenerator 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, @@ -100,4 +102,5 @@ class CartesianPoseMotionGenerator 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 index 0fb47e1..a83e88e 100644 --- a/include/motion_generator/joint_position_motion_generator.hpp +++ b/include/motion_generator/joint_position_motion_generator.hpp @@ -17,9 +17,11 @@ class JointPositionMotionGenerator * @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); + AtomicDoubleBuffer& state_buffer, + double tolerance = 1e-3); /** * Sends joint position calculations @@ -57,4 +59,5 @@ class JointPositionMotionGenerator Vector7d ddq_max_goal_ = (Vector7d() << 5, 5, 5, 5, 5, 5, 5).finished(); AtomicDoubleBuffer* state_buffer_; + double tolerance_; }; diff --git a/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index a5a574a..2dad1b1 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -121,7 +121,7 @@ bool AbstractControlMode::moveToJointPosition(const std::array {{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}}); - JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_); + JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_, tolerance); robot_->control(motion_generator); }catch (const franka::Exception& e) { std::cout << e.what() << std::endl; @@ -156,7 +156,7 @@ bool AbstractControlMode::moveToCartesianPose(const Eigen::Vector3d& target_posi {{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_); + 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; diff --git a/src/motion_generator/cartesian_pose_motion_generator.cpp b/src/motion_generator/cartesian_pose_motion_generator.cpp index 4e548f8..fc2a329 100644 --- a/src/motion_generator/cartesian_pose_motion_generator.cpp +++ b/src/motion_generator/cartesian_pose_motion_generator.cpp @@ -8,6 +8,7 @@ CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( 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, @@ -22,7 +23,8 @@ CartesianPoseMotionGenerator::CartesianPoseMotionGenerator( 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) { + state_buffer_(&state_buffer), + tolerance_(tolerance) { dx_max_sync_.setZero(); pos_start_.setZero(); delta_pos_.setZero(); @@ -68,6 +70,13 @@ franka::CartesianPose CartesianPoseMotionGenerator::operator()( 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(); } diff --git a/src/motion_generator/joint_position_motion_generator.cpp b/src/motion_generator/joint_position_motion_generator.cpp index fae22a4..e658355 100644 --- a/src/motion_generator/joint_position_motion_generator.cpp +++ b/src/motion_generator/joint_position_motion_generator.cpp @@ -4,8 +4,9 @@ #include JointPositionMotionGenerator::JointPositionMotionGenerator(double speed_factor, const std::array& q_goal, - AtomicDoubleBuffer& state_buffer) - : q_goal_(q_goal.data()), state_buffer_(&state_buffer) { + 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; @@ -26,6 +27,14 @@ franka::JointPositions JointPositionMotionGenerator::operator()(const franka::Ro 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(); } From ca7d42e640fbc5cec0132f7daab70728b019f755 Mon Sep 17 00:00:00 2001 From: Simon Essig Date: Sun, 1 Feb 2026 17:03:45 +0100 Subject: [PATCH 10/19] adaptiv for Robot201/202 --- config/GripperConfig.yaml | 9 ---- config/GripperConfig_follower.yaml | 10 ++++ config/GripperConfig_leader.yaml | 10 ++++ config/ProxyConfig.yaml | 9 ---- config/ProxyConfig_follower.yaml | 11 ++++ config/ProxyConfig_leader.yaml | 11 ++++ ...nfig.yaml => RobotArmConfig_follower.yaml} | 10 ++-- config/RobotArmConfig_leader.yaml | 24 +++++++++ .../hybrid_joint_impedance_controller.cfg | 27 +++++----- include/control_mode/control_mode.hpp | 2 + include/control_mode/gravity_comp_control.hpp | 30 +++++++++++ include/franka_gripper_proxy.hpp | 19 +++++-- src/control_mode/gravity_comp_control.cpp | 51 +++++++++++++++++++ src/franka_arm_proxy.cpp | 7 ++- 14 files changed, 188 insertions(+), 42 deletions(-) delete mode 100644 config/GripperConfig.yaml create mode 100644 config/GripperConfig_follower.yaml create mode 100644 config/GripperConfig_leader.yaml delete mode 100644 config/ProxyConfig.yaml create mode 100644 config/ProxyConfig_follower.yaml create mode 100644 config/ProxyConfig_leader.yaml rename config/{RobotArmConfig.yaml => RobotArmConfig_follower.yaml} (84%) create mode 100644 config/RobotArmConfig_leader.yaml create mode 100644 include/control_mode/gravity_comp_control.hpp create mode 100644 src/control_mode/gravity_comp_control.cpp diff --git a/config/GripperConfig.yaml b/config/GripperConfig.yaml deleted file mode 100644 index 4520674..0000000 --- a/config/GripperConfig.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# Gripper settings -name: FrankaPandaGripper -gripper_ip: 10.10.10.10 - -command_topic: franka_gripper_command -state_topic: franka_gripper_state - -gripper_pub_rate_hz: 100 -gripper_command_rcvtimeo_ms: 500 \ No newline at end of file diff --git a/config/GripperConfig_follower.yaml b/config/GripperConfig_follower.yaml new file mode 100644 index 0000000..68f5735 --- /dev/null +++ b/config/GripperConfig_follower.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/GripperConfig_leader.yaml b/config/GripperConfig_leader.yaml new file mode 100644 index 0000000..caaf59d --- /dev/null +++ b/config/GripperConfig_leader.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/ProxyConfig.yaml b/config/ProxyConfig.yaml deleted file mode 100644 index c816f6f..0000000 --- a/config/ProxyConfig.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# Franka Arm Configuration for P10 -node_name: FrankaPanda -group: 224.0.0.1 -group_port: 7720 -group_name: "RobotControlGroup" -proxy_ip: 192.168.0.134 - -arm_config_path: config/RobotArmConfig.yaml -gripper_config_path: config/GripperConfig.yaml \ No newline at end of file diff --git a/config/ProxyConfig_follower.yaml b/config/ProxyConfig_follower.yaml new file mode 100644 index 0000000..27caaa0 --- /dev/null +++ b/config/ProxyConfig_follower.yaml @@ -0,0 +1,11 @@ +# Franka Arm Configuration (follower) +node_name: FrankaFollower +robot_ip: 10.10.10.202 +proxy_ip: 141.3.53.63 +group: 224.0.0.1 +group_port: 7720 +group_name: "RobotControlGroup" + + +arm_config_path: config/RobotArmConfig_follower.yaml +gripper_config_path: config/GripperConfig_follower.yaml diff --git a/config/ProxyConfig_leader.yaml b/config/ProxyConfig_leader.yaml new file mode 100644 index 0000000..9f62dad --- /dev/null +++ b/config/ProxyConfig_leader.yaml @@ -0,0 +1,11 @@ +# Franka Arm Configuration (leader) +node_name: FrankaLeader +robot_ip: 10.10.10.201 +proxy_ip: 141.3.53.63 +group: 224.0.0.1 +group_port: 7720 +group_name: "RobotControlGroup" + + +arm_config_path: config/RobotArmConfig_leader.yaml +gripper_config_path: config/GripperConfig_leader.yaml diff --git a/config/RobotArmConfig.yaml b/config/RobotArmConfig_follower.yaml similarity index 84% rename from config/RobotArmConfig.yaml rename to config/RobotArmConfig_follower.yaml index c425ff4..347a13d 100644 --- a/config/RobotArmConfig.yaml +++ b/config/RobotArmConfig_follower.yaml @@ -1,9 +1,9 @@ -# Franka Arm Configuration for P10 -name: FrankaPanda -robot_ip: 10.10.10.10 +# Franka Arm Configuration (follower) +name: FrankaFollower +robot_ip: 10.10.10.202 # Arm default state -arm_default_state_q: [0.1775, -0.2385, -0.1525, -2.0549, -0.1030, 1.7526, 0] +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 @@ -21,4 +21,4 @@ arm_collision_upper_force_thresholds_acc: [15, 15, 15, 15, 15, 15] 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] \ No newline at end of file +arm_collision_upper_force_thresholds_nom: [20, 20, 20, 20, 20, 20] diff --git a/config/RobotArmConfig_leader.yaml b/config/RobotArmConfig_leader.yaml new file mode 100644 index 0000000..c11daa4 --- /dev/null +++ b/config/RobotArmConfig_leader.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/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 6dde1cc..19eaf43 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,23 +1,22 @@ name: HybridJointImpedance -command_topic: FrankaPanda/joint_position_command +command_topic: FrankaFollower/joint_position_command_leaderfollower #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: [40, 50, 50, 40, 35, 20, 10] -# kqd_gains: [1.0, 1.5, 1.0, 1.5, 0.5, 0.5, 0.5] -# kx_gains: [35, 20, 35, 14, 14, 14] -# kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] +kqd_gains: [2.0, 3.0, 2.0, 2.0, 1.0, 1.0, 1.0] +kx_gains: [50, 50, 50, 20, 20, 20] +kxd_gains: [1.0, 1.0, 1.0, 0.2, 0.2, 0.2] +# kq_gains: [8, 12, 10, 10, 6.5, 4.0, 4.0] +# kqd_gains: [0.25, 0.38, 0.25, 0.25, 0.125, 0.125, 0.125] +# kx_gains: [17.5, 17.5, 17.5, 7, 7, 7] +# kxd_gains: [0.125, 0.125, 0.125, 0.025, 0.025, 0.025] +# kq_gains: [12, 18, 15, 15, 9.5, 6.5, 6.5] +# kqd_gains: [0.7, 0.85, 0.6, 0.6, 0.35, 0.35, 0.35] +# kx_gains: [35, 35, 35, 14, 14, 14] +# kxd_gains: [0.45, 0.35, 0.35, 0.15, 0.15, 0.15] ignore_gravity: true # no use for now -# 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] - # 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 \ No newline at end of file +# kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2] *0.25 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/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp index 8b30dfd..4c96168 100644 --- a/include/franka_gripper_proxy.hpp +++ b/include/franka_gripper_proxy.hpp @@ -44,6 +44,7 @@ struct FrankaGripperConfig std::string command_topic; std::string state_topic; + bool enable_control{true}; FrankaGripperConfig(const std::string& gripper_config_path) { @@ -57,6 +58,7 @@ struct FrankaGripperConfig 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); } }; @@ -74,12 +76,23 @@ class FrankaGripperProxy { { gripper_ = std::make_shared(config_.gripper_ip); gripper_->homing(); - command_.write(GraspCommand{0.0f, 0.1f}); + // 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.1f}); is_running = true; zlc::info("Gripper proxy running flag set to {}", is_running.load()); - zlc::registerSubscriberHandler(config_.command_topic, &FrankaGripperProxy::updateCommand, this); state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); - control_thread_ = std::thread(&FrankaGripperProxy::controlLoopThread, this); + if (config_.enable_control) + { + zlc::registerSubscriberHandler(config_.command_topic, &FrankaGripperProxy::updateCommand, this); + control_thread_ = std::thread(&FrankaGripperProxy::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); + } }; ~FrankaGripperProxy() { diff --git a/src/control_mode/gravity_comp_control.cpp b/src/control_mode/gravity_comp_control.cpp new file mode 100644 index 0000000..ab06ddf --- /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.4, 0.4, 0.4, 0.3, 0.2, 0.4, 0.4}; + + 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/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index a60bcc0..f964ad0 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -29,8 +29,11 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) safety_config_.fromFile("./config/SafetyLimitConfig.cfg"); ControlModeFactory::registerControlModes(control_modes_, *robot_, *model_, current_state, safety_config_); - setControlMode("Idle"); - current_control_mode_->moveToJointPosition(config_.arm_default_state_q); + // 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); initializeService(); } From 19c6e6c798b84dce69402aa505648296385f73bd Mon Sep 17 00:00:00 2001 From: Simon Essig Date: Mon, 2 Feb 2026 19:44:30 +0100 Subject: [PATCH 11/19] go back to control mode --- config/ProxyConfig_follower.yaml | 2 +- config/ProxyConfig_leader.yaml | 2 +- .../controller/hybrid_joint_impedance_controller.cfg | 10 +++++----- src/control_mode/abstract_control_mode.cpp | 1 + src/franka_arm_proxy.cpp | 2 +- 5 files changed, 9 insertions(+), 8 deletions(-) diff --git a/config/ProxyConfig_follower.yaml b/config/ProxyConfig_follower.yaml index 27caaa0..15cae5d 100644 --- a/config/ProxyConfig_follower.yaml +++ b/config/ProxyConfig_follower.yaml @@ -4,7 +4,7 @@ robot_ip: 10.10.10.202 proxy_ip: 141.3.53.63 group: 224.0.0.1 group_port: 7720 -group_name: "RobotControlGroup" +group_name: hardware_collection arm_config_path: config/RobotArmConfig_follower.yaml diff --git a/config/ProxyConfig_leader.yaml b/config/ProxyConfig_leader.yaml index 9f62dad..1a5ea71 100644 --- a/config/ProxyConfig_leader.yaml +++ b/config/ProxyConfig_leader.yaml @@ -4,7 +4,7 @@ robot_ip: 10.10.10.201 proxy_ip: 141.3.53.63 group: 224.0.0.1 group_port: 7720 -group_name: "RobotControlGroup" +group_name: hardware_collection arm_config_path: config/RobotArmConfig_leader.yaml diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 19eaf43..6f5b097 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,10 +1,10 @@ name: HybridJointImpedance -command_topic: FrankaFollower/joint_position_command_leaderfollower +command_topic: FrankaFollower/joint_position_command #polymetis default -kq_gains: [20, 30, 25, 25, 15, 10, 10] -kqd_gains: [2.0, 3.0, 2.0, 2.0, 1.0, 1.0, 1.0] -kx_gains: [50, 50, 50, 20, 20, 20] -kxd_gains: [1.0, 1.0, 1.0, 0.2, 0.2, 0.2] +kq_gains: [24, 36, 30, 30, 20, 15, 15] +kqd_gains: [3.0, 3.0, 3.0, 3.0, 1.5, 1.5, 1.5] +kx_gains: [60, 60, 60, 30, 30, 30] +kxd_gains: [1.5, 1.5, 1.5, 0.3, 0.3, 0.3] # kq_gains: [8, 12, 10, 10, 6.5, 4.0, 4.0] # kqd_gains: [0.25, 0.38, 0.25, 0.25, 0.125, 0.125, 0.125] # kx_gains: [17.5, 17.5, 17.5, 7, 7, 7] diff --git a/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index 60434b5..537b78b 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -130,6 +130,7 @@ bool AbstractControlMode::moveToJointPosition(const std::array return false; } zlc::info("[{}] Reached target joint position.", getModeName()); + startControl(); return true; } diff --git a/src/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index f964ad0..1b00b10 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -185,7 +185,7 @@ std::pair> FrankaArmProxy::moveFrankaArmToJoin } std::array target_q_array{}; std::copy(target_q.begin(), target_q.end(), target_q_array.begin()); - current_control_mode_->stopControl(); + // current_control_mode_->stopControl(); const bool ok = current_control_mode_->moveToJointPosition(target_q_array); return {std::string(ok ? protocol::FrankaResponseCode::SUCCESS : protocol::FrankaResponseCode::FAIL), From d9743f8f57620c16884349ae02ef8f718010697a Mon Sep 17 00:00:00 2001 From: Simon Essig Date: Mon, 2 Feb 2026 21:48:35 +0100 Subject: [PATCH 12/19] add recover to motion generator --- .../control_mode/abstract_control_mode.hpp | 2 +- include/control_mode/idle_control_mode.hpp | 1 + src/control_mode/abstract_control_mode.cpp | 33 +++++++++++-------- src/control_mode/gravity_comp_control.cpp | 2 +- .../hybrid_joint_impedance_control.cpp | 3 ++ src/control_mode/idle_control_mode.cpp | 11 +++++++ src/franka_arm_proxy.cpp | 7 ++-- 7 files changed, 41 insertions(+), 18 deletions(-) diff --git a/include/control_mode/abstract_control_mode.hpp b/include/control_mode/abstract_control_mode.hpp index 7e89b49..721f653 100644 --- a/include/control_mode/abstract_control_mode.hpp +++ b/include/control_mode/abstract_control_mode.hpp @@ -67,7 +67,7 @@ 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, 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/src/control_mode/abstract_control_mode.cpp b/src/control_mode/abstract_control_mode.cpp index 418e438..2699397 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -88,13 +88,12 @@ 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; } } @@ -115,17 +114,25 @@ bool AbstractControlMode::moveToJointPosition(const std::array zlc::warn("[{}] moveToJointPosition 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}}); - JointPositionMotionGenerator motion_generator(max_velocity, target_q, *state_buffer_, tolerance); - robot_->control(motion_generator); + 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) { - std::cout << e.what() << std::endl; - return false; + 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(); diff --git a/src/control_mode/gravity_comp_control.cpp b/src/control_mode/gravity_comp_control.cpp index ab06ddf..6352cc0 100644 --- a/src/control_mode/gravity_comp_control.cpp +++ b/src/control_mode/gravity_comp_control.cpp @@ -35,7 +35,7 @@ franka::Torques GravityCompControl::controlLoop(const franka::RobotState& robot_ // 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.4, 0.4, 0.4, 0.3, 0.2, 0.4, 0.4}; + 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++) diff --git a/src/control_mode/hybrid_joint_impedance_control.cpp b/src/control_mode/hybrid_joint_impedance_control.cpp index a3cbe73..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); } 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/franka_arm_proxy.cpp b/src/franka_arm_proxy.cpp index 568fc29..6c35457 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/franka_arm_proxy.cpp @@ -30,9 +30,10 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) 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); + // 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(); } From 6e701cb9c09e19a3762380adaec0ae4b583813a9 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Tue, 3 Feb 2026 13:21:41 +0100 Subject: [PATCH 13/19] remove set collision behavior --- config/ProxyConfig_follower.yaml | 2 +- config/ProxyConfig_leader.yaml | 2 +- local_test.bash | 0 real_robot_test.bash | 0 scripts/compile.sh | 0 scripts/format.sh | 0 src/control_mode/abstract_control_mode.cpp | 10 +++++----- test.bash | 0 test/request.py | 0 test/subscriber.py | 0 10 files changed, 7 insertions(+), 7 deletions(-) mode change 100755 => 100644 local_test.bash mode change 100755 => 100644 real_robot_test.bash mode change 100755 => 100644 scripts/compile.sh mode change 100755 => 100644 scripts/format.sh mode change 100755 => 100644 test.bash mode change 100755 => 100644 test/request.py mode change 100755 => 100644 test/subscriber.py diff --git a/config/ProxyConfig_follower.yaml b/config/ProxyConfig_follower.yaml index 15cae5d..4754266 100644 --- a/config/ProxyConfig_follower.yaml +++ b/config/ProxyConfig_follower.yaml @@ -3,7 +3,7 @@ node_name: FrankaFollower robot_ip: 10.10.10.202 proxy_ip: 141.3.53.63 group: 224.0.0.1 -group_port: 7720 +group_port: 7721 group_name: hardware_collection diff --git a/config/ProxyConfig_leader.yaml b/config/ProxyConfig_leader.yaml index 1a5ea71..efd3586 100644 --- a/config/ProxyConfig_leader.yaml +++ b/config/ProxyConfig_leader.yaml @@ -3,7 +3,7 @@ node_name: FrankaLeader robot_ip: 10.10.10.201 proxy_ip: 141.3.53.63 group: 224.0.0.1 -group_port: 7720 +group_port: 7721 group_name: hardware_collection diff --git a/local_test.bash b/local_test.bash old mode 100755 new mode 100644 diff --git a/real_robot_test.bash b/real_robot_test.bash old mode 100755 new mode 100644 diff --git a/scripts/compile.sh b/scripts/compile.sh old mode 100755 new mode 100644 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 2699397..e5d02e1 100644 --- a/src/control_mode/abstract_control_mode.cpp +++ b/src/control_mode/abstract_control_mode.cpp @@ -114,11 +114,11 @@ bool AbstractControlMode::moveToJointPosition(const std::array 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}}); + // 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 { diff --git a/test.bash b/test.bash old mode 100755 new mode 100644 diff --git a/test/request.py b/test/request.py old mode 100755 new mode 100644 diff --git a/test/subscriber.py b/test/subscriber.py old mode 100755 new mode 100644 From 7dafb975f567c08227c98d7d2267057f3db827cd Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Tue, 3 Feb 2026 15:28:33 +0100 Subject: [PATCH 14/19] add robotiq gripper & tested --- CMakeLists.txt | 8 +- config/ProxyConfig.yaml | 6 +- config/RobotiqGripperConfig.yaml | 20 + .../hybrid_joint_impedance_controller.cfg | 2 +- include/robotiq_gripper_proxy.hpp | 213 ++++++++++ src/main.cpp | 7 +- .../robotiq-gripper-interface/.clang-format | 116 ++++++ .../robotiq-gripper-interface/.gitignore | 51 +++ .../robotiq-gripper-interface/CMakeLists.txt | 130 ++++++ .../CODE_OF_CONDUCT.md | 4 + .../robotiq-gripper-interface/CONTRIBUTING.md | 59 +++ .../robotiq-gripper-interface/Dockerfile | 18 + third_party/robotiq-gripper-interface/LICENSE | 175 ++++++++ third_party/robotiq-gripper-interface/NOTICE | 1 + .../robotiq-gripper-interface/README.md | 105 +++++ .../RobotiqGripperInterfaceConfig.cmake.in | 8 + .../examples/activate_gripper/CMakeLists.txt | 40 ++ .../activate_gripper/activate_gripper.cc | 54 +++ .../examples/close_gripper/CMakeLists.txt | 40 ++ .../examples/close_gripper/close_gripper.cc | 59 +++ .../examples/open_gripper/CMakeLists.txt | 40 ++ .../examples/open_gripper/open_gripper.cc | 59 +++ .../examples/position_gripper/CMakeLists.txt | 40 ++ .../position_gripper/position_gripper.cc | 78 ++++ .../read_write_simultaneous/CMakeLists.txt | 40 ++ .../read_write_simultaneous.cc | 101 +++++ .../speed_force_setting/CMakeLists.txt | 40 ++ .../speed_force_setting.cc | 98 +++++ .../include/robotiq/constants.h | 36 ++ .../robotiq/robotiq_gripper_interface.h | 183 +++++++++ .../include/robotiq/types.h | 97 +++++ .../robotiq-gripper-interface/src/helpers.cc | 103 +++++ .../robotiq-gripper-interface/src/helpers.h | 43 ++ .../src/robotiq_gripper_interface.cc | 377 ++++++++++++++++++ .../src/timeout_reader.cc | 69 ++++ .../src/timeout_reader.h | 41 ++ .../tests/CMakeLists.txt | 40 ++ .../tests/test_helpers.cc | 27 ++ 38 files changed, 2618 insertions(+), 10 deletions(-) create mode 100644 config/RobotiqGripperConfig.yaml create mode 100644 include/robotiq_gripper_proxy.hpp create mode 100644 third_party/robotiq-gripper-interface/.clang-format create mode 100644 third_party/robotiq-gripper-interface/.gitignore create mode 100644 third_party/robotiq-gripper-interface/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/CODE_OF_CONDUCT.md create mode 100644 third_party/robotiq-gripper-interface/CONTRIBUTING.md create mode 100644 third_party/robotiq-gripper-interface/Dockerfile create mode 100644 third_party/robotiq-gripper-interface/LICENSE create mode 100644 third_party/robotiq-gripper-interface/NOTICE create mode 100644 third_party/robotiq-gripper-interface/README.md create mode 100644 third_party/robotiq-gripper-interface/cmake/RobotiqGripperInterfaceConfig.cmake.in create mode 100644 third_party/robotiq-gripper-interface/examples/activate_gripper/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/activate_gripper/activate_gripper.cc create mode 100644 third_party/robotiq-gripper-interface/examples/close_gripper/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/close_gripper/close_gripper.cc create mode 100644 third_party/robotiq-gripper-interface/examples/open_gripper/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/open_gripper/open_gripper.cc create mode 100644 third_party/robotiq-gripper-interface/examples/position_gripper/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/position_gripper/position_gripper.cc create mode 100644 third_party/robotiq-gripper-interface/examples/read_write_simultaneous/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/read_write_simultaneous/read_write_simultaneous.cc create mode 100644 third_party/robotiq-gripper-interface/examples/speed_force_setting/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/examples/speed_force_setting/speed_force_setting.cc create mode 100644 third_party/robotiq-gripper-interface/include/robotiq/constants.h create mode 100644 third_party/robotiq-gripper-interface/include/robotiq/robotiq_gripper_interface.h create mode 100644 third_party/robotiq-gripper-interface/include/robotiq/types.h create mode 100644 third_party/robotiq-gripper-interface/src/helpers.cc create mode 100644 third_party/robotiq-gripper-interface/src/helpers.h create mode 100644 third_party/robotiq-gripper-interface/src/robotiq_gripper_interface.cc create mode 100644 third_party/robotiq-gripper-interface/src/timeout_reader.cc create mode 100644 third_party/robotiq-gripper-interface/src/timeout_reader.h create mode 100644 third_party/robotiq-gripper-interface/tests/CMakeLists.txt create mode 100644 third_party/robotiq-gripper-interface/tests/test_helpers.cc diff --git a/CMakeLists.txt b/CMakeLists.txt index 1cf338c..6bb9461 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -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} @@ -83,6 +86,7 @@ target_link_libraries(proxy pinocchio::pinocchio Eigen3::Eigen MuJoCo::MuJoCo + robotiq-gripper-interface glfw GL X11 diff --git a/config/ProxyConfig.yaml b/config/ProxyConfig.yaml index c816f6f..add06a1 100644 --- a/config/ProxyConfig.yaml +++ b/config/ProxyConfig.yaml @@ -2,8 +2,8 @@ node_name: FrankaPanda group: 224.0.0.1 group_port: 7720 -group_name: "RobotControlGroup" -proxy_ip: 192.168.0.134 +group_name: RobotControlGroup +proxy_ip: 192.168.0.117 arm_config_path: config/RobotArmConfig.yaml -gripper_config_path: config/GripperConfig.yaml \ No newline at end of file +gripper_config_path: config/RobotiqGripperConfig.yaml \ No newline at end of file diff --git a/config/RobotiqGripperConfig.yaml b/config/RobotiqGripperConfig.yaml new file mode 100644 index 0000000..01d2d3c --- /dev/null +++ b/config/RobotiqGripperConfig.yaml @@ -0,0 +1,20 @@ +# Robotiq gripper settings +name: FrankaPanda + +# Serial communication +gripper_port: /dev/ttyUSB1 +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/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 6dde1cc..aead27e 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -11,7 +11,7 @@ kxd_gains: [1, 1, 1, 0.2, 0.2, 0.2] # kxd_gains: [0.25, 0.25, 0.25, 0.05, 0.05, 0.05] -ignore_gravity: true # no use for now +ignore_gravity: true # use for now # 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] diff --git a/include/robotiq_gripper_proxy.hpp b/include/robotiq_gripper_proxy.hpp new file mode 100644 index 0000000..bcc2a22 --- /dev/null +++ b/include/robotiq_gripper_proxy.hpp @@ -0,0 +1,213 @@ +#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" + +// if public to often maybe block + +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) {}//default values + 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 RobotiqGripperProxy { + +public: + // Constructor & Destructor + explicit RobotiqGripperProxy(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("RobotiqGripperProxy failed to connect on port {}", config_.port); + } else { + gripper_->set_timeout(config_.receive_timeout_ms); + if (!gripper_->is_activated()) { + zlc::info("RobotiqGripperProxy activating gripper..."); + gripper_->activate(true); + } + } + command_.write(RobotiqGraspCommand{}); // initialize command buffer + bool reset_success = gripper_->reset(true); + zlc::info("RobotiqGripperProxy reset result: {}", reset_success); + is_running = true; + zlc::info("RobotiqGripper proxy running flag set to {}", is_running.load()); + zlc::registerSubscriberHandler(config_.command_topic, &RobotiqGripperProxy::updateCommand, this); + command_.write(RobotiqGraspCommand{}); // initialize command buffer + state_pub_thread_ = std::thread(&RobotiqGripperProxy::statePubThread, this); + control_thread_ = std::thread(&RobotiqGripperProxy::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); + + }; + ~RobotiqGripperProxy() { + stop(); + }; + + + void stop() { + zlc::info("Stopping RobotiqGripperProxy..."); + is_running = false; + if (state_pub_thread_.joinable()) state_pub_thread_.join(); + if (control_thread_.joinable()) control_thread_.join(); + gripper_->reset(true); + zlc::info("RobotiqGripperProxy stopped successfully."); + }; + +private: + bool 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; + } + + + // Robotiq gripper + std::shared_ptr gripper_; + + // Threading + std::thread state_pub_thread_; + std::thread control_thread_; + + // Threading Tasks + void controlLoopThread() { + RobotiqGraspCommand last_cmd = command_.read(); + while (is_running) { + RobotiqGraspCommand cmd = command_.read(); + // zlc::info("RobotiqGripperProxy control loop read command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + // cmd.position, cmd.speed, cmd.force, cmd.blocking); + if (gripper_) { + try { + // zlc::info("RobotiqGripperProxy executing command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + // cmd.position, cmd.speed, cmd.force, cmd.blocking); + if (commandChanged(cmd, last_cmd)) { + zlc::info("RobotiqGripperProxy 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);//or use set and read？ + last_cmd = cmd; + } + // else { + // zlc::info("RobotiqGripperProxy command unchanged."); + // } + } + catch (const std::exception& e) { + zlc::error("RobotiqGripperProxy control loop exception: {}", e.what()); + } + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + }; + + void statePubThread() { + zlc::info("RobotiqGripperProxy 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 updateCommand(const RobotiqGraspCommand& cmd) { + command_.write(cmd); + zlc::info("RobotiqGripperProxy received new command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + cmd.position, cmd.speed, cmd.force, cmd.blocking); + }; + + // Synchronization + std::atomic is_running; // for threads + + AtomicDoubleBuffer command_; + + RobotiqGripperConfig config_; +}; diff --git a/src/main.cpp b/src/main.cpp index 6eff8aa..0948aef 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -1,11 +1,10 @@ #include #include #include - #include #include "franka_arm_proxy.hpp" -#include "franka_gripper_proxy.hpp" +#include "robotiq_gripper_proxy.hpp" int main(int argc, char** argv) { @@ -35,7 +34,7 @@ int main(int argc, char** argv) return 1; } FrankaArmProxy robot_proxy(arm_config_path); - + sleep(1); // give some time to initialize before starting gripper std::string gripper_config_path = proxy_reader.getValue("gripper_config_path"); if (gripper_config_path.empty()) { @@ -43,7 +42,7 @@ int main(int argc, char** argv) return 1; } - FrankaGripperProxy gripper_proxy(proxy_reader.getValue("gripper_config_path")); + RobotiqGripperProxy gripper_proxy(proxy_reader.getValue("gripper_config_path")); zlc::spin(); return 0; 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"); +} From 55a14c14e5c1fbd2469f9e4c7a3c4f6dfb7315b2 Mon Sep 17 00:00:00 2001 From: Xinkai Jiang Date: Tue, 3 Feb 2026 17:33:06 +0100 Subject: [PATCH 15/19] finish clean the config for lab --- CMakeLists.txt | 1 - config/ProxyConfig.yaml | 9 - config/ProxyConfig_leader.yaml | 11 - .../CartesianVelocityController.yaml | 8 - .../droid_robotiq.yaml} | 0 .../robot_lab_gripper_201.yaml} | 0 .../robot_lab_gripper_202.yaml} | 0 .../droid.yaml} | 0 config/proxy/robot_lab_teleop_201_202.yaml | 17 ++ .../robot_lab_panda_201.yaml} | 0 .../robot_lab_panda_202.yaml} | 0 include/franka_gripper_proxy.hpp | 168 -------------- include/protocol/codec.hpp | 184 --------------- include/protocol/msg.hpp | 2 +- include/robotiq_gripper_proxy.hpp | 213 ------------------ .../panda_arm.hpp} | 6 +- include/robots/panda_gripper.hpp | 90 ++++++++ include/robots/robotiq_gripper.hpp | 105 +++++++++ include/utils/config_file_reader.hpp | 5 + src/main.cpp | 80 +++++-- .../panda_arm.cpp} | 42 ++-- src/robots/panda_gripper.cpp | 82 +++++++ src/robots/robotiq_gripper.cpp | 109 +++++++++ test/follower_client.py | 97 -------- test/follower_interact_client.py | 115 ---------- test/leader_client.py | 99 -------- test/leader_interact_client.py | 115 ---------- test/request.py | 20 -- test/subscriber.py | 25 -- 29 files changed, 493 insertions(+), 1110 deletions(-) delete mode 100644 config/ProxyConfig.yaml delete mode 100644 config/ProxyConfig_leader.yaml delete mode 100644 config/controller/CartesianVelocityController.yaml rename config/{RobotiqGripperConfig.yaml => gripper/droid_robotiq.yaml} (100%) rename config/{GripperConfig_follower.yaml => gripper/robot_lab_gripper_201.yaml} (100%) rename config/{GripperConfig_leader.yaml => gripper/robot_lab_gripper_202.yaml} (100%) rename config/{ProxyConfig_follower.yaml => proxy/droid.yaml} (100%) create mode 100644 config/proxy/robot_lab_teleop_201_202.yaml rename config/{RobotArmConfig_leader.yaml => robot/robot_lab_panda_201.yaml} (100%) rename config/{RobotArmConfig_follower.yaml => robot/robot_lab_panda_202.yaml} (100%) delete mode 100644 include/franka_gripper_proxy.hpp delete mode 100644 include/protocol/codec.hpp delete mode 100644 include/robotiq_gripper_proxy.hpp rename include/{franka_arm_proxy.hpp => robots/panda_arm.hpp} (97%) create mode 100644 include/robots/panda_gripper.hpp create mode 100644 include/robots/robotiq_gripper.hpp rename src/{franka_arm_proxy.cpp => robots/panda_arm.cpp} (80%) create mode 100644 src/robots/panda_gripper.cpp create mode 100644 src/robots/robotiq_gripper.cpp delete mode 100644 test/follower_client.py delete mode 100644 test/follower_interact_client.py delete mode 100644 test/leader_client.py delete mode 100644 test/leader_interact_client.py delete mode 100644 test/request.py delete mode 100644 test/subscriber.py diff --git a/CMakeLists.txt b/CMakeLists.txt index 6bb9461..7414210 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -86,7 +86,6 @@ target_link_libraries(proxy pinocchio::pinocchio Eigen3::Eigen MuJoCo::MuJoCo - robotiq-gripper-interface glfw GL X11 diff --git a/config/ProxyConfig.yaml b/config/ProxyConfig.yaml deleted file mode 100644 index add06a1..0000000 --- a/config/ProxyConfig.yaml +++ /dev/null @@ -1,9 +0,0 @@ -# Franka Arm Configuration for P10 -node_name: FrankaPanda -group: 224.0.0.1 -group_port: 7720 -group_name: RobotControlGroup -proxy_ip: 192.168.0.117 - -arm_config_path: config/RobotArmConfig.yaml -gripper_config_path: config/RobotiqGripperConfig.yaml \ No newline at end of file diff --git a/config/ProxyConfig_leader.yaml b/config/ProxyConfig_leader.yaml deleted file mode 100644 index efd3586..0000000 --- a/config/ProxyConfig_leader.yaml +++ /dev/null @@ -1,11 +0,0 @@ -# Franka Arm Configuration (leader) -node_name: FrankaLeader -robot_ip: 10.10.10.201 -proxy_ip: 141.3.53.63 -group: 224.0.0.1 -group_port: 7721 -group_name: hardware_collection - - -arm_config_path: config/RobotArmConfig_leader.yaml -gripper_config_path: config/GripperConfig_leader.yaml 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/RobotiqGripperConfig.yaml b/config/gripper/droid_robotiq.yaml similarity index 100% rename from config/RobotiqGripperConfig.yaml rename to config/gripper/droid_robotiq.yaml diff --git a/config/GripperConfig_follower.yaml b/config/gripper/robot_lab_gripper_201.yaml similarity index 100% rename from config/GripperConfig_follower.yaml rename to config/gripper/robot_lab_gripper_201.yaml diff --git a/config/GripperConfig_leader.yaml b/config/gripper/robot_lab_gripper_202.yaml similarity index 100% rename from config/GripperConfig_leader.yaml rename to config/gripper/robot_lab_gripper_202.yaml diff --git a/config/ProxyConfig_follower.yaml b/config/proxy/droid.yaml similarity index 100% rename from config/ProxyConfig_follower.yaml rename to config/proxy/droid.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_leader.yaml b/config/robot/robot_lab_panda_201.yaml similarity index 100% rename from config/RobotArmConfig_leader.yaml rename to config/robot/robot_lab_panda_201.yaml diff --git a/config/RobotArmConfig_follower.yaml b/config/robot/robot_lab_panda_202.yaml similarity index 100% rename from config/RobotArmConfig_follower.yaml rename to config/robot/robot_lab_panda_202.yaml diff --git a/include/franka_gripper_proxy.hpp b/include/franka_gripper_proxy.hpp deleted file mode 100644 index 4590c17..0000000 --- a/include/franka_gripper_proxy.hpp +++ /dev/null @@ -1,168 +0,0 @@ -#pragma once -#include -#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 FrankaGripperConfig -{ - // communication - std::string name; - std::string gripper_ip; - - std::string command_topic; - std::string state_topic; - bool enable_control{true}; - - FrankaGripperConfig(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 FrankaGripperProxy { - -public: - // Constructor & Destructor - explicit FrankaGripperProxy(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.1f}); - is_running = true; - zlc::info("Gripper proxy running flag set to {}", is_running.load()); - state_pub_thread_ = std::thread(&FrankaGripperProxy::statePubThread, this); - if (config_.enable_control) - { - zlc::registerSubscriberHandler(config_.command_topic, &FrankaGripperProxy::updateCommand, this); - control_thread_ = std::thread(&FrankaGripperProxy::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); - } - - }; - ~FrankaGripperProxy() { - stop(); - }; - - - void stop() { - zlc::info("Stopping FrankaGripperProxy..."); - is_running = false; - if (state_pub_thread_.joinable()) state_pub_thread_.join(); - if (control_thread_.joinable()) control_thread_.join(); - gripper_.reset(); - zlc::info("FrankaGripperProxy stopped successfully."); - }; - -private: - - // Franka robot - std::shared_ptr gripper_; - - // Threading - std::thread state_pub_thread_; - std::thread control_thread_; - - // Threading Tasks - void controlLoopThread() { - while (is_running) { - GraspCommand cmd = command_.read(); - try { - gripper_->move(cmd.width, cmd.speed); - } - catch (const std::exception& e) { - zlc::error("FrankaGripperProxy control loop exception: {}", e.what()); - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - }; - - void statePubThread() { - zlc::info("FrankaGripperProxy 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 updateCommand(const GraspCommand& cmd) { - command_.write(cmd); - }; - - // Synchronization - std::atomic is_running; // for threads - - AtomicDoubleBuffer command_; - - FrankaGripperConfig config_; - - // TODO: put all the Constants to a config file - static constexpr int GRIPPER_PUB_RATE_HZ = 100; -}; 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/msg.hpp b/include/protocol/msg.hpp index cd3c04d..0c571ec 100644 --- a/include/protocol/msg.hpp +++ b/include/protocol/msg.hpp @@ -34,7 +34,7 @@ struct FrankaArmState 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; diff --git a/include/robotiq_gripper_proxy.hpp b/include/robotiq_gripper_proxy.hpp deleted file mode 100644 index bcc2a22..0000000 --- a/include/robotiq_gripper_proxy.hpp +++ /dev/null @@ -1,213 +0,0 @@ -#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" - -// if public to often maybe block - -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) {}//default values - 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 RobotiqGripperProxy { - -public: - // Constructor & Destructor - explicit RobotiqGripperProxy(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("RobotiqGripperProxy failed to connect on port {}", config_.port); - } else { - gripper_->set_timeout(config_.receive_timeout_ms); - if (!gripper_->is_activated()) { - zlc::info("RobotiqGripperProxy activating gripper..."); - gripper_->activate(true); - } - } - command_.write(RobotiqGraspCommand{}); // initialize command buffer - bool reset_success = gripper_->reset(true); - zlc::info("RobotiqGripperProxy reset result: {}", reset_success); - is_running = true; - zlc::info("RobotiqGripper proxy running flag set to {}", is_running.load()); - zlc::registerSubscriberHandler(config_.command_topic, &RobotiqGripperProxy::updateCommand, this); - command_.write(RobotiqGraspCommand{}); // initialize command buffer - state_pub_thread_ = std::thread(&RobotiqGripperProxy::statePubThread, this); - control_thread_ = std::thread(&RobotiqGripperProxy::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); - - }; - ~RobotiqGripperProxy() { - stop(); - }; - - - void stop() { - zlc::info("Stopping RobotiqGripperProxy..."); - is_running = false; - if (state_pub_thread_.joinable()) state_pub_thread_.join(); - if (control_thread_.joinable()) control_thread_.join(); - gripper_->reset(true); - zlc::info("RobotiqGripperProxy stopped successfully."); - }; - -private: - bool 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; - } - - - // Robotiq gripper - std::shared_ptr gripper_; - - // Threading - std::thread state_pub_thread_; - std::thread control_thread_; - - // Threading Tasks - void controlLoopThread() { - RobotiqGraspCommand last_cmd = command_.read(); - while (is_running) { - RobotiqGraspCommand cmd = command_.read(); - // zlc::info("RobotiqGripperProxy control loop read command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", - // cmd.position, cmd.speed, cmd.force, cmd.blocking); - if (gripper_) { - try { - // zlc::info("RobotiqGripperProxy executing command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", - // cmd.position, cmd.speed, cmd.force, cmd.blocking); - if (commandChanged(cmd, last_cmd)) { - zlc::info("RobotiqGripperProxy 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);//or use set and read？ - last_cmd = cmd; - } - // else { - // zlc::info("RobotiqGripperProxy command unchanged."); - // } - } - catch (const std::exception& e) { - zlc::error("RobotiqGripperProxy control loop exception: {}", e.what()); - } - } - std::this_thread::sleep_for(std::chrono::milliseconds(10)); - } - }; - - void statePubThread() { - zlc::info("RobotiqGripperProxy 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 updateCommand(const RobotiqGraspCommand& cmd) { - command_.write(cmd); - zlc::info("RobotiqGripperProxy received new command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", - cmd.position, cmd.speed, cmd.force, cmd.blocking); - }; - - // Synchronization - std::atomic is_running; // for threads - - AtomicDoubleBuffer command_; - - RobotiqGripperConfig config_; -}; diff --git a/include/franka_arm_proxy.hpp b/include/robots/panda_arm.hpp similarity index 97% rename from include/franka_arm_proxy.hpp rename to include/robots/panda_arm.hpp index 5656542..d120a73 100644 --- a/include/franka_arm_proxy.hpp +++ b/include/robots/panda_arm.hpp @@ -80,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 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/src/main.cpp b/src/main.cpp index 0948aef..8702446 100644 --- a/src/main.cpp +++ b/src/main.cpp @@ -3,8 +3,10 @@ #include #include -#include "franka_arm_proxy.hpp" -#include "robotiq_gripper_proxy.hpp" +#include "robots/panda_arm.hpp" +#include "robots/panda_gripper.hpp" +#include "robots/robotiq_gripper.hpp" + int main(int argc, char** argv) { @@ -24,26 +26,64 @@ int main(int argc, char** argv) 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); - zlc::info("Starting Franka Control Proxy with node name: {}", node_name); - zlc::info("Using proxy IP address: {} at group {}:{} with group name {}", proxy_ip, group, group_port, group_name); - - 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; - } - FrankaArmProxy robot_proxy(arm_config_path); - sleep(1); // give some time to initialize before starting gripper - std::string gripper_config_path = proxy_reader.getValue("gripper_config_path"); - if (gripper_config_path.empty()) - { - zlc::error("Gripper config path is empty in proxy config file."); - return 1; + 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; + } + + } } - RobotiqGripperProxy gripper_proxy(proxy_reader.getValue("gripper_config_path")); - + 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/franka_arm_proxy.cpp b/src/robots/panda_arm.cpp similarity index 80% rename from src/franka_arm_proxy.cpp rename to src/robots/panda_arm.cpp index 6c35457..88949c3 100644 --- a/src/franka_arm_proxy.cpp +++ b/src/robots/panda_arm.cpp @@ -1,4 +1,4 @@ -#include "franka_arm_proxy.hpp" +#include "robots/panda_arm.hpp" #include @@ -9,7 +9,7 @@ static void signalHandler(int signum) running_flag = false; } -FrankaArmProxy::FrankaArmProxy(const std::string& config_path) +PandaArm::PandaArm(const std::string& config_path) : is_running(false), config_(config_path), current_state(AtomicDoubleBuffer(franka::RobotState{})) @@ -37,30 +37,30 @@ FrankaArmProxy::FrankaArmProxy(const std::string& config_path) initializeService(); } -void FrankaArmProxy::initializeService() +void PandaArm::initializeService() { std::string service_namespace = fmt::format("{}/set_franka_arm_control_mode", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::setControlMode, this); + zlc::registerServiceHandler(service_namespace, &PandaArm::setControlMode, this); service_namespace = fmt::format("{}/get_franka_arm_state", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::getFrankaArmState, this); + zlc::registerServiceHandler(service_namespace, &PandaArm::getFrankaArmState, this); service_namespace = fmt::format("{}/get_franka_arm_control_mode", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::getFrankaArmControlMode, this); + zlc::registerServiceHandler(service_namespace, &PandaArm::getFrankaArmControlMode, this); service_namespace = fmt::format("{}/move_franka_arm_to_joint_position", config_.name); - zlc::registerServiceHandler(service_namespace, &FrankaArmProxy::moveFrankaArmToJointPosition, + zlc::registerServiceHandler(service_namespace, &PandaArm::moveFrankaArmToJointPosition, this); service_namespace = fmt::format("{}/move_franka_arm_to_cartesian_position", config_.name); // zlc::registerServiceHandler(service_namespace, - // &FrankaArmProxy::moveFrankaArmToCartesianPosition, this); + // &PandaArm::moveFrankaArmToCartesianPosition, this); } -FrankaArmProxy::~FrankaArmProxy() +PandaArm::~PandaArm() { stop(); } -void FrankaArmProxy::initRobot() +void PandaArm::initRobot() { //initialize franka robot try @@ -88,14 +88,14 @@ void FrankaArmProxy::initRobot() return; } current_state.write(robot_->readOnce()); - state_pub_thread = std::thread(&FrankaArmProxy::statePublishThread, this); - zlc::info("FrankaArmProxy started successfully."); + state_pub_thread = std::thread(&PandaArm::statePublishThread, this); + zlc::info("PandaArm started successfully."); is_running = true; } -void FrankaArmProxy::stop() +void PandaArm::stop() { - zlc::info("Stopping FrankaArmProxy..."); + zlc::info("Stopping PandaArm..."); is_running = false; if (current_control_mode_) current_control_mode_->stopControl(); @@ -105,11 +105,11 @@ void FrankaArmProxy::stop() robot_.reset(); model_.reset(); current_control_mode_ = nullptr; // reset current control mode - zlc::info("FrankaArmProxy stopped successfully."); + zlc::info("PandaArm stopped successfully."); } // Main loop for processing requests, ctrl-c to stop the server -void FrankaArmProxy::spin() +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."); @@ -123,7 +123,7 @@ void FrankaArmProxy::spin() } // publish threads -void FrankaArmProxy::statePublishThread() +void PandaArm::statePublishThread() { const std::string topic_name = fmt::format("{}/franka_arm_state", config_.name); zlc::Publisher state_pub(topic_name); @@ -137,7 +137,7 @@ void FrankaArmProxy::statePublishThread() } } -zlc::Empty FrankaArmProxy::setControlMode(const std::string& mode) +zlc::Empty PandaArm::setControlMode(const std::string& mode) { if (control_modes_.find(mode) == control_modes_.end()) { @@ -155,12 +155,12 @@ zlc::Empty FrankaArmProxy::setControlMode(const std::string& mode) return zlc::empty; } -FrankaArmState FrankaArmProxy::getFrankaArmState(const zlc::Empty&) +FrankaArmState PandaArm::getFrankaArmState(const zlc::Empty&) { return FrankaArmState(current_state.read()); } -std::string FrankaArmProxy::getFrankaArmControlMode(const zlc::Empty&) +std::string PandaArm::getFrankaArmControlMode(const zlc::Empty&) { if (!current_control_mode_) { @@ -170,7 +170,7 @@ std::string FrankaArmProxy::getFrankaArmControlMode(const zlc::Empty&) return current_control_mode_->getModeName(); } -std::pair> FrankaArmProxy::moveFrankaArmToJointPosition( +std::pair> PandaArm::moveFrankaArmToJointPosition( const std::vector& target_q) { if (!current_control_mode_) 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..7c07370 --- /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/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 100644 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 100644 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() From 562a8fb93a50a01cfbad7c3afd74e0b99a1d3097 Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Tue, 3 Feb 2026 17:59:28 +0100 Subject: [PATCH 16/19] dorid setting config update --- .../hybrid_joint_impedance_controller.cfg | 2 +- config/proxy/droid.yaml | 16 ++++++++----- config/robot/droid_panda.yaml | 24 +++++++++++++++++++ real_robot_test.bash | 2 +- 4 files changed, 36 insertions(+), 8 deletions(-) create mode 100644 config/robot/droid_panda.yaml diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 17a0c6c..02719b5 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,5 +1,5 @@ name: HybridJointImpedance -command_topic: FrankaFollower/joint_position_command +command_topic: joint_position_command #polymetis default kq_gains: [24, 36, 30, 30, 20, 15, 15] kqd_gains: [3.0, 3.0, 3.0, 3.0, 1.5, 1.5, 1.5] diff --git a/config/proxy/droid.yaml b/config/proxy/droid.yaml index 4754266..61b5b91 100644 --- a/config/proxy/droid.yaml +++ b/config/proxy/droid.yaml @@ -1,11 +1,15 @@ # Franka Arm Configuration (follower) -node_name: FrankaFollower +node_name: FrankaPanda robot_ip: 10.10.10.202 -proxy_ip: 141.3.53.63 +proxy_ip: 192.168.0.117 group: 224.0.0.1 -group_port: 7721 -group_name: hardware_collection +group_port: 7720 +group_name: DroidGroup -arm_config_path: config/RobotArmConfig_follower.yaml -gripper_config_path: config/GripperConfig_follower.yaml +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/robot/droid_panda.yaml b/config/robot/droid_panda.yaml new file mode 100644 index 0000000..c425ff4 --- /dev/null +++ b/config/robot/droid_panda.yaml @@ -0,0 +1,24 @@ +# Franka Arm Configuration for P10 +name: FrankaPanda +robot_ip: 10.10.10.10 + +# Arm default state +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 +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] \ No newline at end of file diff --git a/real_robot_test.bash b/real_robot_test.bash index 8094b96..95dcde9 100644 --- a/real_robot_test.bash +++ b/real_robot_test.bash @@ -3,4 +3,4 @@ cd build cmake -DLOCAL_TESTING=OFF .. make -j8 cd .. -./build/proxy ./config/ProxyConfig.yaml \ No newline at end of file +./build/proxy ./config/proxy/droid.yaml \ No newline at end of file From e11954da8c79c8a90a0e799a9302d0f7729af39d Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Fri, 6 Feb 2026 17:00:42 +0100 Subject: [PATCH 17/19] fix hybridjointimepdance config --- .../hybrid_joint_impedance_controller.cfg | 21 +++++++------------ src/robots/robotiq_gripper.cpp | 8 +++---- 2 files changed, 11 insertions(+), 18 deletions(-) diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index 02719b5..d8bbdd1 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,26 +1,19 @@ name: HybridJointImpedance -command_topic: joint_position_command +command_topic: FrankaPanda/joint_position_command #polymetis default -kq_gains: [24, 36, 30, 30, 20, 15, 15] -kqd_gains: [3.0, 3.0, 3.0, 3.0, 1.5, 1.5, 1.5] -kx_gains: [60, 60, 60, 30, 30, 30] -kxd_gains: [1.5, 1.5, 1.5, 0.3, 0.3, 0.3] -# kq_gains: [8, 12, 10, 10, 6.5, 4.0, 4.0] -# kqd_gains: [0.25, 0.38, 0.25, 0.25, 0.125, 0.125, 0.125] -# kx_gains: [17.5, 17.5, 17.5, 7, 7, 7] -# kxd_gains: [0.125, 0.125, 0.125, 0.025, 0.025, 0.025] -# kq_gains: [12, 18, 15, 15, 9.5, 6.5, 6.5] -# kqd_gains: [0.7, 0.85, 0.6, 0.6, 0.35, 0.35, 0.35] -# kx_gains: [35, 35, 35, 14, 14, 14] -# kxd_gains: [0.45, 0.35, 0.35, 0.15, 0.15, 0.15] +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 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 diff --git a/src/robots/robotiq_gripper.cpp b/src/robots/robotiq_gripper.cpp index 7c07370..f1aa34d 100644 --- a/src/robots/robotiq_gripper.cpp +++ b/src/robots/robotiq_gripper.cpp @@ -60,8 +60,8 @@ void RobotiqGripper::controlLoopThread() 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); + // 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; } @@ -104,6 +104,6 @@ void RobotiqGripper::statePubThread() 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); + // zlc::info("RobotiqGripper received new command: Pos {:.2f}, Speed {:.2f}, Force {:.2f}, Blocking {}", + // cmd.position, cmd.speed, cmd.force, cmd.blocking); } From 68a90ef5be1f547d9011fd5347df35f87343c655 Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Tue, 10 Feb 2026 14:58:47 +0100 Subject: [PATCH 18/19] fix config --- config/controller/hybrid_joint_impedance_controller.cfg | 8 ++++---- config/proxy/droid.yaml | 3 +-- 2 files changed, 5 insertions(+), 6 deletions(-) diff --git a/config/controller/hybrid_joint_impedance_controller.cfg b/config/controller/hybrid_joint_impedance_controller.cfg index d8bbdd1..99a7809 100644 --- a/config/controller/hybrid_joint_impedance_controller.cfg +++ b/config/controller/hybrid_joint_impedance_controller.cfg @@ -1,10 +1,10 @@ name: HybridJointImpedance command_topic: FrankaPanda/joint_position_command #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 +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] ignore_gravity: true # use for now diff --git a/config/proxy/droid.yaml b/config/proxy/droid.yaml index 61b5b91..3a3d870 100644 --- a/config/proxy/droid.yaml +++ b/config/proxy/droid.yaml @@ -3,10 +3,9 @@ node_name: FrankaPanda robot_ip: 10.10.10.202 proxy_ip: 192.168.0.117 group: 224.0.0.1 -group_port: 7720 +group_port: 7727 group_name: DroidGroup - robot: - type: panda config_path: config/robot/droid_panda.yaml From 1e4fa6e87e6717345126fae7d9fe577285e9c956 Mon Sep 17 00:00:00 2001 From: ZhuoyueLi Date: Fri, 13 Feb 2026 13:42:21 +0100 Subject: [PATCH 19/19] fix droid config --- config/gripper/droid_robotiq.yaml | 2 +- config/proxy/droid.yaml | 4 ++-- 2 files changed, 3 insertions(+), 3 deletions(-) diff --git a/config/gripper/droid_robotiq.yaml b/config/gripper/droid_robotiq.yaml index 01d2d3c..cedb179 100644 --- a/config/gripper/droid_robotiq.yaml +++ b/config/gripper/droid_robotiq.yaml @@ -2,7 +2,7 @@ name: FrankaPanda # Serial communication -gripper_port: /dev/ttyUSB1 +gripper_port: /dev/ttyUSB0 gripper_baud: 115200 # Scaling (see robotiq interface docs) diff --git a/config/proxy/droid.yaml b/config/proxy/droid.yaml index 3a3d870..57c310d 100644 --- a/config/proxy/droid.yaml +++ b/config/proxy/droid.yaml @@ -1,9 +1,9 @@ # Franka Arm Configuration (follower) node_name: FrankaPanda robot_ip: 10.10.10.202 -proxy_ip: 192.168.0.117 +proxy_ip: 192.168.0.232 group: 224.0.0.1 -group_port: 7727 +group_port: 7730 group_name: DroidGroup robot: