Visual Servoing Platform  version 3.1.0
franka_grasp_object.cpp
1 // Copyright (c) 2017 Franka Emika GmbH
2 // Use of this source code is governed by the Apache-2.0 license, see LICENSE
3 #include <iostream>
4 
5 #include <visp3/core/vpConfig.h>
6 
7 #ifdef VISP_HAVE_FRANKA
8 
9 #include <sstream>
10 #include <string>
11 #include <thread>
12 
13 #include <franka/exception.h>
14 #include <franka/gripper.h>
15 
25 int main(int argc, char **argv)
26 {
27  if (argc != 4) {
28  std::cerr << "Usage: ./grasp_object <gripper-hostname> <homing> <object-width>" << std::endl;
29  return -1;
30  }
31 
32  try {
33  franka::Gripper gripper(argv[1]);
34  double grasping_width = std::stod(argv[3]);
35 
36  std::stringstream ss(argv[2]);
37  bool homing;
38  if (!(ss >> homing)) {
39  std::cerr << "<homing> can be 0 or 1." << std::endl;
40  return -1;
41  }
42 
43  if (homing) {
44  // Do a homing in order to estimate the maximum grasping width with the
45  // current fingers.
46  gripper.homing();
47  }
48 
49  // Check for the maximum grasping width.
50  franka::GripperState gripper_state = gripper.readOnce();
51  if (gripper_state.max_width < grasping_width) {
52  std::cout << "Object is too large for the current fingers on the gripper." << std::endl;
53  return -1;
54  }
55 
56  // Grasp the object.
57  if (!gripper.grasp(grasping_width, 0.1, 300)) {
58  std::cout << "Failed to grasp object." << std::endl;
59  return -1;
60  }
61 
62  // Wait 3s and check afterwards, if the object is still grasped.
63  std::this_thread::sleep_for(std::chrono::duration<double, std::milli>(3000));
64 
65  gripper_state = gripper.readOnce();
66  if (!gripper_state.is_grasped) {
67  std::cout << "Object lost." << std::endl;
68  return -1;
69  }
70 
71  std::cout << "Grasped object, will release it now." << std::endl;
72  gripper.stop();
73  } catch (franka::Exception const &e) {
74  std::cout << e.what() << std::endl;
75  return -1;
76  }
77 
78  return 0;
79 }
80 
81 #else
82 int main() { std::cout << "This example needs libfranka to control Panda robot." << std::endl; }
83 #endif