Merge pull request #1513 from fspindle/feat_franka

Improve Franka Panda robot examples
lagadic · Dec 4, 2024 · f227ddd · f227ddd
2 parents 31d9672 + 55e401a
commit f227ddd
Show file tree

Hide file tree

Showing 4 changed files with 140 additions and 47 deletions.
diff --git a/example/servo-franka/frankaGripper.cpp b/example/servo-franka/frankaGripper.cpp
@@ -68,7 +68,7 @@ int main(int argc, char **argv)
 
   for (int i = 1; i < argc; i++) {
     if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
-      ++ i;
+      ++i;
       opt_robot_ip = std::string(argv[i]);
     }
     else if (std::string(argv[i]) == "--home") {
@@ -80,34 +80,37 @@ int main(int argc, char **argv)
     else if (std::string(argv[i]) == "--close") {
       opt_gripper_state = Gripper_Close;
     }
+    else if (std::string(argv[i]) == "--release") {
+      opt_gripper_state = Gripper_Release;
+    }
     else if (std::string(argv[i]) == "--grasp" && i + 1 < argc) {
-      ++ i;
+      ++i;
       opt_gripper_state = Gripper_Grasp;
       opt_grasping_width = std::atof(argv[i]);
     }
     else if (std::string(argv[i]) == "--grasp-speed" && i + 1 < argc) {
-      ++ i;
+      ++i;
       opt_grasping_speed = std::atof(argv[i]);
     }
     else if (std::string(argv[i]) == "--grasp-force" && i + 1 < argc) {
-      ++ i;
+      ++i;
       opt_grasping_force = std::atof(argv[i]);
     }
     else if (std::string(argv[i]) == "--test" && i + 1 < argc) {
-      ++ i;
+      ++i;
       opt_gripper_state = Gripper_Test;
       opt_grasping_width = std::atof(argv[i]);
     }
     else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
       std::cout << "SYNOPSYS" << std::endl
         << "  " << argv[0]
-        << " [--ip <default " << opt_robot_ip << ">]"
+        << " [--ip <controller ip>]"
         << " [--home]"
         << " [--open]"
         << " [--close]"
         << " [--grasp <width>]"
         << " [--grasp-speed <speed>]"
-        << " [--grasp-force <speed>]"
+        << " [--grasp-force <force>]"
         << " [--release]"
         << " [--test <width>]"
         << " [--help] [-h]\n" << std::endl;

diff --git a/example/servo-franka/servoFrankaIBVS.cpp b/example/servo-franka/servoFrankaIBVS.cpp
@@ -116,40 +116,89 @@ int main(int argc, char **argv)
   double convergence_threshold = 0.00005;
 
   for (int i = 1; i < argc; i++) {
-    if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
+    if ((std::string(argv[i]) == "--tag-size") && (i + 1 < argc)) {
       opt_tagSize = std::stod(argv[i + 1]);
+      ++i;
     }
-    else if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--ip") && (i + 1 < argc)) {
       opt_robot_ip = std::string(argv[i + 1]);
+      ++i;
     }
-    else if (std::string(argv[i]) == "--eMc" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--eMc") && (i + 1 < argc)) {
       opt_eMc_filename = std::string(argv[i + 1]);
+      ++i;
     }
     else if (std::string(argv[i]) == "--verbose") {
       opt_verbose = true;
     }
     else if (std::string(argv[i]) == "--plot") {
       opt_plot = true;
     }
-    else if (std::string(argv[i]) == "--adaptive_gain") {
+    else if (std::string(argv[i]) == "--adaptive-gain") {
       opt_adaptive_gain = true;
     }
-    else if (std::string(argv[i]) == "--task_sequencing") {
+    else if (std::string(argv[i]) == "--task-sequencing") {
       opt_task_sequencing = true;
     }
-    else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--quad-decimate") && (i + 1 < argc)) {
       opt_quad_decimate = std::stoi(argv[i + 1]);
+      ++i;
     }
     else if (std::string(argv[i]) == "--no-convergence-threshold") {
       convergence_threshold = 0.;
     }
-    else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
-      std::cout
-        << argv[0] << " [--ip <default " << opt_robot_ip << ">] [--tag_size <marker size in meter; default "
-        << opt_tagSize << ">] [--eMc <eMc extrinsic file>] "
-        << "[--quad_decimate <decimation; default " << opt_quad_decimate
-        << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
-        << "\n";
+    else if ((std::string(argv[i]) == "--help") || (std::string(argv[i]) == "-h")) {
+      std::cout << "SYNOPSYS" << std::endl
+        << "  " << argv[0]
+        << " [--ip <controller ip>]"
+        << " [--tag-size <size>]"
+        << " [--eMc <extrinsic transformation file>]"
+        << " [--quad-decimate <decimation factor>]"
+        << " [--adaptive-gain]"
+        << " [--plot]"
+        << " [--task-sequencing]"
+        << " [--no-convergence-threshold]"
+        << " [--verbose]"
+        << " [--help] [-h]\n"
+        << std::endl;
+      std::cout << "DESCRIPTION" << std::endl
+        << "  Use an image-based visual-servoing scheme to position the camera in front of an Apriltag." << std::endl
+        << std::endl
+        << "  --ip <controller ip>" << std::endl
+        << "    Franka controller ip address" << std::endl
+        << "    Default: " << opt_robot_ip << std::endl
+        << std::endl
+        << "  --tag-size <size>" << std::endl
+        << "    Apriltag size in [m]." << std::endl
+        << "    Default: " << opt_tagSize << " [m]" << std::endl
+        << std::endl
+        << "  --eMc <extrinsic transformation file>" << std::endl
+        << "    File containing the homogeneous transformation matrix between" << std::endl
+        << "    robot end-effector and camera frame." << std::endl
+        << std::endl
+        << "  --quad-decimate <decimation factor>" << std::endl
+        << "    Decimation factor used during Apriltag detection." << std::endl
+        << "    Default: " << opt_quad_decimate << std::endl
+        << std::endl
+        << "  --adaptive-gain" << std::endl
+        << "    Flag to enable adaptive gain to speed up visual servo near convergence." << std::endl
+        << std::endl
+        << "  --plot" << std::endl
+        << "    Flag to enable curve plotter." << std::endl
+        << std::endl
+        << "  --task-sequencing" << std::endl
+        << "    Flag to enable task sequencing scheme." << std::endl
+        << std::endl
+        << "  --no-convergence-threshold" << std::endl
+        << "    Flag to disable convergence threshold used to stop the visual servo." << std::endl
+        << std::endl
+        << "  --verbose" << std::endl
+        << "    Flag to enable extra verbosity." << std::endl
+        << std::endl
+        << "  --help, -h" << std::endl
+        << "    Print this helper message." << std::endl
+        << std::endl;
+
       return EXIT_SUCCESS;
     }
   }
@@ -182,16 +231,14 @@ int main(int argc, char **argv)
       ePc.loadYAML(opt_eMc_filename, ePc);
     }
     else {
-      std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values."
-        << "\n";
+      std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << std::endl;
     }
     vpHomogeneousMatrix eMc(ePc);
-    std::cout << "eMc:\n" << eMc << "\n";
+    std::cout << "eMc:\n" << eMc << std::endl;
 
     // Get camera intrinsics
-    vpCameraParameters cam =
-      rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion);
-    std::cout << "cam:\n" << cam << "\n";
+    vpCameraParameters cam = rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion);
+    std::cout << "cam:\n" << cam << std::endl;
 
     vpImage<unsigned char> I(height, width);
 
@@ -394,8 +441,7 @@ int main(int argc, char **argv)
 
         if (error < convergence_threshold) {
           has_converged = true;
-          std::cout << "Servo task has converged"
-            << "\n";
+          std::cout << "Servo task has converged" << std::endl;
           vpDisplay::displayText(I, 100, 20, "Servo task has converged", vpColor::red);
         }
         if (first_time) {

diff --git a/example/servo-franka/servoFrankaPBVS.cpp b/example/servo-franka/servoFrankaPBVS.cpp
@@ -114,14 +114,17 @@ int main(int argc, char **argv)
   double convergence_threshold_tu = 0.5;   // Value in [deg]
 
   for (int i = 1; i < argc; i++) {
-    if (std::string(argv[i]) == "--tag_size" && i + 1 < argc) {
+    if ((std::string(argv[i]) == "--tag_size") && (i + 1 < argc)) {
       opt_tagSize = std::stod(argv[i + 1]);
+      ++i;
     }
-    else if (std::string(argv[i]) == "--ip" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--ip") && (i + 1 < argc)) {
       opt_robot_ip = std::string(argv[i + 1]);
+      ++i;
     }
-    else if (std::string(argv[i]) == "--eMc" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--eMc") && (i + 1 < argc)) {
       opt_eMc_filename = std::string(argv[i + 1]);
+      ++i;
     }
     else if (std::string(argv[i]) == "--verbose") {
       opt_verbose = true;
@@ -135,20 +138,65 @@ int main(int argc, char **argv)
     else if (std::string(argv[i]) == "--task_sequencing") {
       opt_task_sequencing = true;
     }
-    else if (std::string(argv[i]) == "--quad_decimate" && i + 1 < argc) {
+    else if ((std::string(argv[i]) == "--quad_decimate") && (i + 1 < argc)) {
       opt_quad_decimate = std::stoi(argv[i + 1]);
+      ++i;
     }
     else if (std::string(argv[i]) == "--no-convergence-threshold") {
       convergence_threshold_t = 0.;
       convergence_threshold_tu = 0.;
     }
-    else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") {
-      std::cout
-        << argv[0] << " [--ip <default " << opt_robot_ip << ">] [--tag_size <marker size in meter; default "
-        << opt_tagSize << ">] [--eMc <eMc extrinsic file>] "
-        << "[--quad_decimate <decimation; default " << opt_quad_decimate
-        << ">] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]"
-        << "\n";
+    else if ((std::string(argv[i]) == "--help") || (std::string(argv[i]) == "-h")) {
+      std::cout << "SYNOPSYS" << std::endl
+        << "  " << argv[0]
+        << " [--ip <controller ip>]"
+        << " [--tag-size <size>]"
+        << " [--eMc <extrinsic transformation file>]"
+        << " [--quad-decimate <decimation factor>]"
+        << " [--adaptive-gain]"
+        << " [--plot]"
+        << " [--task-sequencing]"
+        << " [--no-convergence-threshold]"
+        << " [--verbose]"
+        << " [--help] [-h]\n"
+        << std::endl;
+      std::cout << "DESCRIPTION" << std::endl
+        << "  Use a position-based visual-servoing scheme to position the camera in front of an Apriltag." << std::endl
+        << std::endl
+        << "  --ip <controller ip>" << std::endl
+        << "    Franka controller ip address" << std::endl
+        << "    Default: " << opt_robot_ip << std::endl
+        << std::endl
+        << "  --tag-size <size>" << std::endl
+        << "    Apriltag size in [m]." << std::endl
+        << "    Default: " << opt_tagSize << " [m]" << std::endl
+        << std::endl
+        << "  --eMc <extrinsic transformation file>" << std::endl
+        << "    File containing the homogeneous transformation matrix between" << std::endl
+        << "    robot end-effector and camera frame." << std::endl
+        << std::endl
+        << "  --quad-decimate <decimation factor>" << std::endl
+        << "    Decimation factor used during Apriltag detection." << std::endl
+        << "    Default: " << opt_quad_decimate << std::endl
+        << std::endl
+        << "  --adaptive-gain" << std::endl
+        << "    Flag to enable adaptive gain to speed up visual servo near convergence." << std::endl
+        << std::endl
+        << "  --plot" << std::endl
+        << "    Flag to enable curve plotter." << std::endl
+        << std::endl
+        << "  --task-sequencing" << std::endl
+        << "    Flag to enable task sequencing scheme." << std::endl
+        << std::endl
+        << "  --no-convergence-threshold" << std::endl
+        << "    Flag to disable convergence threshold used to stop the visual servo." << std::endl
+        << std::endl
+        << "  --verbose" << std::endl
+        << "    Flag to enable extra verbosity." << std::endl
+        << std::endl
+        << "  --help, -h" << std::endl
+        << "    Print this helper message." << std::endl
+        << std::endl;
       return EXIT_SUCCESS;
     }
   }
@@ -181,16 +229,14 @@ int main(int argc, char **argv)
       ePc.loadYAML(opt_eMc_filename, ePc);
     }
     else {
-      std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values."
-        << "\n";
+      std::cout << "Warning, opt_eMc_filename is empty! Use hard coded values." << std::endl;
     }
     vpHomogeneousMatrix eMc(ePc);
-    std::cout << "eMc:\n" << eMc << "\n";
+    std::cout << "eMc:\n" << eMc << std::endl;
 
     // Get camera intrinsics
-    vpCameraParameters cam =
-      rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion);
-    std::cout << "cam:\n" << cam << "\n";
+    vpCameraParameters cam = rs.getCameraParameters(RS2_STREAM_COLOR, vpCameraParameters::perspectiveProjWithDistortion);
+    std::cout << "cam:\n" << cam << std::endl;
 
     vpImage<unsigned char> I(height, width);
 

diff --git a/modules/robot/src/real-robot/franka/vpRobotFranka.cpp b/modules/robot/src/real-robot/franka/vpRobotFranka.cpp
@@ -104,8 +104,6 @@ vpRobotFranka::~vpRobotFranka()
     delete m_handler;
 
   if (m_gripper) {
-    std::cout << "Grasped object, will release it now." << std::endl;
-    m_gripper->stop();
     delete m_gripper;
   }