Double grasp does not work

[nielsvd]

I noticed that calling the grasp command twice in a row results in the routine waiting for a response for a while and eventually leads to an exception being thrown. Although grasping twice is redundant and not the most common use case. I do think it should not lead to an exception and definitely not to the waiting behavior observed. (Why not simply returning true?).

I'm actually using the gripper through its Action Server in franka_ros and already hacked a check whether the gripper isn't already closed. But since the behavior is also observed in libfranka, I opted to report the issue here. Find an MWE below.

Minimum working example

The code in the bottom of the issue (modification of examples/grasp_object.cpp) demonstrates the reported behavior.

The program will open the gripper with the following command:

./a.out <robot-ip> 1 

To close the gripper one can run:

./a.out <robot-ip> 0

The following sequence of commands works as expected:

./a.out <robot-ip> 1
./a.out <robot-ip> 0
./a.out <robot-ip> 1
./a.out <robot-ip> 1
./a.out <robot-ip> 0

However, if we grasp the object twice in a row, e.g. like so:

./a.out <robot-ip> 1
./a.out <robot-ip> 0
./a.out <robot-ip> 0

the program will first hang for a while and then crash with the following error message

libfranka gripper: Command failed!

I did not run a debugger, but I guess it first gets stuck here and then times out after a while.

code:

#include <iostream>
#include <sstream>
#include <string>

#include <franka/exception.h>
#include <franka/gripper.h>

int main(int argc, char** argv) {
  if (argc != 3) {
    std::cerr << "Usage: " << argv[0] << " <gripper-hostname> <open/close>" << std::endl;
    return -1;
  }
  try {
    franka::Gripper gripper(argv[1]);

    std::stringstream ss(argv[2]);
    bool open_close;
    if (!(ss >> open_close)) {
      std::cerr << "<open/close> must be either 0 (close) or 1 (open)." << std::endl;
      return -1;
    }

    if (open_close) {
      // open
      bool success = gripper.move(0.08, 0.1);

      std::cout << "Opening gripper" << std::endl;
      std::cout << "Success: " << success << std::endl;
    } else {
      // close
      bool success = gripper.grasp(0, 0.1, 30, 0.2, 0.2);

      std::cout << "Closing gripper" << std::endl;
      std::cout << "Success: " << success << std::endl;
    }

    franka::GripperState state = gripper.readOnce();

    std::cout << "Gripper state: " << state << std::endl;

  } catch (franka::Exception const& e) {
    std::cout << e.what() << std::endl;
    return -1;
  }

  return 0;
}