feat(touch): support target pose offsets

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

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<double, 6, 1> 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<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(),
                                                         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;
         }

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 {