fix:touch app tag rotation
This commit is contained in:
parent
baf883bc27
commit
4f84994e6b
@ -106,6 +106,9 @@ public:
|
||||
double target_ry{0.0};
|
||||
// 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
|
||||
double target_rz{0.0};
|
||||
// 为 true 时，在开始锁定目标时记录当前 tag 相对目标法向姿态的平面内旋转角，
|
||||
// 后续继续使用配置里的 target_rx/target_ry，仅把 target_rz 替换为这次任务锁定到的 yaw。
|
||||
bool lock_tag_yaw_on_target_start{false};
|
||||
|
||||
// IBVS 参数。
|
||||
// 视觉伺服增益 lambda。
|
||||
@ -136,13 +139,12 @@ public:
|
||||
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
|
||||
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"};
|
||||
|
||||
// 视觉对准收敛判据。
|
||||
// 目标点在相机坐标系 x/y 方向的允许误差，单位米。
|
||||
double align_xy_threshold_m{0.003};
|
||||
// 目标点在相机坐标系 z 方向的允许误差，单位米。
|
||||
double align_z_threshold_m{0.010};
|
||||
// tag 当前姿态与目标姿态的允许夹角误差，单位弧度。
|
||||
double align_rot_threshold_rad{0.08726646259971647};
|
||||
// 视觉对准收敛判据 `[x, y, z, rx, ry, rz]`。
|
||||
// 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。
|
||||
std::array<double, 6> align_error_threshold6{{0.003, 0.003, 0.010,
|
||||
0.08726646259971647,
|
||||
0.08726646259971647,
|
||||
0.08726646259971647}};
|
||||
// 连续多少帧都满足阈值，才认为对准完成。
|
||||
int align_stable_frames{5};
|
||||
// 对准阶段超时时间，单位秒。
|
||||
@ -300,6 +302,8 @@ private:
|
||||
double last_touch_pressure_sum_{0.0};
|
||||
double last_touch_pressure_peak_{0.0};
|
||||
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
|
||||
bool locked_target_yaw_valid_{false};
|
||||
double locked_target_yaw_rad_{0.0};
|
||||
bool touch_start_position_valid_{false};
|
||||
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
|
||||
|
||||
|
||||
@ -119,6 +119,14 @@ double rotationErrorRad(const Eigen::Matrix3d& R_current,
|
||||
return std::acos(cos_angle);
|
||||
}
|
||||
|
||||
Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
|
||||
const Eigen::AngleAxisd aa(R);
|
||||
if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) {
|
||||
return Eigen::Vector3d::Zero();
|
||||
}
|
||||
return aa.axis() * aa.angle();
|
||||
}
|
||||
|
||||
bool appendRequestedTactileRegions(
|
||||
const device::AbstractDexHand::FingerType finger,
|
||||
const TouchScreenApp::TactileRegion region,
|
||||
@ -241,6 +249,28 @@ void applyTwist6FromConfig(const cmvr::config::TouchScreenTwist6& src,
|
||||
}
|
||||
}
|
||||
|
||||
void applyErrorThreshold6FromConfig(const cmvr::config::TouchScreenErrorThreshold6& src,
|
||||
Eigen::Matrix<double, 6, 1>& dst) {
|
||||
if (src.has_x()) {
|
||||
dst[0] = src.x();
|
||||
}
|
||||
if (src.has_y()) {
|
||||
dst[1] = src.y();
|
||||
}
|
||||
if (src.has_z()) {
|
||||
dst[2] = src.z();
|
||||
}
|
||||
if (src.has_rx()) {
|
||||
dst[3] = src.rx();
|
||||
}
|
||||
if (src.has_ry()) {
|
||||
dst[4] = src.ry();
|
||||
}
|
||||
if (src.has_rz()) {
|
||||
dst[5] = src.rz();
|
||||
}
|
||||
}
|
||||
|
||||
void applyMatrix3dFromConfig(const cmvr::config::TouchScreenMatrix3d& src,
|
||||
Eigen::Matrix3d& dst) {
|
||||
if (src.has_m00()) {
|
||||
@ -499,6 +529,8 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_active_tag_id_ = -1;
|
||||
last_align_error_camera_.setZero();
|
||||
locked_target_yaw_valid_ = false;
|
||||
locked_target_yaw_rad_ = 0.0;
|
||||
touch_start_position_valid_ = false;
|
||||
touch_start_position_base_.setZero();
|
||||
phase_ = Phase::ALIGNING;
|
||||
@ -578,6 +610,8 @@ void TouchScreenApp::stop() {
|
||||
last_touch_pressure_sum_ = 0.0;
|
||||
last_touch_pressure_peak_ = 0.0;
|
||||
last_align_error_camera_.setZero();
|
||||
locked_target_yaw_valid_ = false;
|
||||
locked_target_yaw_rad_ = 0.0;
|
||||
touch_start_position_valid_ = false;
|
||||
touch_start_position_base_.setZero();
|
||||
last_status_ = Status::STOPPED;
|
||||
@ -684,6 +718,9 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
|
||||
if (config.has_target_rz()) {
|
||||
options.target_rz = config.target_rz();
|
||||
}
|
||||
if (config.has_lock_tag_yaw_on_target_start()) {
|
||||
options.lock_tag_yaw_on_target_start = config.lock_tag_yaw_on_target_start();
|
||||
}
|
||||
|
||||
if (config.has_ibvs_lambda()) {
|
||||
options.ibvs_lambda = config.ibvs_lambda();
|
||||
@ -703,6 +740,28 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
|
||||
options.ibvs_vmax6[static_cast<size_t>(i)] = ibvs_vmax[i];
|
||||
}
|
||||
}
|
||||
if (config.has_align_error_threshold6()) {
|
||||
Eigen::Matrix<double, 6, 1> align_err;
|
||||
align_err << options.align_error_threshold6[0], options.align_error_threshold6[1], options.align_error_threshold6[2],
|
||||
options.align_error_threshold6[3], options.align_error_threshold6[4], options.align_error_threshold6[5];
|
||||
applyErrorThreshold6FromConfig(config.align_error_threshold6(), align_err);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
options.align_error_threshold6[static_cast<size_t>(i)] = align_err[i];
|
||||
}
|
||||
} else {
|
||||
if (config.has_align_xy_threshold_m()) {
|
||||
options.align_error_threshold6[0] = config.align_xy_threshold_m();
|
||||
options.align_error_threshold6[1] = config.align_xy_threshold_m();
|
||||
}
|
||||
if (config.has_align_z_threshold_m()) {
|
||||
options.align_error_threshold6[2] = config.align_z_threshold_m();
|
||||
}
|
||||
if (config.has_align_rot_threshold_rad()) {
|
||||
options.align_error_threshold6[3] = config.align_rot_threshold_rad();
|
||||
options.align_error_threshold6[4] = config.align_rot_threshold_rad();
|
||||
options.align_error_threshold6[5] = config.align_rot_threshold_rad();
|
||||
}
|
||||
}
|
||||
if (config.has_enable_joint_limit_avoidance()) {
|
||||
options.enable_joint_limit_avoidance = config.enable_joint_limit_avoidance();
|
||||
}
|
||||
@ -729,13 +788,16 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
|
||||
}
|
||||
|
||||
if (config.has_align_xy_threshold_m()) {
|
||||
options.align_xy_threshold_m = config.align_xy_threshold_m();
|
||||
options.align_error_threshold6[0] = config.align_xy_threshold_m();
|
||||
options.align_error_threshold6[1] = config.align_xy_threshold_m();
|
||||
}
|
||||
if (config.has_align_z_threshold_m()) {
|
||||
options.align_z_threshold_m = config.align_z_threshold_m();
|
||||
options.align_error_threshold6[2] = config.align_z_threshold_m();
|
||||
}
|
||||
if (config.has_align_rot_threshold_rad()) {
|
||||
options.align_rot_threshold_rad = config.align_rot_threshold_rad();
|
||||
options.align_error_threshold6[3] = config.align_rot_threshold_rad();
|
||||
options.align_error_threshold6[4] = config.align_rot_threshold_rad();
|
||||
options.align_error_threshold6[5] = config.align_rot_threshold_rad();
|
||||
}
|
||||
if (config.has_align_stable_frames()) {
|
||||
options.align_stable_frames = config.align_stable_frames();
|
||||
@ -816,6 +878,11 @@ bool TouchScreenApp::applyOptions() {
|
||||
options_.touch_speedl_forward_l < 0.0) {
|
||||
return false;
|
||||
}
|
||||
for (const double threshold_i : options_.align_error_threshold6) {
|
||||
if (!std::isfinite(threshold_i) || threshold_i < 0.0) {
|
||||
return false;
|
||||
}
|
||||
}
|
||||
if (!options_.touch_use_speedl) {
|
||||
Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
|
||||
if (!computeLinearMoveDeltaTool(options_.touch_twist_base,
|
||||
@ -947,13 +1014,40 @@ bool TouchScreenApp::stepAligning() {
|
||||
return true;
|
||||
}
|
||||
|
||||
const auto* current_tag = perception_->findTag(tag_id);
|
||||
if (!current_tag) {
|
||||
sendZeroJointVelocity();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
}
|
||||
|
||||
Eigen::Vector3d target_rotvec(options_.target_rx, options_.target_ry, options_.target_rz);
|
||||
Eigen::Matrix3d R_target =
|
||||
rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz);
|
||||
if (options_.lock_tag_yaw_on_target_start) {
|
||||
if (!locked_target_yaw_valid_) {
|
||||
const Eigen::Matrix3d R_delta = R_target.transpose() * current_tag->T_c_t.block<3, 3>(0, 0);
|
||||
locked_target_yaw_rad_ = std::atan2(R_delta(1, 0), R_delta(0, 0));
|
||||
if (!std::isfinite(locked_target_yaw_rad_)) {
|
||||
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
|
||||
return false;
|
||||
}
|
||||
locked_target_yaw_valid_ = true;
|
||||
}
|
||||
const Eigen::Matrix3d Rz_locked =
|
||||
Eigen::AngleAxisd(locked_target_yaw_rad_, Eigen::Vector3d::UnitZ()).toRotationMatrix();
|
||||
R_target = R_target * Rz_locked;
|
||||
target_rotvec = rotvecFromRotationMatrix(R_target);
|
||||
}
|
||||
|
||||
ibvs_.setTrackedTagId(tag_id);
|
||||
if (!ibvs_target_initialized_ || tracker_.lastSwitched()) {
|
||||
if (!ibvs_.setTargetFromPointInTag(p_t_target,
|
||||
options_.hover_target_in_camera,
|
||||
options_.target_rx,
|
||||
options_.target_ry,
|
||||
options_.target_rz)) {
|
||||
target_rotvec.x(),
|
||||
target_rotvec.y(),
|
||||
target_rotvec.z())) {
|
||||
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
|
||||
return false;
|
||||
}
|
||||
@ -994,22 +1088,15 @@ bool TouchScreenApp::stepAligning() {
|
||||
}
|
||||
|
||||
last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera;
|
||||
const auto* current_tag = perception_->findTag(tag_id);
|
||||
if (!current_tag) {
|
||||
sendZeroJointVelocity();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
}
|
||||
|
||||
const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
|
||||
const Eigen::Matrix3d R_target =
|
||||
rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz);
|
||||
const double rot_error_rad = rotationErrorRad(R_current, R_target);
|
||||
if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m &&
|
||||
std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m &&
|
||||
std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m &&
|
||||
rot_error_rad <= options_.align_rot_threshold_rad) {
|
||||
const Eigen::Vector3d rot_error_vec =
|
||||
rotvecFromRotationMatrix(R_target.transpose() * R_current);
|
||||
if (std::abs(last_align_error_camera_.x()) <= options_.align_error_threshold6[0] &&
|
||||
std::abs(last_align_error_camera_.y()) <= options_.align_error_threshold6[1] &&
|
||||
std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2] &&
|
||||
std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] &&
|
||||
std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4] &&
|
||||
std::abs(rot_error_vec.z()) <= options_.align_error_threshold6[5]) {
|
||||
++align_stable_count_;
|
||||
} else {
|
||||
align_stable_count_ = 0;
|
||||
|
||||
@ -51,6 +51,7 @@ hover_target_in_camera {
|
||||
target_rx: 3.14159265358979323846
|
||||
target_ry: 0.0
|
||||
target_rz: 0.0
|
||||
lock_tag_yaw_on_target_start: true
|
||||
|
||||
ibvs_lambda: 0.3
|
||||
ibvs_mu: 0.1
|
||||
@ -87,9 +88,14 @@ control_joint_names: "R_WRIST_P"
|
||||
control_joint_names: "R_WRIST_Y"
|
||||
control_joint_names: "R_WRIST_R"
|
||||
|
||||
align_xy_threshold_m: 0.003
|
||||
align_z_threshold_m: 0.003
|
||||
align_rot_threshold_rad: 0.1026646259971647
|
||||
align_error_threshold6 {
|
||||
x: 0.003
|
||||
y: 0.003
|
||||
z: 0.003
|
||||
rx: 0.1026646259971647
|
||||
ry: 0.1026646259971647
|
||||
rz: 0.1026646259971647
|
||||
}
|
||||
align_stable_frames: 3
|
||||
align_timeout_s: 30.0
|
||||
pause_after_align_reached: false
|
||||
|
||||
@ -29,6 +29,15 @@ message TouchScreenMatrix3d {
|
||||
optional double m22 = 9;
|
||||
}
|
||||
|
||||
message TouchScreenErrorThreshold6 {
|
||||
optional double x = 1;
|
||||
optional double y = 2;
|
||||
optional double z = 3;
|
||||
optional double rx = 4;
|
||||
optional double ry = 5;
|
||||
optional double rz = 6;
|
||||
}
|
||||
|
||||
message TouchScreenInitJointPoint {
|
||||
optional string joint_name = 1;
|
||||
optional double rad = 2;
|
||||
@ -84,6 +93,7 @@ message TouchScreenAppConfig {
|
||||
optional double target_rx = 9;
|
||||
optional double target_ry = 10;
|
||||
optional double target_rz = 11;
|
||||
optional bool lock_tag_yaw_on_target_start = 54;
|
||||
|
||||
optional double ibvs_lambda = 12;
|
||||
optional double ibvs_mu = 13;
|
||||
@ -102,6 +112,7 @@ message TouchScreenAppConfig {
|
||||
optional double align_xy_threshold_m = 23;
|
||||
optional double align_z_threshold_m = 24;
|
||||
optional double align_rot_threshold_rad = 25;
|
||||
TouchScreenErrorThreshold6 align_error_threshold6 = 55;
|
||||
optional int32 align_stable_frames = 26;
|
||||
optional double align_timeout_s = 27;
|
||||
optional bool pause_after_align_reached = 28;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user