From 352639159d21d549530b94ff9a487ec2c2ab7cf8 Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sat, 13 Feb 2021 23:09:18 -0700 Subject: [PATCH 1/2] enforce min rotation between samples --- .../src/handeye_control_widget.cpp | 28 +++++++++++++++++++ 1 file changed, 28 insertions(+) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 12ee5b9..8795739 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -39,6 +39,7 @@ namespace moveit_rviz_plugin { const std::string LOGNAME = "handeye_control_widget"; +const double MIN_ROTATION = M_PI / 36.; // Smallest allowed rotation, 5 degrees ProgressBarWidget::ProgressBarWidget(QWidget* parent, int min, int max, int value) : QWidget(parent) { @@ -370,6 +371,33 @@ bool ControlTabWidget::takeTransformSamples() // Get the transform of the end-effector w.r.t the robot base base_to_eef_tf = tf_buffer_->lookupTransform(frame_names_["base"], frame_names_["eef"], ros::Time(0)); + // Verify that sample contains sufficient rotation + Eigen::Isometry3d base_to_eef_eig, camera_to_object_eig; + base_to_eef_eig = tf2::transformToEigen(base_to_eef_tf); + camera_to_object_eig = tf2::transformToEigen(camera_to_object_tf); + + if (!effector_wrt_world_.empty()) + { + Eigen::AngleAxisd eff_rot((base_to_eef_eig.inverse() * effector_wrt_world_.back()).rotation()); + if (eff_rot.angle() < MIN_ROTATION) + { + QMessageBox::warning(this, tr("Error"), + tr("Not enough end-effector rotation since last sample. Sample not recorded.")); + return false; + } + } + + if (!object_wrt_sensor_.empty()) + { + Eigen::AngleAxisd cam_rot((camera_to_object_eig.inverse() * object_wrt_sensor_.back()).rotation()); + if (cam_rot.angle() < MIN_ROTATION) + { + QMessageBox::warning(this, tr("Error"), + tr("Not enough camera rotation since last sample. Sample not recorded.")); + return false; + } + } + // save the pose samples effector_wrt_world_.push_back(tf2::transformToEigen(base_to_eef_tf)); object_wrt_sensor_.push_back(tf2::transformToEigen(camera_to_object_tf)); From 0cc2b4f6db4e45001b84e9c3ac8a8ddb4472412d Mon Sep 17 00:00:00 2001 From: John Stechschulte Date: Sun, 14 Feb 2021 16:13:03 -0700 Subject: [PATCH 2/2] some improvements --- .../src/handeye_control_widget.cpp | 20 +++++++++---------- 1 file changed, 10 insertions(+), 10 deletions(-) diff --git a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp index 8795739..d76420e 100644 --- a/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp +++ b/moveit_calibration_gui/handeye_calibration_rviz_plugin/src/handeye_control_widget.cpp @@ -376,31 +376,31 @@ bool ControlTabWidget::takeTransformSamples() base_to_eef_eig = tf2::transformToEigen(base_to_eef_tf); camera_to_object_eig = tf2::transformToEigen(camera_to_object_tf); - if (!effector_wrt_world_.empty()) + for (const auto& prior_tf : effector_wrt_world_) { - Eigen::AngleAxisd eff_rot((base_to_eef_eig.inverse() * effector_wrt_world_.back()).rotation()); - if (eff_rot.angle() < MIN_ROTATION) + Eigen::AngleAxisd rot((base_to_eef_eig.inverse() * prior_tf).rotation()); + if (rot.angle() < MIN_ROTATION) { QMessageBox::warning(this, tr("Error"), - tr("Not enough end-effector rotation since last sample. Sample not recorded.")); + tr("End-effector orientation is too similar to a prior sample. Sample not recorded.")); return false; } } - if (!object_wrt_sensor_.empty()) + for (const auto prior_tf : object_wrt_sensor_) { - Eigen::AngleAxisd cam_rot((camera_to_object_eig.inverse() * object_wrt_sensor_.back()).rotation()); - if (cam_rot.angle() < MIN_ROTATION) + Eigen::AngleAxisd rot((camera_to_object_eig.inverse() * prior_tf).rotation()); + if (rot.angle() < MIN_ROTATION) { QMessageBox::warning(this, tr("Error"), - tr("Not enough camera rotation since last sample. Sample not recorded.")); + tr("Camera orientation is too similar to a prior sample. Sample not recorded.")); return false; } } // save the pose samples - effector_wrt_world_.push_back(tf2::transformToEigen(base_to_eef_tf)); - object_wrt_sensor_.push_back(tf2::transformToEigen(camera_to_object_tf)); + effector_wrt_world_.push_back(base_to_eef_eig); + object_wrt_sensor_.push_back(camera_to_object_eig); ControlTabWidget::addPoseSampleToTreeView(camera_to_object_tf, base_to_eef_tf, effector_wrt_world_.size()); }