diff --git a/example/servo-franka/frankaGripper.cpp b/example/servo-franka/frankaGripper.cpp index deeccaccda..cfc66a1888 100644 --- a/example/servo-franka/frankaGripper.cpp +++ b/example/servo-franka/frankaGripper.cpp @@ -64,7 +64,6 @@ int main(int argc, char **argv) double opt_grasping_width = 0.; double opt_grasping_speed = 0.1; double opt_grasping_force = 60; - double opt_grasping_time = 0; GripperState_t opt_gripper_state = Gripper_None; for (int i = 1; i < argc; i++) { @@ -97,10 +96,6 @@ int main(int argc, char **argv) ++i; opt_grasping_force = std::atof(argv[i]); } - else if (std::string(argv[i]) == "--grasp-time" && i + 1 < argc) { - ++i; - opt_grasping_time = std::atof(argv[i]); - } else if (std::string(argv[i]) == "--test" && i + 1 < argc) { ++i; opt_gripper_state = Gripper_Test; @@ -109,14 +104,13 @@ int main(int argc, char **argv) else if (std::string(argv[i]) == "--help" || std::string(argv[i]) == "-h") { std::cout << "SYNOPSYS" << std::endl << " " << argv[0] - << " [--ip ]" + << " [--ip ]" << " [--home]" << " [--open]" << " [--close]" << " [--grasp ]" << " [--grasp-speed ]" << " [--grasp-force ]" - << " [--grasp-time ]" << " [--release]" << " [--test ]" << " [--help] [-h]\n" << std::endl; @@ -149,11 +143,6 @@ int main(int argc, char **argv) << " Force in [N] applied during grasping." << std::endl << " Default: " << opt_grasping_force << " [N]" << std::endl << std::endl - << " --grasp-time " << std::endl - << " Duration in [s] the grasping force will be applied before releasing the object." << std::endl - << " When duration is set to -1, grasping will be applied until stopped by CTRL-C." << std::endl - << " Default: " << opt_grasping_time << " [s]" << std::endl - << std::endl << " --release" << std::endl << " Release an object that is grasped." << std::endl << std::endl @@ -205,16 +194,6 @@ int main(int argc, char **argv) else if (opt_gripper_state == Gripper_Grasp) { std::cout << "Gripper grasp " << opt_grasping_width << "m object width, with speed: " << opt_grasping_speed << " and force: " << opt_grasping_force << "N..." << std::endl; robot.gripperGrasp(opt_grasping_width, opt_grasping_speed, opt_grasping_force); - if (opt_grasping_time < 0) { - std::cout << "Hit CTRL-C to quit. The grasping force will continue to be applied..." << std::endl; - while (1) { - vpTime::sleepMs(1000); - } - } - else { - std::cout << "Wait " << 1000 * opt_grasping_time << " seconds before releasing the object" << std::endl; - vpTime::sleepMs(1000 * opt_grasping_time); - } } else if (opt_gripper_state == Gripper_Release) { std::cout << "Gripper release object..." << std::endl; diff --git a/example/servo-franka/servoFrankaIBVS.cpp b/example/servo-franka/servoFrankaIBVS.cpp index be6503f7ca..1fd7d89f2f 100644 --- a/example/servo-franka/servoFrankaIBVS.cpp +++ b/example/servo-franka/servoFrankaIBVS.cpp @@ -116,7 +116,7 @@ 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; } @@ -134,13 +134,13 @@ int main(int argc, char **argv) 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; } @@ -148,12 +148,57 @@ int main(int argc, char **argv) convergence_threshold = 0.; } else if ((std::string(argv[i]) == "--help") || (std::string(argv[i]) == "-h")) { - std::cout - << argv[0] << " [--ip ] [--tag_size ] [--eMc ] " - << "[--quad_decimate ] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]" + std::cout << "SYNOPSYS" << std::endl + << " " << argv[0] + << " [--ip ]" + << " [--tag-size ]" + << " [--eMc ]" + << " [--quad-decimate ]" + << " [--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 " << std::endl + << " Franka controller ip address" << std::endl + << " Default: " << opt_robot_ip << std::endl + << std::endl + << " --tag-size " << std::endl + << " Apriltag size in [m]." << std::endl + << " Default: " << opt_tagSize << " [m]" << std::endl + << std::endl + << " --eMc " << std::endl + << " File containing the homogeneous transformation matrix between" << std::endl + << " robot end-effector and camera frame." << std::endl + << std::endl + << " --quad-decimate " << 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; } } diff --git a/example/servo-franka/servoFrankaPBVS.cpp b/example/servo-franka/servoFrankaPBVS.cpp index 31a7e44161..ba6edba196 100644 --- a/example/servo-franka/servoFrankaPBVS.cpp +++ b/example/servo-franka/servoFrankaPBVS.cpp @@ -147,11 +147,55 @@ int main(int argc, char **argv) convergence_threshold_tu = 0.; } else if ((std::string(argv[i]) == "--help") || (std::string(argv[i]) == "-h")) { - std::cout - << argv[0] << " [--ip ] [--tag_size ] [--eMc ] " - << "[--quad_decimate ] [--adaptive_gain] [--plot] [--task_sequencing] [--no-convergence-threshold] [--verbose] [--help] [-h]" + std::cout << "SYNOPSYS" << std::endl + << " " << argv[0] + << " [--ip ]" + << " [--tag-size ]" + << " [--eMc ]" + << " [--quad-decimate ]" + << " [--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 " << std::endl + << " Franka controller ip address" << std::endl + << " Default: " << opt_robot_ip << std::endl + << std::endl + << " --tag-size " << std::endl + << " Apriltag size in [m]." << std::endl + << " Default: " << opt_tagSize << " [m]" << std::endl + << std::endl + << " --eMc " << std::endl + << " File containing the homogeneous transformation matrix between" << std::endl + << " robot end-effector and camera frame." << std::endl + << std::endl + << " --quad-decimate " << 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; }