feat:add touch align_mode
This commit is contained in:
parent
4f84994e6b
commit
9faa2a3852
@ -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()};
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
@ -58,7 +58,7 @@
|
||||
<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"/>
|
||||
</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="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"/>
|
||||
@ -73,27 +73,27 @@
|
||||
</Robot>
|
||||
|
||||
<BioHead>
|
||||
<!-- <esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">-->
|
||||
<esp32 id="bio_head" serial="/dev/ttyUSB0" ctrlFreq="50">
|
||||
|
||||
<!-- <!– 眉毛 –>-->
|
||||
<!-- <EyeBrow serial="64:0~3"-->
|
||||
<!-- offest="90 90 90 90"-->
|
||||
<!-- jLmtUp="90 170 155 110"-->
|
||||
<!-- jLmtLow="20 77 90 20"/>-->
|
||||
<!-- 眉毛 -->
|
||||
<EyeBrow serial="64:0~3"
|
||||
offest="90 90 90 90"
|
||||
jLmtUp="90 170 155 110"
|
||||
jLmtLow="20 77 90 20"/>
|
||||
|
||||
<!-- <!– 眼睛 –>-->
|
||||
<!-- <Eye serial="64:4~9"-->
|
||||
<!-- offest="90 90 90 90 90 90"-->
|
||||
<!-- jLmtUp="90 150 165 90 120 115"-->
|
||||
<!-- jLmtLow="20 90 90 25 70 75"/>-->
|
||||
<!-- 眼睛 -->
|
||||
<Eye serial="64:4~9"
|
||||
offest="90 90 90 90 90 90"
|
||||
jLmtUp="90 150 165 90 120 115"
|
||||
jLmtLow="20 90 90 25 70 75"/>
|
||||
|
||||
<!-- <!– 嘴巴 –>-->
|
||||
<!-- <Mouth serial="65:0~9"-->
|
||||
<!-- offest="90 90 90 90 90 90 90 90 90 90"-->
|
||||
<!-- jLmtUp="150 110 130 140 100 105 110 125 90 95"-->
|
||||
<!-- jLmtLow="70 30 80 80 65 55 45 80 85 90"/>-->
|
||||
<!-- 嘴巴 -->
|
||||
<Mouth serial="65:0~9"
|
||||
offest="90 90 90 90 90 90 90 90 90 90"
|
||||
jLmtUp="150 110 130 140 100 105 110 125 90 95"
|
||||
jLmtLow="70 30 80 80 65 55 45 80 85 90"/>
|
||||
|
||||
<!-- </esp32>-->
|
||||
</esp32>
|
||||
</BioHead >
|
||||
|
||||
<Microphone>
|
||||
|
||||
@ -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
|
||||
|
||||
|
||||
@ -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;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user