Reworked gripper API

TimSchneider42 · Jun 4, 2024 · 11506e6 · 11506e6
1 parent 2b003e9
commit 11506e6
Show file tree

Hide file tree

Showing 5 changed files with 108 additions and 180 deletions.
diff --git a/franky/__init__.py b/franky/__init__.py
@@ -1,4 +1,3 @@
-from .gripper import Gripper
 from .robot import Robot
 from .robot_web_session import RobotWebSession
 from .reaction import (
@@ -35,6 +34,7 @@
     LinearMotion,
     CartesianPoseStopMotion,
     JointPositionStopMotion,
+    Gripper,
     Kinematics,
     RobotPose,
     RobotVelocity,

diff --git a/franky/gripper.py b/franky/gripper.py
diff --git a/include/franky/gripper.hpp b/include/franky/gripper.hpp
@@ -1,7 +1,5 @@
 #pragma once
 
-#include <cmath>
-#include <iostream>
 #include <future>
 
 #include <franka/exception.h>
@@ -16,85 +14,94 @@ struct GripperException : public std::runtime_error {
   using std::runtime_error::runtime_error;
 };
 
+/**
+ * @brief A wrapper around the franka::Gripper class that adds asynchronous functionality.
+ */
 class Gripper : public franka::Gripper {
  public:
-  // Maximum speed of the gripper [m/s]
-  static constexpr double max_speed{0.1};
-
-  /**
-   * @param fci_hostname The hostname of the Franka robot.
-   * @param speed The speed of the gripper. Default is 0.04.
-   * @param force The force of the gripper. Default is 20.0.
-   */
-  explicit Gripper(const std::string &fci_hostname, double speed = 0.04, double force = 20.0);
-
-  // Force of the gripper [N]
-  double gripper_force{20.0};
-
-  // Speed of the gripper [m/s]
-  double gripper_speed{0.04};
+  explicit Gripper(const std::string &franka_address) : franka::Gripper(franka_address) {}
 
-  // Flag to indicate if the gripper has an error
-  bool has_error{false};
+  Gripper(Gripper &&gripper) noexcept: franka::Gripper(std::move(gripper)) {}
 
-  // Maximum width of the gripper [m]
-  const double max_width{0.081 + width_calibration};
-
-  /*
-   * @brief Get the current width of the gripper.
+  /**
+   * @brief Asynchronous variant of the franka::Gripper::grasp function.
+   *
+   * @param width Size of the object to grasp in [m].
+   * @param speed Closing speed in [m/s].
+   * @param force Grasping force in [N].
+   * @param epsilon_inner Maximum tolerated deviation when the actual grasped width is smaller than the commanded grasp
+   * width.
+   * @param epsilon_outer Maximum tolerated deviation when the actual grasped width is larger than the commanded grasp
+   * width.
+   * @return Future that becomes ready when the grasp is finished. Contains true if an object has been grasped, false
+   * otherwise.
    */
-  [[nodiscard]] double width() const;
+  std::future<bool> graspAsync(
+      double width, double speed, double force, double epsilon_inner = 0.005, double epsilon_outer = 0.005) const;
 
-  /*
-   * @brief Check if the gripper is grasping.
+  /**
+   * @brief Asynchronous variant of the franka::Gripper::move function.
+   * @param width Intended opening width in [m].
+   * @param speed Speed of the movement in [m/s].
+   * @return Future that becomes ready when the movement is finished. Contains true if the movement was successful,
    */
-  [[nodiscard]] bool isGrasping() const;
+  std::future<bool> moveAsync(double width, double speed) const;
 
-  /*
-   * @brief Move the gripper to the given width.
-   *
-   * @param width The width to move to [m]
-   * @return True if the gripper moved successfully.
+  /**
+   * @brief Opens the gripper fully.
+   * @param speed Speed of the movement in [m/s].
+   * @return True if the gripper was opened successfully.
    */
-  bool move(double width);
+  bool open(double speed);
 
-  /*
-   * @brief Move the gripper to the given width without checking the current width.
-   *
-   * @param width The width to move to [m]
-   * @return True if the gripper moved successfully.
+  /**
+   * @brief Asynchronous variant of the open function.
+   * @param speed Speed of the movement in [m/s].
+   * @return Future that becomes ready when the gripper is fully opened. Contains true if the gripper was opened
+   * successfully.
    */
-  bool moveUnsafe(double width);
+  std::future<bool> openAsync(double speed);
 
-  /*
-   * @brief Move the gripper to the given width asynchronously.
-   *
-   * @param width The width to move to [m]
-   * @return A future that will be set to true if the gripper moved successfully.
+  /**
+   * @brief Asynchronous variant of the franka::Gripper::homing function.
+   * @return A future that becomes ready when the homing is finished. Contains true if the homing was successful.
    */
-  std::future<bool> moveAsync(double width);
+  std::future<bool> homingAsync();
 
-  bool open();
-  bool clamp();
-  bool clamp(double min_clamping_width);
+  /**
+    * @brief Asynchronous variant of the franka::Gripper::stop function.
+    * @return A future that becomes ready when the stop is finished. Contains true if the stop was successful.
+    */
+  std::future<bool> stopAsync();
 
-  bool release();
-  bool release(double width); // [m]
-  bool releaseRelative(double width); // [m]
+  /**
+   * @brief Current opening width of the gripper [m]
+   */
+  [[nodiscard]] inline double width() const {
+    return state().width;
+  }
 
-  ::franka::GripperState get_state();
+  /**
+   * @brief Whether the gripper is grasping
+   */
+  [[nodiscard]] inline bool is_grasped() const {
+    return state().is_grasped;
+  }
 
- private:
-  // Difference from gripper jaws [m
-  const double width_calibration{0.004};
-  // Minimum width of the gripper [m]
-  const double min_width{0.002};
+  /**
+   * @brief Maximum width of the gripper [m]
+   */
+  [[nodiscard]] inline double max_width() const {
+    return state().max_width;
+  }
 
   /**
-  * Save clamp width and compare it in the is_grasping method. If it's smaller,
-  * the gripper moves and the object is missing. [m]
-  */
-  double last_clamp_width{};
+   * @brief Current gripper state.
+   */
+  [[nodiscard]] inline franka::GripperState state() const {
+    return readOnce();
+  }
+
 };
 
-} // namepace franky
+} // namespace franky
diff --git a/python/python.cpp b/python/python.cpp
@@ -867,32 +867,33 @@ PYBIND11_MODULE(_franky, m) {
              std::shared_ptr<StopMotion<franka::JointPositions>>>(m, "JointPositionStopMotion")
       .def(py::init<>());
 
-  py::class_<Gripper>(m, "GripperInternal")
-      .def(py::init<const std::string &, double, double>(), "fci_hostname"_a, "speed"_a = 0.02, "force"_a = 20.0)
-      .def_readwrite("gripper_force", &Gripper::gripper_force)
-      .def_readwrite("gripper_speed", &Gripper::gripper_speed)
-      .def_readonly("max_width", &Gripper::max_width)
-      .def_readonly("has_error", &Gripper::has_error)
-      .def("homing", &Gripper::homing, py::call_guard<py::gil_scoped_release>())
+  py::class_<Gripper>(m, "Gripper")
+      .def(py::init<const std::string &>(), "fci_hostname"_a)
       .def("grasp", &Gripper::grasp,
-           "width"_a, "speed"_a, "force"_a, "epsilon_inner"_a = 0.005, "epsilon_outer"_a = 0.005,
-           py::call_guard<py::gil_scoped_release>())
-      .def("move", &franka::Gripper::move, "width"_a, "speed"_a, py::call_guard<py::gil_scoped_release>())
+           "width"_a,
+           "speed"_a,
+           "force"_a,
+           "epsilon_inner"_a = 0.005,
+           "epsilon_outer"_a = 0.005)
+      .def("grasp_async", &Gripper::graspAsync,
+           "width"_a,
+           "speed"_a,
+           "force"_a,
+           "epsilon_inner"_a = 0.005,
+           "epsilon_outer"_a = 0.005)
+      .def("move", &Gripper::move, "width"_a, "speed"_a)
+      .def("move_async", &Gripper::moveAsync, "width"_a, "speed"_a)
+      .def("open", &Gripper::open, "speed"_a)
+      .def("open_async", &Gripper::openAsync, "speed"_a)
+      .def("homing", &Gripper::homing)
+      .def("homing_async", &Gripper::homingAsync)
       .def("stop", &Gripper::stop)
-      .def("move", &Gripper::move, "width"_a, py::call_guard<py::gil_scoped_release>())
-      .def("move_unsafe", &Gripper::move, "width"_a, py::call_guard<py::gil_scoped_release>())
-      .def("open", &Gripper::open, py::call_guard<py::gil_scoped_release>())
-      .def("clamp", py::overload_cast<>(&Gripper::clamp), py::call_guard<py::gil_scoped_release>())
-      .def("clamp", py::overload_cast<double>(&Gripper::clamp),
-           "min_clamping_width"_a, py::call_guard<py::gil_scoped_release>())
-      .def("release", py::overload_cast<>(&Gripper::release), py::call_guard<py::gil_scoped_release>())
-      .def("release", py::overload_cast<double>(&Gripper::release),
-           "width"_a, py::call_guard<py::gil_scoped_release>())
-      .def("release_relative", &Gripper::releaseRelative, "width_relative"_a, py::call_guard<py::gil_scoped_release>())
-      .def_property_readonly("state", &Gripper::get_state)
+      .def("stop_async", &Gripper::stopAsync)
+      .def_property_readonly("state", &Gripper::state)
       .def_property_readonly("server_version", (uint16_t (Gripper::*)()) &Gripper::serverVersion)
       .def_property_readonly("width", &Gripper::width)
-      .def_property_readonly("is_grasping", &Gripper::isGrasping);
+      .def_property_readonly("is_grasped", &Gripper::is_grasped)
+      .def_property_readonly("max_width", &Gripper::max_width);
 
   py::class_<Kinematics>(m, "Kinematics")
       .def_static("forward", &Kinematics::forward, "q"_a)

diff --git a/src/gripper.cpp b/src/gripper.cpp
@@ -2,94 +2,29 @@
 
 namespace franky {
 
-Gripper::Gripper(const std::string &fci_hostname, double speed, double force)
-    : franka::Gripper(fci_hostname), gripper_speed(speed), gripper_force(force) {}
-
-double Gripper::width() const {
-  auto state = readOnce();
-  return state.width + width_calibration;
-}
-
-bool Gripper::isGrasping() const {
-  const double current_width = width();
-  const bool libfranka_is_grasped = readOnce().is_grasped;
-  const bool width_is_grasped = std::abs(current_width - last_clamp_width) < 0.003; // [m], magic number
-  const bool width_larger_than_threshold = current_width > 0.005; // [m]
-  return libfranka_is_grasped && width_is_grasped && width_larger_than_threshold;
-}
-
-bool Gripper::move(double width) { // [m]
-  try {
-    const bool result = ((franka::Gripper *) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
-    const double current_width = this->width();
-    if (current_width > 0.01 && std::abs(current_width - width) > 0.01) {
-      has_error = true;
-      throw GripperException(
-          "Gripper does (" + std::to_string(current_width) + ") not do what it should (" + std::to_string(width) +
-              ").");
-      return false;
-    }
-    if (result) {
-      has_error = false;
-    }
-    return result;
-  } catch (franka::Exception const &e) {
-    has_error = true;
-    std::cout << e.what() << std::endl;
-    this->stop();
-    this->homing();
-    return ((franka::Gripper *) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
-  }
-}
-
-bool Gripper::moveUnsafe(double width) { // [m]
-  const bool result_stop = ((franka::Gripper *) this)->stop();
-  return ((franka::Gripper *) this)->move(width - width_calibration, gripper_speed); // [m] [m/s]
-}
-
-std::future<bool> Gripper::moveAsync(double width) { // [m]
-  return std::async(std::launch::async, &Gripper::move, this, width - width_calibration);
-}
-
-bool Gripper::open() {
-  return move(max_width);
-}
-
-bool Gripper::clamp() {
-  const bool success = grasp(min_width, gripper_speed, gripper_force, min_width, 1.0); // [m] [m/s] [N] [m] [m]
-  last_clamp_width = width();
-  return success;
+std::future<bool> Gripper::graspAsync(
+    double width, double speed, double force, double epsilon_inner, double epsilon_outer) const {
+  return std::async(std::launch::async, &Gripper::grasp, this, width, speed, force, epsilon_inner, epsilon_outer);
 }
 
-bool Gripper::clamp(double min_clamping_width) {
-  const bool success = this->grasp(min_clamping_width, gripper_speed, gripper_force, min_width,
-                                   1.0); // [m] [m/s] [N] [m] [m]
-  last_clamp_width = this->width();
-  return success;
+std::future<bool> Gripper::moveAsync(double width, double speed) const {
+  return std::async(std::launch::async, &Gripper::move, this, width, speed);
 }
 
-bool Gripper::release() { // [m]
-  return release(last_clamp_width);
+bool Gripper::open(double speed) {
+  return move(max_width(), speed);
 }
 
-bool Gripper::release(double width) { // [m]
-  try {
-    // stop();
-    return move(width);
-  } catch (franka::Exception const &e) {
-    std::cout << e.what() << std::endl;
-    homing();
-    stop();
-    return move(width);
-  }
+std::future<bool> Gripper::openAsync(double speed) {
+  return std::async(std::launch::async, &Gripper::open, this, speed);
 }
 
-bool Gripper::releaseRelative(double width_relative) { // [m]
-  return release(width() + width_relative);
+std::future<bool> Gripper::homingAsync() {
+  return std::async(std::launch::async, &Gripper::homing, this);
 }
 
-::franka::GripperState Gripper::get_state() {
-  return readOnce();
+std::future<bool> Gripper::stopAsync() {
+  return std::async(std::launch::async, &Gripper::stop, this);
 }
 
-} // namepace franky
+} // namespace franky