From 724da3d0001a6093dab350c64f7cc0c3fd276742 Mon Sep 17 00:00:00 2001 From: lgv Date: Fri, 4 Sep 2026 11:03:14 +0800 Subject: [PATCH] fix(touch): support repeated MuJoCo touch runs --- .../include/cartesian_velocity_controller.h | 2 + .../src/cartesian_velocity_controller.cpp | 84 ++++-- .../pinocchio_cartesian_motion_planner.cpp | 6 +- cmvr-es/config/devices/dexhand/dexhand.pb.txt | 5 + cmvr-es/config/manager/device_manager.pb.txt | 19 +- cmvr-es/config/manager/task_manager.pb.txt | 6 +- .../touch_screen_task_mujoco.pb.txt | 10 +- cmvr-es/devices/dexhand/CMakeLists.txt | 2 + cmvr-es/devices/dexhand/dexhand_factory.h | 5 + .../zero_sim_touch_dexhand/CMakeLists.txt | 13 + .../include/zero_sim_touch_dexhand.h | 57 ++++ .../src/zero_sim_touch_dexhand.cpp | 96 +++++++ cmvr-es/task/CMakeLists.txt | 38 +-- .../src/touch_screen_task.cpp | 1 + .../src/touch_screen_task_test.cpp | 264 +++++------------- .../dexhand_config/dexhand_config.proto | 5 + 16 files changed, 359 insertions(+), 254 deletions(-) create mode 100644 cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt create mode 100644 cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h create mode 100644 cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp diff --git a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h b/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h index 9dd0fc29..9d138abc 100644 --- a/cmvr-es/algorithms/controllers/arm_control/include/cartesian_velocity_controller.h +++ b/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 acceleration = std::nullopt); + void abortCommand_(); void sendZero_(); static double velocityNorm_(const std::vector& velocity); diff --git a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp b/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp index efad63b6..daf69fb7 100644 --- a/cmvr-es/algorithms/controllers/arm_control/src/cartesian_velocity_controller.cpp +++ b/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)) { - return Result::failure(ArmErrorCode::RobotNotReady, "arm is busy"); + if (!worker_ || !worker_->joinable()) { + 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 acceleratio if (!worker_ || !worker_->joinable()) { return Result::success(); } - { - std::lock_guard 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(); + requestStop_(acceleration); 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) { - std::lock_guard lock(mutex_); - command_active_ = false; - sendZero_(); - busy_.store(false); + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); break; } CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] updateSpeedLAcceleration failed, acceleration=" << acceleration; - sendZero_(); - busy_.store(false); - return; + requestStop_(); + continue; } std::vector q_now; std::vector qd_now; if (!read_state_(q_now, qd_now)) { CMVR_LOG(ERROR) << "[CartesianVelocityController][speedL] read_state failed"; - sendZero_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + break; + } + requestStop_(); + continue; } std::vector 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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_(); - busy_.store(false); - return; + if (twistNorm_(target_twist) < config_.stop_twist_norm) { + abortCommand_(); + 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 acceleration) +{ + { + std::lock_guard 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 lock(mutex_); + command_active_ = false; + target_twist_ = {}; + target_frame_ = FrameType::Base; + } + sendZero_(); + busy_.store(false); +} + void CartesianVelocityController::sendZero_() { if (!send_velocity_) { diff --git a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp b/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp index 329c7b96..160c2570 100644 --- a/cmvr-es/algorithms/motion_planner/arm_motion/cartesian_motion/pinocchio/src/pinocchio_cartesian_motion_planner.cpp +++ b/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 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)) { diff --git a/cmvr-es/config/devices/dexhand/dexhand.pb.txt b/cmvr-es/config/devices/dexhand/dexhand.pb.txt index 10e2f9f6..75a8c811 100644 --- a/cmvr-es/config/devices/dexhand/dexhand.pb.txt +++ b/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 {} + } } diff --git a/cmvr-es/config/manager/device_manager.pb.txt b/cmvr-es/config/manager/device_manager.pb.txt index d9ad1d68..a024bfd1 100644 --- a/cmvr-es/config/manager/device_manager.pb.txt +++ b/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 { diff --git a/cmvr-es/config/manager/task_manager.pb.txt b/cmvr-es/config/manager/task_manager.pb.txt index 64db2085..1b4c4fed 100644 --- a/cmvr-es/config/manager/task_manager.pb.txt +++ b/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" - enable: false + config_file: "tasks/touch_screen_task/touch_screen_task_mujoco.pb.txt" + 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 } } 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 d607f7e4..91714aaf 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 @@ -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 } - acceleration: 8.0 - duration_s: 5.0 + twist_tool { x: 0.0 y: 0.06 z: 0.0 rx: 0.0 ry: 0.0 rz: 0.0 } + acceleration: 5.0 + duration_s: 2.0 } } diff --git a/cmvr-es/devices/dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/CMakeLists.txt index 41130532..f162ecd8 100644 --- a/cmvr-es/devices/dexhand/CMakeLists.txt +++ b/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 ) diff --git a/cmvr-es/devices/dexhand/dexhand_factory.h b/cmvr-es/devices/dexhand/dexhand_factory.h index 0bba4dfb..de7ea7d9 100644 --- a/cmvr-es/devices/dexhand/dexhand_factory.h +++ b/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( backendWithId_(cfg.id(), cfg.px_6ax_gen3())); + case config::DexHandDeviceConfig::kZeroSimTouch: + return std::make_shared( + backendWithId_(cfg.id(), cfg.zero_sim_touch())); + case config::DexHandDeviceConfig::BACKEND_NOT_SET: default: { diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/CMakeLists.txt new file mode 100644 index 00000000..429666eb --- /dev/null +++ b/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) diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/include/zero_sim_touch_dexhand.h new file mode 100644 index 00000000..a44b5ddb --- /dev/null +++ b/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 +#include +#include +#include + +#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& finger_joint_angles) override; + void setTactilePollingRegions(const std::vector& regions) override; + std::vector 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 polling_regions_; + std::array 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 diff --git a/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp b/cmvr-es/devices/dexhand/zero_sim_touch_dexhand/src/zero_sim_touch_dexhand.cpp new file mode 100644 index 00000000..c04387e9 --- /dev/null +++ b/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 + +namespace cmvr::device { + +ZeroSimTouchDexHand::ZeroSimTouchDexHand(const config::ZeroSimTouchDexHand& cfg) { + id_ = cfg.id(); +} + +bool ZeroSimTouchDexHand::init() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STREAMING; + last_error_.clear(); + return true; +} + +bool ZeroSimTouchDexHand::start() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STREAMING; + last_error_.clear(); + return true; +} + +bool ZeroSimTouchDexHand::stop() { + std::lock_guard lock(mutex_); + lifecycle_state_ = Status::STOPPED; + return true; +} + +ZeroSimTouchDexHand::Status ZeroSimTouchDexHand::state() const { + std::lock_guard lock(mutex_); + return lifecycle_state_; +} + +std::string ZeroSimTouchDexHand::lastError() const { + std::lock_guard lock(mutex_); + return last_error_; +} + +void ZeroSimTouchDexHand::getState(DexHandState& state) { + std::lock_guard 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&) { + // The simulation has no finger actuators; commands are intentionally ignored. +} + +void ZeroSimTouchDexHand::setTactilePollingRegions( + const std::vector& regions) { + std::lock_guard lock(mutex_); + polling_regions_ = regions; +} + +std::vector ZeroSimTouchDexHand::getSensorData() { + std::lock_guard lock(mutex_); + if (polling_regions_.empty()) { + polling_regions_.push_back({FingerType::INDEX, TactileRegion::TIP}); + } + + std::vector 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 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 diff --git a/cmvr-es/task/CMakeLists.txt b/cmvr-es/task/CMakeLists.txt index e16bbb5f..b10b0978 100644 --- a/cmvr-es/task/CMakeLists.txt +++ b/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 -# touch_screen_task/src/touch_screen_task_test.cpp -#) -# -#target_link_libraries(touch_screen_task_test PRIVATE -# cmvr_es::task -# cmvr_es::device::arm -# cmvr_es::device::motor_manager -# cmvr_es::device::mujoco_motor_driver -# cmvr_es::device::mujoco_camera -# cmvr_es::mujoco_viewer -# cmvr_es::proto -# cmvr_es::device_manager -# cmvr_es::service -# gtest -# gtest_main -# pthread -# glog -#) +add_executable(touch_screen_task_test + touch_screen_task/src/touch_screen_task_test.cpp +) + +target_link_libraries(touch_screen_task_test PRIVATE + cmvr_es::task + cmvr_es::device::arm + cmvr_es::device::motor_manager + cmvr_es::device::mujoco_motor_driver + cmvr_es::device::mujoco_camera + cmvr_es::mujoco_viewer + cmvr_es::proto + cmvr_es::device_manager + cmvr_es::service + gtest + gtest_main + pthread + glog +) 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 7640cc33..fc493498 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 @@ -385,6 +385,7 @@ bool TouchScreenTask::init(const std::shared_ptr& arm, bool TouchScreenTask::touch(const int u, const int v) { std::lock_guard lock(mutex_); if (isBusyUnlocked()) { + last_status_ = Status::TASK_BUSY; return false; } return startFromPixelUnlocked(u, v); diff --git a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp index 55a45b27..7e14ed09 100644 --- a/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp +++ b/cmvr-es/task/touch_screen_task/src/touch_screen_task_test.cpp @@ -4,7 +4,6 @@ #include #include #include -#include #include #include #include @@ -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 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 waitCameraReady( - const std::chrono::milliseconds timeout) - { - std::unique_lock 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( - [this](std::vector& rgb, - std::vector& 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 lock(camera_mutex_); - camera_ = std::move(camera); - } - camera_cv_.notify_all(); - bridge_->markReady(valid); - } - - void controlCallback(mjModel* model, mjData* data) override - { - std::vector measured_position(kDof, 0.0); - std::vector 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 bridge_; - std::array position_actuator_ids_{}; - std::array qpos_ids_{}; - std::array qvel_ids_{}; - std::array position_reference_{}; - std::array last_mode_{}; - std::mutex camera_mutex_; - std::condition_variable camera_cv_; - std::shared_ptr 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& finger_joint_angles) override - { - (void)finger_joint_angles; - } - - void setTactilePollingRegions(const std::vector& regions) override - { - polling_regions_ = regions; - } - - std::vector getSensorData() override - { - std::vector 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 polling_regions_; - std::array 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("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"); - if (!bridge) { - CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco bridge not found: mujoco_motors"; + auto world = cmvr::device::MotorManager::mujocoWorldFor("mujoco_motors"); + if (!world || !world->isLoaded() || !world->isRunning()) { + CMVR_LOG(ERROR) << "[TouchScreenTaskTest] Mujoco world is not ready: mujoco_world"; return; } - auto arm = device_manager.getDevice("right_arm_mujoco"); + auto arm = device_manager.getDevice("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( - (project_root / "model/xiaoyan_description/dual_arm.xml").string(), bridge); + cmvr::MuJocoViewer viewer(world); + viewer.setupCamera(2.5, -160.0, -25.0); + viewer.enablePiPCamera("hand_cam"); + + auto camera = std::make_shared( + [&viewer](std::vector& rgb, + std::vector& 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 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()); 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( + 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; diff --git a/protos/cmvr/config/dexhand_config/dexhand_config.proto b/protos/cmvr/config/dexhand_config/dexhand_config.proto index b822f3f5..6ef3b307 100644 --- a/protos/cmvr/config/dexhand_config/dexhand_config.proto +++ b/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; } }