diff --git a/example/servo-franka/frankaGripper.cpp b/example/servo-franka/frankaGripper.cpp index 5b98186c5f..cfc66a1888 100644 --- 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 ]" + << " [--ip ]" << " [--home]" << " [--open]" << " [--close]" << " [--grasp ]" << " [--grasp-speed ]" - << " [--grasp-force ]" + << " [--grasp-force ]" << " [--release]" << " [--test ]" << " [--help] [-h]\n" << std::endl; diff --git a/example/servo-franka/servoFrankaIBVS.cpp b/example/servo-franka/servoFrankaIBVS.cpp index 6b89eaf987..1fd7d89f2f 100644 --- a/example/servo-franka/servoFrankaIBVS.cpp +++ b/example/servo-franka/servoFrankaIBVS.cpp @@ -116,14 +116,17 @@ 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; @@ -131,25 +134,71 @@ 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; } 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 ] [--tag_size ] [--eMc ] " - << "[--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 ]" + << " [--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; } } @@ -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 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 index 1594db60cc..ba6edba196 100644 --- 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 ] [--tag_size ] [--eMc ] " - << "[--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 ]" + << " [--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; } } @@ -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 I(height, width); diff --git a/modules/robot/src/real-robot/franka/vpRobotFranka.cpp b/modules/robot/src/real-robot/franka/vpRobotFranka.cpp index bfcc2de660..69f5090a84 100644 --- 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; }