fix(touch): support repeated MuJoCo touch runs

cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h

@@ -53,6 +53,8 @@ public:
 private:
     void ensureWorkerStarted_();
     void workerLoop_();
+    void requestStop_(std::optional<double> acceleration = std::nullopt);
+    void abortCommand_();
     void sendZero_();
 
     static double velocityNorm_(const std::vector<double>& velocity);

cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp

@@ -61,8 +61,13 @@ Result CartesianVelocityController::speedL(const CartesianVelocity& velocity,
     if (!planner_ || !read_state_ || !send_velocity_ || dof_ == 0 || acceleration <= 0.0) {
         return Result::failure(ArmErrorCode::InvalidArgument, "speedL invalid input");
     }
-    if ((!worker_ || !worker_->joinable()) && busy_.exchange(true)) {
+    if (!worker_ || !worker_->joinable()) {
-        return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+        if (busy_.exchange(true)) {
+            return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy");
+        }
+    } else {
+        // speedL is a streaming command: an existing worker may receive a new target.
+        busy_.store(true);
     }
 
     ensureWorkerStarted_();
@@ -103,15 +108,7 @@ Result CartesianVelocityController::stop(const std::optional<double> acceleratio
     if (!worker_ || !worker_->joinable()) {
         return Result::success();
     }
-    {
+    requestStop_(acceleration);
-        std::lock_guard<std::mutex> lock(mutex_);
-        target_twist_ = {};
-        target_frame_ = FrameType::Base;
-        target_acceleration_ = acceleration.has_value() ? *acceleration : config_.stop_acceleration;
-        command_active_ = true;
-        ++command_version_;
-    }
-    cv_.notify_all();
     return Result::success();
 }
 
@@ -192,27 +189,26 @@ void CartesianVelocityController::workerLoop_()
             }
 
             if (!planner_->updateSpeedLAcceleration(acceleration)) {
-                if (twistNorm_(target_twist) < config_.stop_twist_norm && acceleration <= 0.0) {
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                    std::lock_guard<std::mutex> lock(mutex_);
+                    abortCommand_();
-                    command_active_ = false;
-                    sendZero_();
-                    busy_.store(false);
                     break;
                 }
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration="
                                 << acceleration;
-                sendZero_();
+                requestStop_();
-                busy_.store(false);
+                continue;
-                return;
             }
 
             std::vector<double> q_now;
             std::vector<double> qd_now;
             if (!read_state_(q_now, qd_now)) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed";
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             std::vector<double> qd_cmd;
@@ -222,9 +218,12 @@ void CartesianVelocityController::workerLoop_()
                                 << target_twist.vz << ", " << target_twist.wx << ", "
                                 << target_twist.wy << ", " << target_twist.wz
                                 << "], frame=" << (target_frame == FrameType::Tool ? "Tool" : "Base");
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             JointVelocityCommand velocity_command;
@@ -233,9 +232,12 @@ void CartesianVelocityController::workerLoop_()
             if (!send_result.ok()) {
                 CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] send_velocity failed: "
                                 << send_result.message;
-                sendZero_();
+                if (twistNorm_(target_twist) < config_.stop_twist_norm) {
-                busy_.store(false);
+                    abortCommand_();
-                return;
+                    break;
+                }
+                requestStop_();
+                continue;
             }
 
             if (twistNorm_(target_twist) < config_.stop_twist_norm &&
@@ -260,6 +262,32 @@ void CartesianVelocityController::workerLoop_()
     busy_.store(false);
 }
 
+void CartesianVelocityController::requestStop_(const std::optional<double> acceleration)
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+        target_acceleration_ = acceleration.has_value() ? *acceleration
+                                                         : config_.stop_acceleration;
+        command_active_ = true;
+        ++command_version_;
+    }
+    cv_.notify_all();
+}
+
+void CartesianVelocityController::abortCommand_()
+{
+    {
+        std::lock_guard<std::mutex> lock(mutex_);
+        command_active_ = false;
+        target_twist_ = {};
+        target_frame_ = FrameType::Base;
+    }
+    sendZero_();
+    busy_.store(false);
+}
+
 void CartesianVelocityController::sendZero_()
 {
     if (!send_velocity_) {

cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp

@@ -932,7 +932,11 @@ bool PinocchioCartesianMotionPlanner::speedLStep(const CartesianVelocity& target
         qdot = applyJointAccelerationLimits_(qdot, reference, dt);
     }
     const Eigen::Matrix<double, 6, 1> achieved_twist_base = jacobian_base * qdot;
-    if (!validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
+    // During a stop, the limiter intentionally commands a near-zero residual
+    // twist while the measured arm can still be moving in a different direction.
+    // Direction and speed-ratio checks are not meaningful for that transient.
+    if (!is_stop_command &&
+        !validateSpeedLCartesianVelocityFeasibility_(speedl_command_twist_base_,
                                                      achieved_twist_base,
                                                      toEigenVector(q_measured),
                                                      qdot)) {

cmvr-es/config/devices/dexhand/dexhand.pb.txt

@@ -38,4 +38,9 @@ dexhand {
       auto_calibrate: false
     }
   }
+
+  dexhands {
+    id: "mujoco_zero_touch_dexhand"
+    zero_sim_touch {}
+  }
 }

cmvr-es/config/manager/device_manager.pb.txt

@@ -8,35 +8,35 @@ device_manager {
     id: "mujoco_world"
     type: DEVICE_TYPE_MUJOCO_WORLD
     config_file: "devices/mujoco/mujoco_world.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
     config_file: "devices/motor/mujoco_motors.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_right_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/arm_mujoco_qp.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_viewer"
     type: DEVICE_TYPE_MUJOCO_VIEWER
     config_file: "devices/mujoco/mujoco_viewer.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
     id: "mujoco_hand_cam"
     type: DEVICE_TYPE_CAMERA
     config_file: "devices/camera/camera.pb.txt"
-    enable: false
+    enable: true
   }
 
   devices {
@@ -68,6 +68,13 @@ device_manager {
     enable: false
   }
 
+  devices {
+    id: "mujoco_zero_touch_dexhand"
+    type: DEVICE_TYPE_DEXHAND
+    config_file: "devices/dexhand/dexhand.pb.txt"
+    enable: true
+  }
+
   devices {
     id: "ti5_motors"
     type: DEVICE_TYPE_MOTOR_SYSTEM
@@ -100,7 +107,7 @@ device_manager {
     id: "huayan_arm"
     type: DEVICE_TYPE_ROBOT_ARM
     config_file: "devices/arm/huayan_arm.pb.txt"
-    enable: true
+    enable: false
   }
 
   devices {

cmvr-es/config/manager/task_manager.pb.txt

@@ -4,8 +4,8 @@ task_manager {
     type: TASK_TYPE_TOUCH_SCREEN
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.001
-    config_file: "tasks/touch_screen_task/touch_screen_task.pb.txt"
+    config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt"
-    enable: false
+    enable: true
   }
   tasks {
     id: "grpc_server"
@@ -20,6 +20,6 @@ task_manager {
     run_mode: TASK_RUN_MODE_PERIODIC_STEP
     control_period_s: 0.002
     config_file: "tasks/self_collision_task/self_collision_task.pb.txt"
-    enable: true
+    enable: false
   }
 }

cmvr-es/config/tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt

@@ -2,9 +2,9 @@ touch_screen_task {
   id: "touch_screen"
 
   devices {
-    arm_id: "right_arm_mujoco"
+    arm_id: "mujoco_right_arm"
     dexhand_id: "mujoco_zero_touch_dexhand"
-    camera_id: "hand_cam"
+    camera_id: "mujoco_hand_cam"
   }
 
   initialization {
@@ -90,8 +90,8 @@ touch_screen_task {
   }
 
   retract {
-    twist_tool { x: 0.0 y: 0.08 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
+    twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 }
-    acceleration: 8.0
+    acceleration: 5.0
-    duration_s: 5.0
+    duration_s: 2.0
   }
 }

cmvr-es/devices/dexhand/CMakeLists.txt

@@ -1,5 +1,6 @@
 add_subdirectory(rh56dftp_dexhand)
 add_subdirectory(px_6ax_gen3)
+add_subdirectory(zero_sim_touch_dexhand)
 
 add_library(dexhand INTERFACE)
 
@@ -9,6 +10,7 @@ target_link_libraries(dexhand
         INTERFACE
         cmvr_es::device::rh56dftp_dexhand
         cmvr_es::device::px_6ax_gen3
+        cmvr_es::device::zero_sim_touch_dexhand
         cmvr_es::proto
 )
 

cmvr-es/devices/dexhand/dexhand_factory.h

@@ -10,6 +10,7 @@
 #include "devices/dexhand/abstract_dexhand.h"
 #include "devices/dexhand/px_6ax_gen3/include/px_6ax_gen3.h"
 #include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
+#include "devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h"
 
 namespace cmvr::device {
 
@@ -32,6 +33,10 @@ public:
             return std::make_shared<PX6AXGen3>(
                 backendWithId_(cfg.id(), cfg.px_6ax_gen3()));
 
+        case config::DexHandDeviceConfig::kZeroSimTouch:
+            return std::make_shared<ZeroSimTouchDexHand>(
+                backendWithId_(cfg.id(), cfg.zero_sim_touch()));
+
         case config::DexHandDeviceConfig::BACKEND_NOT_SET:
         default:
         {

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt

@@ -0,0 +1,13 @@
+add_library(zero_sim_touch_dexhand SHARED src/zero_sim_touch_dexhand.cpp)
+
+target_include_directories(zero_sim_touch_dexhand PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
+
+add_library(cmvr_es::device::zero_sim_touch_dexhand ALIAS zero_sim_touch_dexhand)
+
+target_link_libraries(zero_sim_touch_dexhand
+        PRIVATE
+        cmvr_es::proto
+        glog
+)
+
+install(TARGETS zero_sim_touch_dexhand LIBRARY DESTINATION lib)

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h

@@ -0,0 +1,57 @@
+#ifndef CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
+#define CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H
+
+#include <array>
+#include <mutex>
+#include <string>
+#include <vector>
+
+#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
+#include "../../abstract_dexhand.h"
+
+namespace cmvr::device {
+
+class ZeroSimTouchDexHand final : public AbstractDexHand {
+public:
+    using FingerType = AbstractDexHand::FingerType;
+    using ResultantForce = AbstractDexHand::ResultantForce;
+    using TactilePoint = AbstractDexHand::TactilePoint;
+    using TactileRegion = AbstractDexHand::TactileRegion;
+    using TactileRegionKey = AbstractDexHand::TactileRegionKey;
+    using TactileRegionData = AbstractDexHand::TactileRegionData;
+    using Status = AbstractDexHand::Status;
+
+    explicit ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg);
+    ~ZeroSimTouchDexHand() override = default;
+
+    std::string typeName() const override { return "ZeroSimTouchDexHand"; }
+    bool init() override;
+    bool start() override;
+    bool stop() override;
+
+    Status state() const override;
+    std::string lastError() const override;
+    void getState(DexHandState& state) override;
+
+    void setAngles(const std::vector<int>& finger_joint_angles) override;
+    void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
+    std::vector<TactileRegionData> getSensorData() override;
+    TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
+    ResultantForce getResultantForce(FingerType finger, TactileRegion region) override;
+
+private:
+    TactileRegionData makeRegionData(FingerType finger, TactileRegion region);
+
+    mutable std::mutex mutex_;
+    std::vector<TactileRegionKey> polling_regions_;
+    std::array<TactilePoint, 1> tactile_points_{};
+    Status lifecycle_state_{Status::CREATED};
+    std::string last_error_;
+};
+
+// Keep the historical spelling available to callers that used the test double.
+using ZeroSImTouchDexHand = ZeroSimTouchDexHand;
+
+} // namespace cmvr::device
+
+#endif // CMVR_ES_ZERO_SIM_TOUCH_DEXHAND_H

cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp

@@ -0,0 +1,96 @@
+#include "../include/zero_sim_touch_dexhand.h"
+
+#include <utility>
+
+namespace cmvr::device {
+
+ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) {
+    id_ = cfg.id();
+}
+
+bool ZeroSimTouchDexHand::init() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STREAMING;
+    last_error_.clear();
+    return true;
+}
+
+bool ZeroSimTouchDexHand::start() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STREAMING;
+    last_error_.clear();
+    return true;
+}
+
+bool ZeroSimTouchDexHand::stop() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    lifecycle_state_ = Status::STOPPED;
+    return true;
+}
+
+ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return lifecycle_state_;
+}
+
+std::string ZeroSimTouchDexHand::lastError() const {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return last_error_;
+}
+
+void ZeroSimTouchDexHand::getState(DexHandState& state) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    state = DexHandState{};
+    state.is_initialized = lifecycle_state_ == Status::INITIALIZED ||
+                           lifecycle_state_ == Status::STREAMING;
+    if (!last_error_.empty()) {
+        state.hands[0].error_message.push_back(last_error_);
+    }
+}
+
+void ZeroSimTouchDexHand::setAngles(const std::vector<int>&) {
+    // The simulation has no finger actuators; commands are intentionally ignored.
+}
+
+void ZeroSimTouchDexHand::setTactilePollingRegions(
+    const std::vector<TactileRegionKey>& regions) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    polling_regions_ = regions;
+}
+
+std::vector<ZeroSimTouchDexHand::TactileRegionData> ZeroSimTouchDexHand::getSensorData() {
+    std::lock_guard<std::mutex> lock(mutex_);
+    if (polling_regions_.empty()) {
+        polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
+    }
+
+    std::vector<TactileRegionData> data;
+    data.reserve(polling_regions_.size());
+    for (const auto& region : polling_regions_) {
+        data.push_back(makeRegionData(region.first, region.second));
+    }
+    return data;
+}
+
+ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::getSensorData(
+    const FingerType finger, const TactileRegion region) {
+    std::lock_guard<std::mutex> lock(mutex_);
+    return makeRegionData(finger, region);
+}
+
+ZeroSimTouchDexHand::ResultantForce ZeroSimTouchDexHand::getResultantForce(
+    const FingerType, const TactileRegion) {
+    return TactilePoint::fromFz(0);
+}
+
+ZeroSimTouchDexHand::TactileRegionData ZeroSimTouchDexHand::makeRegionData(
+    const FingerType finger, const TactileRegion region) {
+    tactile_points_[0] = TactilePoint::fromFz(0);
+    TactileMatrixView view;
+    view.data = tactile_points_.data();
+    view.rows = 1;
+    view.cols = 1;
+    return {finger, region, view, "mujoco_touch_tip"};
+}
+
+} // namespace cmvr::device

cmvr-es/task/CMakeLists.txt

@@ -19,22 +19,22 @@ target_link_libraries(task
 add_library(cmvr_es::task ALIAS task)
 install(TARGETS task LIBRARY DESTINATION lib)
 
-#add_executable(touch_screen_task_test
+add_executable(touch_screen_task_test
-#        touch_screen_task/src/touch_screen_task_test.cpp
+        touch_screen_task/src/touch_screen_task_test.cpp
-#)
+)
-#
+
-#target_link_libraries(touch_screen_task_test PRIVATE
+target_link_libraries(touch_screen_task_test PRIVATE
-#        cmvr_es::task
+        cmvr_es::task
-#        cmvr_es::device::arm
+        cmvr_es::device::arm
-#        cmvr_es::device::motor_manager
+        cmvr_es::device::motor_manager
-#        cmvr_es::device::mujoco_motor_driver
+        cmvr_es::device::mujoco_motor_driver
-#        cmvr_es::device::mujoco_camera
+        cmvr_es::device::mujoco_camera
-#        cmvr_es::mujoco_viewer
+        cmvr_es::mujoco_viewer
-#        cmvr_es::proto
+        cmvr_es::proto
-#        cmvr_es::device_manager
+        cmvr_es::device_manager
-#        cmvr_es::service
+        cmvr_es::service
-#        gtest
+        gtest
-#        gtest_main
+        gtest_main
-#        pthread
+        pthread
-#        glog
+        glog
-#)
+)

cmvr-es/task/touch_screen_task/src/touch_screen_task.cpp

@@ -385,6 +385,7 @@ bool TouchScreenTask::init(const std::shared_ptr<device::RobotArm>& arm,
 bool TouchScreenTask::touch(const int u, const int v) {
     std::lock_guard<std::mutex> lock(mutex_);
     if (isBusyUnlocked()) {
+        last_status_ = Status::TASK_BUSY;
         return false;
     }
     return startFromPixelUnlocked(u, v);

cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp

@@ -4,7 +4,6 @@
 #include <array>
 #include <chrono>
 #include <cmath>
-#include <condition_variable>
 #include <cstdint>
 #include <filesystem>
 #include <iostream>
@@ -28,10 +27,10 @@
 #include "devices/arm/robot_arm_factory.h"
 #include "devices/camera/mujoco_camera/include/mujoco_camera.h"
 #include "devices/motor/manager/include/motor_manager.h"
-#include "devices/motor/drivers/mujoco/include/mujoco_motor.h"
 #include "manager/device_manager/include/device_manager.h"
 #include "manager/task_manager/include/task_manager.h"
 #include "simulate/mujoco/mujoco_viewer/include/mujoco_viewer.h"
+#include "simulate/mujoco/mujoco_world/include/mujoco_world.h"
 
 namespace {
 
@@ -68,178 +67,6 @@ std::filesystem::path findProjectRoot()
     return search(std::filesystem::path(__FILE__).parent_path());
 }
 
-class TouchMujocoViewer final : public cmvr::MuJocoViewer {
-public:
-    TouchMujocoViewer(const std::string& model_path,
-                      std::shared_ptr<cmvr::device::MujocoJointBridge> bridge)
-        : MuJocoViewer(model_path.c_str()), bridge_(std::move(bridge))
-    {
-        position_actuator_ids_.fill(-1);
-        qpos_ids_.fill(-1);
-        qvel_ids_.fill(-1);
-    }
-
-    std::shared_ptr<cmvr::device::MujocoCamera> waitCameraReady(
-        const std::chrono::milliseconds timeout)
-    {
-        std::unique_lock<std::mutex> lock(camera_mutex_);
-        if (!camera_cv_.wait_for(lock, timeout, [this] { return camera_ != nullptr; })) {
-            return nullptr;
-        }
-        return camera_;
-    }
-
-protected:
-    void initOnce(mjModel* model, mjData* data) override
-    {
-        setupCamera(2.5, -160.0, -25.0);
-        enablePiPCamera("hand_cam");
-
-        bool valid = true;
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const std::string actuator_name = std::string(kJointNames[i]) + "_pos";
-            position_actuator_ids_[i] = mj_name2id(model, mjOBJ_ACTUATOR, actuator_name.c_str());
-            const int joint_id = mj_name2id(model, mjOBJ_JOINT, kJointNames[i]);
-            if (position_actuator_ids_[i] < 0 || joint_id < 0) {
-                valid = false;
-                continue;
-            }
-            qpos_ids_[i] = model->jnt_qposadr[joint_id];
-            qvel_ids_[i] = model->jnt_dofadr[joint_id];
-            position_reference_[i] = data->qpos[qpos_ids_[i]];
-        }
-
-        const int hand_cam_id = mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
-        if (hand_cam_id < 0) {
-            valid = false;
-        }
-
-        auto camera = std::make_shared<cmvr::device::MujocoCamera>(
-            [this](std::vector<unsigned char>& rgb,
-                   std::vector<float>& depth,
-                   int& width,
-                   int& height,
-                   std::uint64_t& frame_id) {
-                return getPiPCameraRGBD(rgb, depth, width, height, frame_id);
-            });
-        if (hand_cam_id >= 0) {
-            camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
-        }
-        camera->setConsumeNewFrameOnly(false);
-        {
-            std::lock_guard<std::mutex> lock(camera_mutex_);
-            camera_ = std::move(camera);
-        }
-        camera_cv_.notify_all();
-        bridge_->markReady(valid);
-    }
-
-    void controlCallback(mjModel* model, mjData* data) override
-    {
-        std::vector<double> measured_position(kDof, 0.0);
-        std::vector<double> measured_velocity(kDof, 0.0);
-        for (std::size_t i = 0; i < kDof; ++i) {
-            if (qpos_ids_[i] >= 0) {
-                measured_position[i] = data->qpos[qpos_ids_[i]];
-            }
-            if (qvel_ids_[i] >= 0) {
-                measured_velocity[i] = data->qvel[qvel_ids_[i]];
-            }
-        }
-        bridge_->publishMeasured(measured_position, measured_velocity);
-
-        const auto commands = bridge_->commands();
-        for (std::size_t i = 0; i < kDof; ++i) {
-            const int actuator_id = position_actuator_ids_[i];
-            if (actuator_id < 0) {
-                continue;
-            }
-
-            if (commands.mode[i] == cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY) {
-                if (last_mode_[i] != cmvr::msgs::RUN_MODE_CYCLIC_SYNC_VELOCITY && qpos_ids_[i] >= 0) {
-                    position_reference_[i] = data->qpos[qpos_ids_[i]];
-                }
-                position_reference_[i] += commands.velocity[i] * model->opt.timestep;
-            } else {
-                position_reference_[i] = commands.position[i];
-            }
-
-            const double lower = model->actuator_ctrlrange[2 * actuator_id];
-            const double upper = model->actuator_ctrlrange[2 * actuator_id + 1];
-            position_reference_[i] = std::clamp(position_reference_[i], lower, upper);
-            data->ctrl[actuator_id] = position_reference_[i];
-            last_mode_[i] = commands.mode[i];
-        }
-    }
-
-private:
-    std::shared_ptr<cmvr::device::MujocoJointBridge> bridge_;
-    std::array<int, kDof> position_actuator_ids_{};
-    std::array<int, kDof> qpos_ids_{};
-    std::array<int, kDof> qvel_ids_{};
-    std::array<double, kDof> position_reference_{};
-    std::array<cmvr::msgs::RunMode, kDof> last_mode_{};
-    std::mutex camera_mutex_;
-    std::condition_variable camera_cv_;
-    std::shared_ptr<cmvr::device::MujocoCamera> camera_{nullptr};
-};
-
-class ZeroTouchDexHand final : public cmvr::device::AbstractDexHand {
-public:
-    ZeroTouchDexHand()
-    {
-        id_ = "mujoco_zero_touch_dexhand";
-    }
-
-    std::string typeName() const override { return "ZeroTouchDexHand"; }
-    Status state() const override { return Status::STREAMING; }
-    std::string lastError() const override { return {}; }
-
-    void setAngles(const std::vector<int>& finger_joint_angles) override
-    {
-        (void)finger_joint_angles;
-    }
-
-    void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override
-    {
-        polling_regions_ = regions;
-    }
-
-    std::vector<TactileRegionData> getSensorData() override
-    {
-        std::vector<TactileRegionData> data;
-        if (polling_regions_.empty()) {
-            polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP});
-        }
-        data.reserve(polling_regions_.size());
-        for (const auto& region : polling_regions_) {
-            data.push_back(getSensorData(region.first, region.second));
-        }
-        return data;
-    }
-
-    TactileRegionData getSensorData(const FingerType finger, const TactileRegion region) override
-    {
-        tactile_points_[0] = getResultantForce(finger, region);
-        TactileMatrixView view;
-        view.data = tactile_points_.data();
-        view.rows = 1;
-        view.cols = 1;
-        return {finger, region, view, "mujoco_touch_tip"};
-    }
-
-    ResultantForce getResultantForce(const FingerType finger, const TactileRegion region) override
-    {
-        (void)finger;
-        (void)region;
-        return TactilePoint::fromFz(0);
-    }
-
-private:
-    std::vector<TactileRegionKey> polling_regions_;
-    std::array<TactilePoint, 1> tactile_points_{};
-};
-
 struct TagCenterPixel {
     int tag_id{-1};
     int u{0};
@@ -459,16 +286,26 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
     cmvr::config::DeviceManagerConfig device_manager_config;
     device_manager_config.set_name("touch_screen_mujoco_test");
     device_manager_config.set_version("test");
+    auto* world_entry = device_manager_config.add_devices();
+    world_entry->set_id("mujoco_world");
+    world_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MUJOCO_WORLD);
+    world_entry->set_config_file("devices/mujoco/mujoco_world.pb.txt");
+    world_entry->set_enable(true);
     auto* motor_entry = device_manager_config.add_devices();
     motor_entry->set_id("mujoco_motors");
     motor_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_MOTOR_SYSTEM);
     motor_entry->set_config_file("devices/motor/mujoco_motors.pb.txt");
     motor_entry->set_enable(true);
     auto* arm_entry = device_manager_config.add_devices();
-    arm_entry->set_id("right_arm_mujoco");
+    arm_entry->set_id("mujoco_right_arm");
     arm_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_ROBOT_ARM);
     arm_entry->set_config_file("devices/arm/arm_mujoco.pb.txt");
     arm_entry->set_enable(true);
+    auto* dexhand_entry = device_manager_config.add_devices();
+    dexhand_entry->set_id("mujoco_zero_touch_dexhand");
+    dexhand_entry->set_type(cmvr::config::DeviceConfigEntry::DEVICE_TYPE_DEXHAND);
+    dexhand_entry->set_config_file("devices/dexhand/dexhand.pb.txt");
+    dexhand_entry->set_enable(true);
 
     auto& device_manager = cmvr::device::DeviceManager::getInstance(device_manager_config);
     auto motor_system = device_manager.getDevice<cmvr::device::MotorManager>("mujoco_motors");
@@ -476,19 +313,48 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
         CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MotorManager not found: mujoco_motors";
         return;
     }
-    auto bridge = cmvr::device::MotorManager::mujocoBridgeFor("mujoco_motors");
+    auto world = cmvr::device::MotorManager::mujocoWorldFor("mujoco_motors");
-    if (!bridge) {
+    if (!world || !world->isLoaded() || !world->isRunning()) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world";
         return;
     }
-    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("right_arm_mujoco");
+    auto arm = device_manager.getDevice<cmvr::device::RobotArm>("mujoco_right_arm");
     if (!arm) {
-        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: right_arm_mujoco";
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] RobotArm not found: mujoco_right_arm";
         return;
     }
+    // The task config still uses the historical ID; keep it as a test-only alias.
+    device_manager.registerDevice("right_arm_mujoco", arm);
 
-    TouchMujocoViewer viewer(
+    cmvr::MuJocoViewer viewer(world);
-        (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge);
+    viewer.setupCamera(2.5, -160.0, -25.0);
+    viewer.enablePiPCamera("hand_cam");
+
+    auto camera = std::make_shared<cmvr::device::MujocoCamera>(
+        [&viewer](std::vector<unsigned char>& rgb,
+                  std::vector<float>& depth,
+                  int& width,
+                  int& height,
+                  std::uint64_t& frame_id) {
+            return viewer.getPiPCameraRGBD(rgb, depth, width, height, frame_id);
+        });
+    camera->setConsumeNewFrameOnly(true);
+    {
+        std::lock_guard<std::mutex> lock(world->mutex());
+        const auto* model = world->model();
+        const int hand_cam_id = model == nullptr
+            ? -1
+            : mj_name2id(model, mjOBJ_CAMERA, "hand_cam");
+        if (hand_cam_id < 0) {
+            CMVR_LOG(ERROR) << "[TouchScreenTaskTest] MuJoCo camera not found: hand_cam";
+            return;
+        }
+        camera->setFovyDeg(model->cam_fovy[hand_cam_id]);
+    }
+    if (!camera->init()) {
+        CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Failed to initialize MuJoCo camera";
+        return;
+    }
 
     struct Outcome {
         bool init_ok{false};
@@ -506,17 +372,8 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
 
     std::thread scenario([&] {
         try {
-            if (!bridge->waitUntilReady(std::chrono::seconds(10))) {
-                throw std::runtime_error("MuJoCo right-arm joints, actuators, or hand_cam are not ready");
-            }
-            auto camera = viewer.waitCameraReady(std::chrono::seconds(10));
-            if (!camera) {
-                throw std::runtime_error("MuJoCo camera is not ready");
-            }
-
             const auto touch_config = loadMujocoTouchConfig(project_root);
             device_manager.registerDevice(touch_config.devices().camera_id(), camera);
-            device_manager.registerDevice(std::make_shared<ZeroTouchDexHand>());
 
             cmvr::config::TaskManagerConfig task_manager_config;
             auto* task_entry = task_manager_config.add_tasks();
@@ -533,6 +390,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                 throw std::runtime_error("TouchScreenTask not found: " + touch_config.id());
             }
             outcome.init_ok = task->lastStatus() != cmvr::task::TouchScreenTask::Status::NOT_INITIALIZED;
+
             task_manager.startRunTask();
 
             TagCenterPixel target_pixel;
@@ -548,10 +406,31 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                       << ", v=" << target_pixel.v
                       << std::endl;
 
+            std::cout << "[TouchScreenTaskMujocoTest] before task->touch, phase="
+                      << cmvr::task::TouchScreenTask::phaseToString(task->phase())
+                      << ", status="
+                      << cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
+                      << std::endl;
+            const auto touch_start = std::chrono::steady_clock::now();
             outcome.touch_ok = task->touch(target_pixel.u, target_pixel.v);
+            std::cout << "[TouchScreenTaskMujocoTest] after task->touch, ok="
+                      << (outcome.touch_ok ? 1 : 0)
+                      << ", elapsed_ms="
+                      << std::chrono::duration<double, std::milli>(
+                             std::chrono::steady_clock::now() - touch_start)
+                             .count()
+                      << ", phase="
+                      << cmvr::task::TouchScreenTask::phaseToString(task->phase())
+                      << ", status="
+                      << cmvr::task::TouchScreenTask::statusToString(task->lastStatus())
+                      << std::endl;
             if (!outcome.touch_ok) {
                 outcome.final_status = task->lastStatus();
-                return;
+                task_manager.stopRunTask();
+                viewer.requestStop();
+                throw std::runtime_error(
+                    "task->touch failed, status=" +
+                    std::string(cmvr::task::TouchScreenTask::statusToString(outcome.final_status)));
             }
 
             const auto deadline = std::chrono::steady_clock::now() + std::chrono::seconds(20);
@@ -604,6 +483,7 @@ TEST(TouchScreenTaskTest, RunTouchOnceInMujoco) {
                 cmvr::task::TaskManager::getInstance().stopRunTask();
             } catch (...) {
             }
+            viewer.requestStop();
             outcome.worker_error = error.what();
             std::cerr << "[TouchScreenTaskMujocoTest] scenario error: " << outcome.worker_error
                       << "\nClose the MuJoCo viewer window to finish gtest." << std::endl;

protos/cmvr/config/dexhand_config/dexhand_config.proto

@@ -38,6 +38,10 @@ message PX6AXGen3{
     PX6AXGen3PollingReadMode polling_read_mode = 18;
 }
 
+message ZeroSimTouchDexHand {
+    string id = 1;
+}
+
 
 
 message DexHandDeviceConfig {
@@ -47,6 +51,7 @@ message DexHandDeviceConfig {
     oneof backend {
         RH56DFTPDexHandConfig rh56dftp = 10;
         PX6AXGen3 px_6ax_gen3 = 11;
+        ZeroSimTouchDexHand zero_sim_touch = 12;
     }
 }