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

feat(touch): support target pose offsets

Browse Source
This commit is contained in:
lgv 2026-09-09 17:47:47 +08:00
parent 181fb15591
commit 6bfe01b854
3 changed files with 47 additions and 3 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

18
cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt
View File

@ -50,8 +50,24 @@ touch_screen_task {
twist_filter_alpha: 1.0 twist_filter_alpha: 1.0
} }
target { target {
rotation_vector { x: 1.57 y: 0.0 z: 0.0 } rotation_vector {
x: 3.141592653589793
y: 0.0
z: 0.0
}
position_offset_G {
x: 0.0
y: 0.0
z: 0.05
}
rotation_offset_G {
x: 0.0
y: 0.0
z: 0.0
}
mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY mode: TOUCH_SCREEN_ALIGN_MODE_POSITION_ONLY
} }
error_threshold { error_threshold {
x: 0.005 x: 0.005

30
cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp
View File

@ -757,6 +757,14 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
const Eigen::Vector3d target_rotation = const Eigen::Vector3d target_rotation =
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
const Eigen::Vector3d position_offset_G =
target.has_position_offset_g()
? cmvr::common::math::toEigenVec3(target.position_offset_g())
: Eigen::Vector3d::Zero();
const Eigen::Vector3d rotation_offset_G =
target.has_rotation_offset_g()
? cmvr::common::math::toEigenVec3(target.rotation_offset_g())
: Eigen::Vector3d::Zero();
const Eigen::Matrix<double, 6, 1> error_threshold = const Eigen::Matrix<double, 6, 1> error_threshold =
cmvr::common::math::toEigenVec6(alignment.error_threshold()); cmvr::common::math::toEigenVec6(alignment.error_threshold());
const Eigen::Vector3d position_gain = const Eigen::Vector3d position_gain =
@ -775,6 +783,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig&
external_apriltag.hand_tag_id() < 0 || external_apriltag.hand_tag_id() < 0 ||
!isHomogeneousTransform(T_H_P) || !isHomogeneousTransform(T_H_P) ||
!target_rotation.allFinite() || !target_rotation.allFinite() ||
!position_offset_G.allFinite() ||
!rotation_offset_G.allFinite() ||
!position_gain.allFinite() || !position_gain.allFinite() ||
!rotation_gain.allFinite() || !rotation_gain.allFinite() ||
!vmax6.allFinite() || !vmax6.allFinite() ||
@ -950,6 +960,15 @@ bool TouchScreenTask::stepAligning(const double dt) {
const auto& alignment = config_.alignment(); const auto& alignment = config_.alignment();
const auto target_rotation_vector = const auto target_rotation_vector =
cmvr::common::math::toEigenVec3(alignment.target().rotation_vector()); cmvr::common::math::toEigenVec3(alignment.target().rotation_vector());
const auto& target = alignment.target();
const Eigen::Vector3d position_offset_G =
target.has_position_offset_g()
? cmvr::common::math::toEigenVec3(target.position_offset_g())
: Eigen::Vector3d::Zero();
const Eigen::Vector3d rotation_offset_G =
target.has_rotation_offset_g()
? cmvr::common::math::toEigenVec3(target.rotation_offset_g())
: Eigen::Vector3d::Zero();
const auto now = Clock::now(); const auto now = Clock::now();
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count(); const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
@ -1088,8 +1107,15 @@ bool TouchScreenTask::stepAligning(const double dt) {
Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(), Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(),
target_rotation_vector.y(), target_rotation_vector.y(),
target_rotation_vector.z()); target_rotation_vector.z());
if (rotation_offset_G.norm() > 1e-12) {
const double offset_angle = rotation_offset_G.norm();
R_target = Eigen::AngleAxisd(
offset_angle, rotation_offset_G / offset_angle)
.toRotationMatrix() *
R_target;
}
const Eigen::Matrix3d R_current = T_G_P.block<3, 3>(0, 0); const Eigen::Matrix3d R_current = T_G_P.block<3, 3>(0, 0);
switch (alignment.target().mode()) { switch (target.mode()) {
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION:
break; break;
case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION:
@ -1116,7 +1142,7 @@ bool TouchScreenTask::stepAligning(const double dt) {
if (tracker_.lastSwitched()) { if (tracker_.lastSwitched()) {
pbvs_.resetTwistCommandState(); pbvs_.resetTwistCommandState();
} }
if (!pbvs_.setTargetPose(p_target_G, R_target)) { if (!pbvs_.setTargetPose(p_target_G + position_offset_G, R_target)) {
enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); enterFailed(Status::ALIGN_TARGET_SETUP_FAILED);
return false; return false;
} }

2
protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto
View File

@ -43,6 +43,8 @@ message TouchScreenTaskPerceptionConfig {
message TouchScreenAlignmentTargetConfig { message TouchScreenAlignmentTargetConfig {
.cmvr.common.Vec3 rotation_vector = 2; .cmvr.common.Vec3 rotation_vector = 2;
optional TouchScreenAlignMode mode = 3; optional TouchScreenAlignMode mode = 3;
.cmvr.common.Vec3 position_offset_G = 4;
.cmvr.common.Vec3 rotation_offset_G = 5;
} }
message TouchScreenTaskAlignmentConfig { message TouchScreenTaskAlignmentConfig {