diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h
index 586065ba..6b23e7d1 100644
--- a/cmvr-es/applications/include/touch_screen_app.h
+++ b/cmvr-es/applications/include/touch_screen_app.h
@@ -65,6 +65,12 @@ public:
THUMB_MIDDLE
};
+ enum class AlignMode {
+ POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
+ RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
+ POSITION_ONLY // 保留锁定时看到的完整 tag 姿态，只按位置对齐。
+ };
+
struct Options {
// 是否在触控流程开始前先回到指定初始关节位姿。
bool move_to_init_position_before_start{false};
@@ -106,9 +112,8 @@ 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};
+ // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。
+ AlignMode align_mode{AlignMode::POSE_AND_POSITION};
// IBVS 参数。
// 视觉伺服增益 lambda。
@@ -302,8 +307,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 locked_target_rotation_valid_{false};
+ Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
bool touch_start_position_valid_{false};
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp
index 9bfa5ce6..d8a4155e 100644
--- a/cmvr-es/applications/src/touch_screen_app.cpp
+++ b/cmvr-es/applications/src/touch_screen_app.cpp
@@ -214,6 +214,18 @@ TouchScreenApp::TactileRegion toTactileRegion(
return TouchScreenApp::TactileRegion::TIP;
}
+TouchScreenApp::AlignMode toAlignMode(const cmvr::config::TouchScreenAlignMode mode) {
+ switch (mode) {
+ case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
+ return TouchScreenApp::AlignMode::POSE_AND_POSITION;
+ case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
+ return TouchScreenApp::AlignMode::RX_RY_AND_POSITION;
+ case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY:
+ return TouchScreenApp::AlignMode::POSITION_ONLY;
+ }
+ return TouchScreenApp::AlignMode::POSE_AND_POSITION;
+}
+
void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src,
Eigen::Vector3d& dst) {
if (src.has_x()) {
@@ -529,8 +541,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;
+ locked_target_rotation_valid_ = false;
+ locked_target_rotation_.setIdentity();
touch_start_position_valid_ = false;
touch_start_position_base_.setZero();
phase_ = Phase::ALIGNING;
@@ -610,8 +622,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;
+ locked_target_rotation_valid_ = false;
+ locked_target_rotation_.setIdentity();
touch_start_position_valid_ = false;
touch_start_position_base_.setZero();
last_status_ = Status::STOPPED;
@@ -718,8 +730,8 @@ 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_align_mode()) {
+ options.align_mode = toAlignMode(config.align_mode());
}
if (config.has_ibvs_lambda()) {
@@ -1022,27 +1034,40 @@ bool TouchScreenApp::stepAligning() {
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;
+ const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
+ switch (options_.align_mode) {
+ case AlignMode::POSE_AND_POSITION:
+ break;
+ case AlignMode::RX_RY_AND_POSITION:
+ if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
+ const Eigen::Matrix3d R_delta = R_target.transpose() * R_current;
+ const double locked_yaw_rad = std::atan2(R_delta(1, 0), R_delta(0, 0));
+ if (!std::isfinite(locked_yaw_rad)) {
+ enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
+ return false;
+ }
+ const Eigen::Matrix3d Rz_locked =
+ Eigen::AngleAxisd(locked_yaw_rad, Eigen::Vector3d::UnitZ()).toRotationMatrix();
+ locked_target_rotation_ = R_target * Rz_locked;
+ locked_target_rotation_valid_ = true;
}
- 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);
+ R_target = locked_target_rotation_;
+ break;
+ case AlignMode::POSITION_ONLY:
+ // True position-only mode: keep the desired orientation equal to the
+ // current tag orientation every frame so IBVS does not actively try to
+ // correct rotational error.
+ R_target = R_current;
+ break;
}
+ const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target);
ibvs_.setTrackedTagId(tag_id);
- if (!ibvs_target_initialized_ || tracker_.lastSwitched()) {
+ const bool refresh_target = !ibvs_target_initialized_ || tracker_.lastSwitched() ||
+ options_.align_mode == AlignMode::POSITION_ONLY;
+ if (refresh_target) {
if (!ibvs_.setTargetFromPointInTag(p_t_target,
options_.hover_target_in_camera,
target_rotvec.x(),
@@ -1088,15 +1113,28 @@ bool TouchScreenApp::stepAligning() {
}
last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera;
- const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
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] &&
+ bool align_ok =
+ 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]) {
+ std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2];
+ switch (options_.align_mode) {
+ case AlignMode::POSE_AND_POSITION:
+ align_ok = align_ok &&
+ 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];
+ break;
+ case AlignMode::RX_RY_AND_POSITION:
+ align_ok = align_ok &&
+ std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] &&
+ std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4];
+ break;
+ case AlignMode::POSITION_ONLY:
+ break;
+ }
+ if (align_ok) {
++align_stable_count_;
} else {
align_stable_count_ = 0;
diff --git a/cmvr-es/common/config/cabin_robot.xml b/cmvr-es/common/config/cabin_robot.xml
index fa833a13..25928296 100644
--- a/cmvr-es/common/config/cabin_robot.xml
+++ b/cmvr-es/common/config/cabin_robot.xml
@@ -58,7 +58,7 @@
-
+
@@ -73,27 +73,27 @@
-
+
-
-
-
-
-
+
+
-
-
-
-
-
+
+
-
-
-
-
-
+
+
-
+
diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
index df778a62..410d517c 100644
--- a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
+++ b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
@@ -51,15 +51,15 @@ hover_target_in_camera {
target_rx: 3.14159265358979323846
target_ry: 0.0
target_rz: 0.0
-lock_tag_yaw_on_target_start: true
+align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
ibvs_lambda: 0.3
ibvs_mu: 0.1
-ibvs_qdot_max: 0.15
+ibvs_qdot_max: 0.35
ibvs_vmax6 {
- vx: 0.15
- vy: 0.15
- vz: 0.20
+ vx: 0.55
+ vy: 0.55
+ vz: 0.55
wx: 0.6
wy: 0.6
wz: 0.6
@@ -89,14 +89,14 @@ control_joint_names: "R_WRIST_Y"
control_joint_names: "R_WRIST_R"
align_error_threshold6 {
- x: 0.003
- y: 0.003
- z: 0.003
+ x: 0.006
+ y: 0.006
+ z: 0.006
rx: 0.1026646259971647
ry: 0.1026646259971647
rz: 0.1026646259971647
}
-align_stable_frames: 3
+align_stable_frames: 2
align_timeout_s: 30.0
pause_after_align_reached: false
diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
index 3dcb6324..3c4f2dc5 100644
--- a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
+++ b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
@@ -70,6 +70,12 @@ enum TouchScreenTactileRegion {
TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4;
}
+enum TouchScreenAlignMode {
+ TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION = 0;
+ TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION = 1;
+ TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY = 2;
+}
+
message TouchScreenAppConfig {
optional string robot_id = 40;
optional string dexhand_id = 41;
@@ -93,7 +99,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 TouchScreenAlignMode align_mode = 54;
optional double ibvs_lambda = 12;
optional double ibvs_mu = 13;