From b38b2a229cdc9025356c975a944a2715ead437f4 Mon Sep 17 00:00:00 2001
From: hph <kjustdoitno1@gmail.com>
Date: Fri, 16 May 2025 11:00:37 +0800
Subject: [PATCH] add gripper bindings

---
 build_install.sh            | 12 ++++++++
 franka_bindings/__init__.py |  4 +++
 include/franka_bindings.h   | 24 ++++++++++++++++
 src/franka_bindings.cpp     | 56 ++++++++++++++++++++++++++++++++++++-
 4 files changed, 95 insertions(+), 1 deletion(-)
 create mode 100644 build_install.sh

diff --git a/build_install.sh b/build_install.sh
new file mode 100644
index 0000000..981dc79
--- /dev/null
+++ b/build_install.sh
@@ -0,0 +1,12 @@
+#!/bin/bash
+#
+
+if [ ! -d "build" ]; then
+    mkdir build
+fi
+
+cd build
+cmake ..
+make -j8
+cd ..
+pip install -e .
diff --git a/franka_bindings/__init__.py b/franka_bindings/__init__.py
index 21806b8..724dd59 100644
--- a/franka_bindings/__init__.py
+++ b/franka_bindings/__init__.py
@@ -1,4 +1,6 @@
 from .franka_bindings import (
+    Gripper,
+    GripperState,
     Robot,
     ControllerMode,
     Torques,
@@ -10,6 +12,8 @@
 )
 
 __all__ = [
+    'Gripper',
+    'GripperState',
     'Robot',
     'ControllerMode',
     'Torques',
diff --git a/include/franka_bindings.h b/include/franka_bindings.h
index 706050c..867378b 100644
--- a/include/franka_bindings.h
+++ b/include/franka_bindings.h
@@ -6,6 +6,7 @@
 
 #include <Eigen/Core>
 #include <franka/robot.h>
+#include <franka/gripper.h>
 #include <franka/active_control_base.h>
 #include <franka/active_control.h>
 #include <franka/active_motion_generator.h>
@@ -52,4 +53,27 @@ class PyRobot {
     std::unique_ptr<RealtimeControl> realtime_control_;
 };
 
+class PyGripper {
+public:
+    explicit PyGripper(const std::string& franka_address);
+    ~PyGripper() = default;
+
+    bool homing();
+    bool grasp(double width,
+             double speed,
+             double force,
+             double epsilon_inner = 0.005,
+             double epsilon_outer = 0.005);
+
+    franka::GripperState readOnce();
+
+    bool stop();
+    bool move(double width, double speed);
+    franka::Gripper::ServerVersion serverVersion();
+    
+
+private:
+    std::unique_ptr<franka::Gripper> gripper_;
+};
+
 } // namespace franka_bindings 
\ No newline at end of file
diff --git a/src/franka_bindings.cpp b/src/franka_bindings.cpp
index a0c22d3..776a003 100644
--- a/src/franka_bindings.cpp
+++ b/src/franka_bindings.cpp
@@ -78,6 +78,42 @@ RealtimeControl* PyRobot::getRealtimeControl() {
     return realtime_control_.get();
 }
 
+PyGripper::PyGripper(const std::string& franka_address) 
+    : gripper_(std::make_unique<franka::Gripper>(franka_address)) {}
+
+bool PyGripper::homing()
+{
+    return gripper_->homing();
+}
+
+bool PyGripper::grasp(double width,
+            double speed,
+            double force,
+            double epsilon_inner,
+            double epsilon_outer)
+{
+    return gripper_->grasp(width, speed, force, epsilon_inner, epsilon_outer);    
+}
+
+franka::GripperState PyGripper::readOnce()
+{
+    return gripper_->readOnce();
+}
+
+bool PyGripper::stop()
+{
+    return gripper_->stop();
+}
+bool PyGripper::move(double width, double speed)
+{
+    return gripper_->move(width, speed);
+}
+
+franka::Gripper::ServerVersion PyGripper::serverVersion()
+{
+    return gripper_->serverVersion();
+}
+
 PYBIND11_MODULE(franka_bindings, m) {
     // Bind exceptions
     py::register_exception<franka::Exception>(m, "FrankaException");
@@ -115,7 +151,7 @@ PYBIND11_MODULE(franka_bindings, m) {
         .def_readwrite("O_T_EE_d", &franka::RobotState::O_T_EE_d)
         .def_readwrite("F_T_EE", &franka::RobotState::F_T_EE)
         .def_readwrite("EE_T_K", &franka::RobotState::EE_T_K);
-
+    
     // Bind ActiveControlBase
     py::class_<franka::ActiveControlBase>(m, "ActiveControlBase")
         .def("readOnce", [](franka::ActiveControlBase& self) {
@@ -177,6 +213,24 @@ PYBIND11_MODULE(franka_bindings, m) {
             self.startRealtimeControl();
         })
         .def("get_realtime_control", &PyRobot::getRealtimeControl, py::return_value_policy::reference);
+    
+    // Bind franka::GripperState
+    py::class_<franka::GripperState>(m, "GripperState")
+        .def_readwrite("width", &franka::GripperState::width)
+        .def_readwrite("max_width", &franka::GripperState::max_width)
+        .def_readwrite("is_grasped", &franka::GripperState::is_grasped)
+        .def_readwrite("temperature", &franka::GripperState::temperature)
+        .def_readwrite("time", &franka::GripperState::time);
+
+    // Bind PyGripper
+    py::class_<PyGripper>(m, "Gripper")
+        .def(py::init<const std::string&>())
+        .def("homing", &PyGripper::homing)
+        .def("grasp", &PyGripper::grasp)
+        .def("read_once", &PyGripper::readOnce)
+        .def("stop", &PyGripper::stop)
+        .def("move", &PyGripper::move)
+        .def("server_version", &PyGripper::serverVersion);
 
     // Add docstring
     m.doc() = "Python bindings for libfranka realtime control";