SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

feat:add touch align_mode

Browse Source
This commit is contained in:
lgv 2026-03-27 15:19:30 +08:00
parent 4f84994e6b
commit 9faa2a3852
5 changed files with 109 additions and 60 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

15
cmvr-es/applications/include/touch_screen_app.h
View File

@ -65,6 +65,12 @@ public:
THUMB_MIDDLE 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 { struct Options {
// 是否在触控流程开始前先回到指定初始关节位姿。 // 是否在触控流程开始前先回到指定初始关节位姿。
bool move_to_init_position_before_start{false}; bool move_to_init_position_before_start{false};
@ -106,9 +112,8 @@ public:
double target_ry{0.0}; double target_ry{0.0};
// 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。
double target_rz{0.0}; double target_rz{0.0};
// 为 true 时，在开始锁定目标时记录当前 tag 相对目标法向姿态的平面内旋转角， // 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。
// 后续继续使用配置里的 target_rx/target_ry，仅把 target_rz 替换为这次任务锁定到的 yaw。 AlignMode align_mode{AlignMode::POSE_AND_POSITION};
bool lock_tag_yaw_on_target_start{false};
// IBVS 参数。 // IBVS 参数。
// 视觉伺服增益 lambda。 // 视觉伺服增益 lambda。
@ -302,8 +307,8 @@ private:
double last_touch_pressure_sum_{0.0}; double last_touch_pressure_sum_{0.0};
double last_touch_pressure_peak_{0.0}; double last_touch_pressure_peak_{0.0};
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
bool locked_target_yaw_valid_{false}; bool locked_target_rotation_valid_{false};
double locked_target_yaw_rad_{0.0}; Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
bool touch_start_position_valid_{false}; bool touch_start_position_valid_{false};
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()}; Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};

92
cmvr-es/applications/src/touch_screen_app.cpp
View File

@ -214,6 +214,18 @@ TouchScreenApp::TactileRegion toTactileRegion(
return TouchScreenApp::TactileRegion::TIP; 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, void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src,
Eigen::Vector3d& dst) { Eigen::Vector3d& dst) {
if (src.has_x()) { if (src.has_x()) {
@ -529,8 +541,8 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
last_touch_pressure_peak_ = 0.0; last_touch_pressure_peak_ = 0.0;
last_active_tag_id_ = -1; last_active_tag_id_ = -1;
last_align_error_camera_.setZero(); last_align_error_camera_.setZero();
locked_target_yaw_valid_ = false; locked_target_rotation_valid_ = false;
locked_target_yaw_rad_ = 0.0; locked_target_rotation_.setIdentity();
touch_start_position_valid_ = false; touch_start_position_valid_ = false;
touch_start_position_base_.setZero(); touch_start_position_base_.setZero();
phase_ = Phase::ALIGNING; phase_ = Phase::ALIGNING;
@ -610,8 +622,8 @@ void TouchScreenApp::stop() {
last_touch_pressure_sum_ = 0.0; last_touch_pressure_sum_ = 0.0;
last_touch_pressure_peak_ = 0.0; last_touch_pressure_peak_ = 0.0;
last_align_error_camera_.setZero(); last_align_error_camera_.setZero();
locked_target_yaw_valid_ = false; locked_target_rotation_valid_ = false;
locked_target_yaw_rad_ = 0.0; locked_target_rotation_.setIdentity();
touch_start_position_valid_ = false; touch_start_position_valid_ = false;
touch_start_position_base_.setZero(); touch_start_position_base_.setZero();
last_status_ = Status::STOPPED; last_status_ = Status::STOPPED;
@ -718,8 +730,8 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
if (config.has_target_rz()) { if (config.has_target_rz()) {
options.target_rz = config.target_rz(); options.target_rz = config.target_rz();
} }
if (config.has_lock_tag_yaw_on_target_start()) { if (config.has_align_mode()) {
options.lock_tag_yaw_on_target_start = config.lock_tag_yaw_on_target_start(); options.align_mode = toAlignMode(config.align_mode());
} }
if (config.has_ibvs_lambda()) { if (config.has_ibvs_lambda()) {
@ -1022,27 +1034,40 @@ bool TouchScreenApp::stepAligning() {
return true; return true;
} }
Eigen::Vector3d target_rotvec(options_.target_rx, options_.target_ry, options_.target_rz);
Eigen::Matrix3d R_target = Eigen::Matrix3d R_target =
rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz); rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz);
if (options_.lock_tag_yaw_on_target_start) { const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0);
if (!locked_target_yaw_valid_) { switch (options_.align_mode) {
const Eigen::Matrix3d R_delta = R_target.transpose() * current_tag->T_c_t.block<3, 3>(0, 0); case AlignMode::POSE_AND_POSITION:
locked_target_yaw_rad_ = std::atan2(R_delta(1, 0), R_delta(0, 0)); break;
if (!std::isfinite(locked_target_yaw_rad_)) { case AlignMode::RX_RY_AND_POSITION:
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); if (!locked_target_rotation_valid_ || tracker_.lastSwitched()) {
return false; 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; R_target = locked_target_rotation_;
} break;
const Eigen::Matrix3d Rz_locked = case AlignMode::POSITION_ONLY:
Eigen::AngleAxisd(locked_target_yaw_rad_, Eigen::Vector3d::UnitZ()).toRotationMatrix(); // True position-only mode: keep the desired orientation equal to the
R_target = R_target * Rz_locked; // current tag orientation every frame so IBVS does not actively try to
target_rotvec = rotvecFromRotationMatrix(R_target); // correct rotational error.
R_target = R_current;
break;
} }
const Eigen::Vector3d target_rotvec = rotvecFromRotationMatrix(R_target);
ibvs_.setTrackedTagId(tag_id); 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, if (!ibvs_.setTargetFromPointInTag(p_t_target,
options_.hover_target_in_camera, options_.hover_target_in_camera,
target_rotvec.x(), target_rotvec.x(),
@ -1088,15 +1113,28 @@ bool TouchScreenApp::stepAligning() {
} }
last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera; 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 = const Eigen::Vector3d rot_error_vec =
rotvecFromRotationMatrix(R_target.transpose() * R_current); 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_.y()) <= options_.align_error_threshold6[1] &&
std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2] && std::abs(last_align_error_camera_.z()) <= options_.align_error_threshold6[2];
std::abs(rot_error_vec.x()) <= options_.align_error_threshold6[3] && switch (options_.align_mode) {
std::abs(rot_error_vec.y()) <= options_.align_error_threshold6[4] && case AlignMode::POSE_AND_POSITION:
std::abs(rot_error_vec.z()) <= options_.align_error_threshold6[5]) { 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_; ++align_stable_count_;
} else { } else {
align_stable_count_ = 0; align_stable_count_ = 0;

36
cmvr-es/common/config/cabin_robot.xml
View File

@ -58,7 +58,7 @@
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/> <Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/> <Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan> </RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="false"> <HeadCan id = " " devId = " " channelId ="2" enable="true">
<Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/> <Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
@ -73,27 +73,27 @@
</Robot> </Robot>
<BioHead> <BioHead>
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">--> <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">
<!-- &lt;!&ndash; 眉毛 &ndash;&gt;--> <!-- 眉毛 -->
<!-- <EyeBrow serial="64:0~3"--> <EyeBrow serial="64:0~3"
<!-- offest="90 90 90 90"--> offest="90 90 90 90"
<!-- jLmtUp="90 170 155 110"--> jLmtUp="90 170 155 110"
<!-- jLmtLow="20 77 90 20"/>--> jLmtLow="20 77 90 20"/>
<!-- &lt;!&ndash; 眼睛 &ndash;&gt;--> <!-- 眼睛 -->
<!-- <Eye serial="64:4~9"--> <Eye serial="64:4~9"
<!-- offest="90 90 90 90 90 90"--> offest="90 90 90 90 90 90"
<!-- jLmtUp="90 150 165 90 120 115"--> jLmtUp="90 150 165 90 120 115"
<!-- jLmtLow="20 90 90 25 70 75"/>--> jLmtLow="20 90 90 25 70 75"/>
<!-- &lt;!&ndash; 嘴巴 &ndash;&gt;--> <!-- 嘴巴 -->
<!-- <Mouth serial="65:0~9"--> <Mouth serial="65:0~9"
<!-- offest="90 90 90 90 90 90 90 90 90 90"--> offest="90 90 90 90 90 90 90 90 90 90"
<!-- jLmtUp="150 110 130 140 100 105 110 125 90 95"--> jLmtUp="150 110 130 140 100 105 110 125 90 95"
<!-- jLmtLow="70 30 80 80 65 55 45 80 85 90"/>--> jLmtLow="70 30 80 80 65 55 45 80 85 90"/>
<!-- </esp32>--> </esp32>
</BioHead > </BioHead >
<Microphone> <Microphone>

18
cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
View File

@ -51,15 +51,15 @@ hover_target_in_camera {
target_rx: 3.14159265358979323846 target_rx: 3.14159265358979323846
target_ry: 0.0 target_ry: 0.0
target_rz: 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_lambda: 0.3
ibvs_mu: 0.1 ibvs_mu: 0.1
ibvs_qdot_max: 0.15 ibvs_qdot_max: 0.35
ibvs_vmax6 { ibvs_vmax6 {
vx: 0.15 vx: 0.55
vy: 0.15 vy: 0.55
vz: 0.20 vz: 0.55
wx: 0.6 wx: 0.6
wy: 0.6 wy: 0.6
wz: 0.6 wz: 0.6
@ -89,14 +89,14 @@ control_joint_names: "R_WRIST_Y"
control_joint_names: "R_WRIST_R" control_joint_names: "R_WRIST_R"
align_error_threshold6 { align_error_threshold6 {
x: 0.003 x: 0.006
y: 0.003 y: 0.006
z: 0.003 z: 0.006
rx: 0.1026646259971647 rx: 0.1026646259971647
ry: 0.1026646259971647 ry: 0.1026646259971647
rz: 0.1026646259971647 rz: 0.1026646259971647
} }
align_stable_frames: 3 align_stable_frames: 2
align_timeout_s: 30.0 align_timeout_s: 30.0
pause_after_align_reached: false pause_after_align_reached: false

8
protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
View File

@ -70,6 +70,12 @@ enum TouchScreenTactileRegion {
TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; 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 { message TouchScreenAppConfig {
optional string robot_id = 40; optional string robot_id = 40;
optional string dexhand_id = 41; optional string dexhand_id = 41;
@ -93,7 +99,7 @@ message TouchScreenAppConfig {
optional double target_rx = 9; optional double target_rx = 9;
optional double target_ry = 10; optional double target_ry = 10;
optional double target_rz = 11; 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_lambda = 12;
optional double ibvs_mu = 13; optional double ibvs_mu = 13;