diff --git a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt index 951a3287..329c12a2 100644 --- a/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt +++ b/cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt @@ -50,8 +50,24 @@ touch_screen_task { twist_filter_alpha: 1.0 } 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 + } error_threshold { x: 0.005 diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp index 8351a848..7e20796a 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp @@ -757,6 +757,14 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& const Eigen::Vector3d target_rotation = 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 error_threshold = cmvr::common::math::toEigenVec6(alignment.error_threshold()); const Eigen::Vector3d position_gain = @@ -775,6 +783,8 @@ bool TouchScreenTask::validateConfig(const cmvr::config::TouchScreenTaskConfig& external_apriltag.hand_tag_id() < 0 || !isHomogeneousTransform(T_H_P) || !target_rotation.allFinite() || + !position_offset_G.allFinite() || + !rotation_offset_G.allFinite() || !position_gain.allFinite() || !rotation_gain.allFinite() || !vmax6.allFinite() || @@ -950,6 +960,15 @@ bool TouchScreenTask::stepAligning(const double dt) { const auto& alignment = config_.alignment(); const auto 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 double elapsed = std::chrono::duration(now - phase_start_time_).count(); @@ -1088,8 +1107,15 @@ bool TouchScreenTask::stepAligning(const double dt) { Eigen::Matrix3d R_target = rotationFromTargetRotvec(target_rotation_vector.x(), target_rotation_vector.y(), 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); - switch (alignment.target().mode()) { + switch (target.mode()) { case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_POSE_AND_POSITION: break; case cmvr::config::TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION: @@ -1116,7 +1142,7 @@ bool TouchScreenTask::stepAligning(const double dt) { if (tracker_.lastSwitched()) { 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); return false; } diff --git a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto index 6dd9554d..ea977381 100644 --- a/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto +++ b/protos/cmvr/config/touch_screen_task_config/touch_screen_task_config.proto @@ -43,6 +43,8 @@ message TouchScreenTaskPerceptionConfig { message TouchScreenAlignmentTargetConfig { .cmvr.common.Vec3 rotation_vector = 2; optional TouchScreenAlignMode mode = 3; + .cmvr.common.Vec3 position_offset_G = 4; + .cmvr.common.Vec3 rotation_offset_G = 5; } message TouchScreenTaskAlignmentConfig {