diff --git a/CMakeLists.txt b/CMakeLists.txt index 3d86d397..9e100d3a 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -121,6 +121,7 @@ target_link_libraries(cmvr_es PRIVATE cmvr_es::planner cmvr_es::device::humanoid_robot cmvr_es::common + cmvr_es::applications ) install(TARGETS cmvr_es RUNTIME DESTINATION bin) diff --git a/cmvr-es/CMakeLists.txt b/cmvr-es/CMakeLists.txt index 19bdfe64..7f38a336 100644 --- a/cmvr-es/CMakeLists.txt +++ b/cmvr-es/CMakeLists.txt @@ -11,5 +11,6 @@ add_subdirectory(planner) add_subdirectory(ik_solver) add_subdirectory(data_center) +add_subdirectory(applications) add_subdirectory(simulate) add_subdirectory(common) diff --git a/cmvr-es/applications/CMakeLists.txt b/cmvr-es/applications/CMakeLists.txt new file mode 100644 index 00000000..3f31216f --- /dev/null +++ b/cmvr-es/applications/CMakeLists.txt @@ -0,0 +1,25 @@ +add_library(applications + src/touch_screen_app.cpp +) + +target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) + +target_link_libraries(applications PUBLIC + cmvr_es::controller +) + +add_library(cmvr_es::applications ALIAS applications) +install(TARGETS applications LIBRARY DESTINATION lib) + +add_executable(touch_screen_app_test + src/touch_screen_app_test.cpp +) + +target_link_libraries(touch_screen_app_test PRIVATE + cmvr_es::applications + cmvr_es::device_manager + gtest + gtest_main + pthread + glog +) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h new file mode 100644 index 00000000..75769aa5 --- /dev/null +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -0,0 +1,225 @@ +#pragma once + +#ifndef CMVR_ES_TOUCH_SCREEN_APP_H +#define CMVR_ES_TOUCH_SCREEN_APP_H + +#include +#include +#include +#include +#include + +#include + +#include "controller/include/ibvs_controller.h" +#include "devices/camera/abstract_camera.h" +#include "devices/dexhand/abstract_dexhand.h" +#include "devices/robot/abstract_robot.h" +#include "perception/include/apriltag_perception.h" +#include "perception/include/tag_relative_target_3d.h" + +namespace cmvr::app { + +class TouchScreenApp { +public: + enum class Phase { + IDLE = 0, + ALIGNING, + ALIGN_REACHED, + TOUCHING, + DWELLING, + RETRACTING, + DONE, + FAILED + }; + + enum class Status { + IDLE = 0, + NOT_INITIALIZED, + INVALID_CONFIG, + CONTROL_JOINT_MISMATCH, + ALIGN_WAITING_PERCEPTION, + ALIGN_WAITING_TRACK, + ALIGN_TARGET_SETUP_FAILED, + ALIGN_COMPUTE_FAILED, + ALIGN_TIMEOUT, + ALIGNING, + ALIGN_REACHED, + TOUCHING, + TOUCH_TRIGGERED, + TOUCH_TIMEOUT, + RETRACTING, + DONE, + STOPPED, + ROBOT_STATE_FAILED, + ROBOT_COMMAND_FAILED + }; + + enum class TactileRegion { + TIP = 0, + FINGER, + PAD, + TIP_AND_FINGER, + THUMB_MIDDLE + }; + + struct Options { + // IBVS / IK 初始化参数。 + std::string urdf_path; + std::string base_link{"PELVIS_S"}; + std::string flange_link{"R_WRIST_R_S"}; + std::string camera_link; + + // 视觉感知参数。 + double tag_size_m{0.12}; + perception::AprilTagPerception::DepthPolicy depth_policy{ + perception::AprilTagPerception::DepthPolicy::NONE}; + perception::TagRelativeTarget3D::TargetPointMethod target_point_method{ + perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE}; + + // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。 + Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.12}; + double target_rx{3.14159265358979323846}; + double target_ry{0.0}; + double target_rz{0.0}; + + // IBVS 参数。 + double ibvs_lambda{0.7}; + double ibvs_mu{0.02}; + double ibvs_qdot_max{0.6}; + std::array ibvs_vmax6{{0.4, 0.4, 0.4, 0.4, 0.4, 0.4}}; + bool enable_joint_limit_avoidance{true}; + double joint_limit_avoidance_gain{0.2}; + double joint_limit_avoidance_margin_ratio{0.05}; + double joint_limit_avoidance_max_push{0.25}; + Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()}; + Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()}; + + // 关节控制链，默认右臂 7 轴。 + std::vector control_joint_names{ + "R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y", + "R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"}; + + // 视觉对准收敛判据。 + double align_xy_threshold_m{0.003}; + double align_z_threshold_m{0.010}; + int align_stable_frames{5}; + double align_timeout_s{10.0}; + bool pause_after_align_reached{false}; + + // 触控阶段：speedL 目标 twist（base_link 系）。 + Eigen::Matrix touch_twist_base{ + (Eigen::Matrix() << 0.0, 0.0, -0.02, 0.0, 0.0, 0.0).finished()}; + double touch_acceleration{0.6}; + double touch_timeout_s{2.0}; + + // 接触后停留与回退。 + double dwell_time_s{0.05}; + Eigen::Matrix retract_twist_base{ + (Eigen::Matrix() << 0.0, 0.0, 0.03, 0.0, 0.0, 0.0).finished()}; + double retract_acceleration{0.8}; + double retract_duration_s{0.20}; + + // 指尖触觉判据。 + device::FingerType tactile_finger{device::FingerType::INDEX}; + TactileRegion tactile_region{TactileRegion::FINGER}; + double tactile_pressure_sum_threshold{3000.0}; + double tactile_pressure_peak_threshold{0.0}; + }; + + TouchScreenApp(); + ~TouchScreenApp() = default; + + bool init(const std::shared_ptr& robot, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera, + const Options& options); + + void setOptions(const Options& options); + const Options& options() const { return options_; } + + bool startFromPixel(int u, int v); + bool step(); + void stop(); + + Phase phase() const { return phase_; } + Status lastStatus() const { return last_status_; } + static const char* phaseToString(Phase phase); + static const char* statusToString(Status status); + + bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED || + phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING || + phase_ == Phase::RETRACTING; } + bool isFinished() const { return phase_ == Phase::DONE; } + bool isFailed() const { return phase_ == Phase::FAILED; } + + int targetU() const { return target_u_; } + int targetV() const { return target_v_; } + double lastTouchPressureSum() const { return last_touch_pressure_sum_; } + double lastTouchPressurePeak() const { return last_touch_pressure_peak_; } + int lastActiveTagId() const { return last_active_tag_id_; } + const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; } + + const std::shared_ptr& perception() const { return perception_; } + const perception::TagRelativeTarget3D& tracker() const { return tracker_; } + const IbvsController& ibvs() const { return ibvs_; } + +private: + using Clock = std::chrono::steady_clock; + + bool applyOptions(); + bool validateControlJointNames() const; + bool stepAligning(); + bool stepTouching(); + bool stepDwelling(); + bool stepRetracting(); + + bool readControlledJointPositions(std::vector& q_out) const; + bool sendJointVelocity(const std::vector& qdot) const; + bool sendZeroJointVelocity() const; + bool holdCurrentControlledPosition() const; + + bool startTouchPhase(); + bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract); + void enterFailed(Status status); + + bool updateTouchPressure(); + +private: + std::shared_ptr robot_{nullptr}; + std::shared_ptr dexhand_{nullptr}; + std::shared_ptr camera_{nullptr}; + + std::shared_ptr perception_{nullptr}; + perception::TagRelativeTarget3D tracker_; + IbvsController ibvs_; + Options options_{}; + + Phase phase_{Phase::IDLE}; + Phase phase_after_retract_{Phase::DONE}; + Status last_status_{Status::NOT_INITIALIZED}; + + bool initialized_{false}; + bool target_locked_{false}; + bool ibvs_target_initialized_{false}; + bool touch_command_started_{false}; + bool retract_command_started_{false}; + + int target_u_{-1}; + int target_v_{-1}; + int align_stable_count_{0}; + int last_active_tag_id_{-1}; + + double last_touch_pressure_sum_{0.0}; + double last_touch_pressure_peak_{0.0}; + Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()}; + + Clock::time_point phase_start_time_{}; + Status final_status_after_retract_{Status::DONE}; +}; + +using touch_screen_app = TouchScreenApp; + +} // namespace cmvr::app + +#endif // CMVR_ES_TOUCH_SCREEN_APP_H diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp new file mode 100644 index 00000000..3cfdd781 --- /dev/null +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -0,0 +1,704 @@ +#include "applications/include/touch_screen_app.h" + +#include +#include +#include +#include + +namespace cmvr::app { + +namespace { + +std::vector toStdVector6(const Eigen::Matrix& twist) { + std::vector out(6, 0.0); + for (int i = 0; i < 6; ++i) { + out[static_cast(i)] = twist[i]; + } + return out; +} + +void accumulateMatrixStats(const std::vector>& matrix, + double& sum_out, + double& peak_out) { + for (const auto& row : matrix) { + for (const auto value : row) { + const double v = static_cast(value); + sum_out += v; + peak_out = std::max(peak_out, v); + } + } +} + +} // namespace + +TouchScreenApp::TouchScreenApp() + : tracker_(nullptr) {} + +bool TouchScreenApp::init(const std::shared_ptr& robot, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera, + const Options& options) { + robot_ = robot; + dexhand_ = dexhand; + camera_ = camera; + options_ = options; + + if (!robot_ || !dexhand_ || !camera_) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if (options_.urdf_path.empty() || options_.camera_link.empty() || options_.control_joint_names.empty()) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + perception_ = std::make_shared(camera_); + perception_->setTagSize(options_.tag_size_m); + + tracker_.setPerception(perception_); + tracker_.setTargetPointMethod(options_.target_point_method); + + if (!ibvs_.init(options_.urdf_path, + options_.base_link, + options_.flange_link, + options_.camera_link)) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + ibvs_.setPerception(perception_); + if (!validateControlJointNames()) { + initialized_ = false; + last_status_ = Status::CONTROL_JOINT_MISMATCH; + return false; + } + + initialized_ = applyOptions(); + if (initialized_) { + tracker_.clear(); + tracker_.resetActiveTagTracking(); + ibvs_.reset(); + phase_ = Phase::IDLE; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + last_active_tag_id_ = -1; + last_touch_pressure_sum_ = 0.0; + last_touch_pressure_peak_ = 0.0; + last_align_error_camera_.setZero(); + last_status_ = Status::IDLE; + } else { + last_status_ = Status::INVALID_CONFIG; + } + return initialized_; +} + +void TouchScreenApp::setOptions(const Options& options) { + options_ = options; + if (initialized_) { + applyOptions(); + } +} + +bool TouchScreenApp::startFromPixel(int u, int v) { + if (!initialized_) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + if (u < 0 || v < 0) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + + stop(); + tracker_.clear(); + tracker_.resetActiveTagTracking(); + ibvs_.reset(); + + target_u_ = u; + target_v_ = v; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + last_touch_pressure_sum_ = 0.0; + last_touch_pressure_peak_ = 0.0; + last_active_tag_id_ = -1; + last_align_error_camera_.setZero(); + phase_ = Phase::ALIGNING; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + phase_start_time_ = Clock::now(); + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; +} + +bool TouchScreenApp::step() { + if (!initialized_) { + last_status_ = Status::NOT_INITIALIZED; + return false; + } + + switch (phase_) { + case Phase::IDLE: + last_status_ = Status::IDLE; + return true; + case Phase::ALIGNING: + return stepAligning(); + case Phase::ALIGN_REACHED: + last_status_ = Status::ALIGN_REACHED; + if (options_.pause_after_align_reached) { + return true; + } + if (!startTouchPhase()) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + case Phase::TOUCHING: + return stepTouching(); + case Phase::DWELLING: + return stepDwelling(); + case Phase::RETRACTING: + return stepRetracting(); + case Phase::DONE: + last_status_ = Status::DONE; + return true; + case Phase::FAILED: + return false; + } + last_status_ = Status::INVALID_CONFIG; + return false; +} + +void TouchScreenApp::stop() { + if (robot_) { + try { + robot_->stopSpeedL(); + } catch (...) { + } + } + + sendZeroJointVelocity(); + holdCurrentControlledPosition(); + + phase_ = Phase::IDLE; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + target_locked_ = false; + ibvs_target_initialized_ = false; + touch_command_started_ = false; + retract_command_started_ = false; + align_stable_count_ = 0; + last_active_tag_id_ = -1; + last_touch_pressure_sum_ = 0.0; + last_touch_pressure_peak_ = 0.0; + last_align_error_camera_.setZero(); + last_status_ = Status::STOPPED; +} + +const char* TouchScreenApp::phaseToString(const Phase phase) { + switch (phase) { + case Phase::IDLE: return "IDLE"; + case Phase::ALIGNING: return "ALIGNING"; + case Phase::ALIGN_REACHED: return "ALIGN_REACHED"; + case Phase::TOUCHING: return "TOUCHING"; + case Phase::DWELLING: return "DWELLING"; + case Phase::RETRACTING: return "RETRACTING"; + case Phase::DONE: return "DONE"; + case Phase::FAILED: return "FAILED"; + } + return "UNKNOWN"; +} + +const char* TouchScreenApp::statusToString(const Status status) { + switch (status) { + case Status::IDLE: return "IDLE"; + case Status::NOT_INITIALIZED: return "NOT_INITIALIZED"; + case Status::INVALID_CONFIG: return "INVALID_CONFIG"; + case Status::CONTROL_JOINT_MISMATCH: return "CONTROL_JOINT_MISMATCH"; + case Status::ALIGN_WAITING_PERCEPTION: return "ALIGN_WAITING_PERCEPTION"; + case Status::ALIGN_WAITING_TRACK: return "ALIGN_WAITING_TRACK"; + case Status::ALIGN_TARGET_SETUP_FAILED: return "ALIGN_TARGET_SETUP_FAILED"; + case Status::ALIGN_COMPUTE_FAILED: return "ALIGN_COMPUTE_FAILED"; + case Status::ALIGN_TIMEOUT: return "ALIGN_TIMEOUT"; + case Status::ALIGNING: return "ALIGNING"; + case Status::ALIGN_REACHED: return "ALIGN_REACHED"; + case Status::TOUCHING: return "TOUCHING"; + case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED"; + case Status::TOUCH_TIMEOUT: return "TOUCH_TIMEOUT"; + case Status::RETRACTING: return "RETRACTING"; + case Status::DONE: return "DONE"; + case Status::STOPPED: return "STOPPED"; + case Status::ROBOT_STATE_FAILED: return "ROBOT_STATE_FAILED"; + case Status::ROBOT_COMMAND_FAILED: return "ROBOT_COMMAND_FAILED"; + } + return "UNKNOWN"; +} + +bool TouchScreenApp::applyOptions() { + if (!perception_) { + return false; + } + + perception_->setTagSize(options_.tag_size_m); + tracker_.setTargetPointMethod(options_.target_point_method); + ibvs_.setLambda(options_.ibvs_lambda); + ibvs_.setMu(options_.ibvs_mu); + ibvs_.setQdotMax(options_.ibvs_qdot_max); + ibvs_.setVelocityLimit6(options_.ibvs_vmax6); + ibvs_.setJointLimitAvoidance(options_.enable_joint_limit_avoidance, + options_.joint_limit_avoidance_gain, + options_.joint_limit_avoidance_margin_ratio, + options_.joint_limit_avoidance_max_push); + ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp); + ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf); + return true; +} + +bool TouchScreenApp::validateControlJointNames() const { + std::vector solver_joint_names; + if (!ibvs_.getChainJointNames(solver_joint_names)) { + return false; + } + if (solver_joint_names == options_.control_joint_names) { + return true; + } + + std::cerr << "[TouchScreenApp] control_joint_names mismatch with IbvsController IK chain\n"; + std::cerr << " app joints :"; + for (const auto& name : options_.control_joint_names) { + std::cerr << " " << name; + } + std::cerr << "\n solver joints:"; + for (const auto& name : solver_joint_names) { + std::cerr << " " << name; + } + std::cerr << '\n'; + return false; +} + +bool TouchScreenApp::stepAligning() { + const auto now = Clock::now(); + const double elapsed = std::chrono::duration(now - phase_start_time_).count(); + if (elapsed > options_.align_timeout_s) { + enterFailed(Status::ALIGN_TIMEOUT); + return false; + } + + perception::AprilTagPerception::Options perception_options; + perception_options.depth_policy = options_.depth_policy; + perception_options.detect_tags = true; + perception_options.fetch_encoded = false; + if (!perception_->update(perception_options)) { + sendZeroJointVelocity(); + last_status_ = Status::ALIGN_WAITING_PERCEPTION; + return true; + } + + bool tracking_ok = false; + if (!target_locked_) { + tracking_ok = tracker_.startTrackingFromPixel(target_u_, target_v_); + if (tracking_ok) { + target_locked_ = true; + ibvs_target_initialized_ = false; + align_stable_count_ = 0; + } + } else { + tracking_ok = tracker_.track(); + } + + if (!tracking_ok) { + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + const int tag_id = tracker_.activeTagId(); + last_active_tag_id_ = tag_id; + if (tag_id < 0) { + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); + if (!tracker_.getAnchorInTag(tag_id, p_t_target)) { + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + ibvs_.setTrackedTagId(tag_id); + if (!ibvs_target_initialized_ || tracker_.lastSwitched()) { + if (!ibvs_.setTargetFromPointInTag(p_t_target, + options_.hover_target_in_camera, + options_.target_rx, + options_.target_ry, + options_.target_rz)) { + enterFailed(Status::ALIGN_TARGET_SETUP_FAILED); + return false; + } + ibvs_target_initialized_ = true; + } + + std::vector q_now; + if (!readControlledJointPositions(q_now)) { + enterFailed(Status::ROBOT_STATE_FAILED); + return false; + } + + std::vector qdot_cmd; + if (!ibvs_.compute(q_now, qdot_cmd)) { + switch (ibvs_.lastComputeStatus()) { + case IbvsController::ComputeStatus::NO_NEW_FRAME: + case IbvsController::ComputeStatus::NO_TAG: + case IbvsController::ComputeStatus::TAG_MISMATCH: + case IbvsController::ComputeStatus::NO_DEPTH: + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + case IbvsController::ComputeStatus::OK: + case IbvsController::ComputeStatus::NOT_READY: + case IbvsController::ComputeStatus::BAD_IMAGE: + case IbvsController::ComputeStatus::INVALID_INPUT: + case IbvsController::ComputeStatus::IK_FAILED: + default: + enterFailed(Status::ALIGN_COMPUTE_FAILED); + return false; + } + } + + if (!sendJointVelocity(qdot_cmd)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + + last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera; + if (std::abs(last_align_error_camera_.x()) <= options_.align_xy_threshold_m && + std::abs(last_align_error_camera_.y()) <= options_.align_xy_threshold_m && + std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m) { + ++align_stable_count_; + } else { + align_stable_count_ = 0; + } + + if (align_stable_count_ >= options_.align_stable_frames) { + sendZeroJointVelocity(); + phase_ = Phase::ALIGN_REACHED; + phase_start_time_ = Clock::now(); + touch_command_started_ = false; + last_status_ = Status::ALIGN_REACHED; + return true; + } + + last_status_ = Status::ALIGNING; + return true; +} + +bool TouchScreenApp::stepTouching() { + if (!touch_command_started_) { + if (!startTouchPhase()) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + } + + updateTouchPressure(); + if (last_touch_pressure_sum_ >= options_.tactile_pressure_sum_threshold || + (options_.tactile_pressure_peak_threshold > 0.0 && + last_touch_pressure_peak_ >= options_.tactile_pressure_peak_threshold)) { + try { + robot_->stopSpeedL(); + } catch (...) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + phase_ = Phase::DWELLING; + phase_start_time_ = Clock::now(); + last_status_ = Status::TOUCH_TRIGGERED; + return true; + } + + const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); + if (elapsed > options_.touch_timeout_s) { + try { + robot_->stopSpeedL(); + } catch (...) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + if (!startRetractPhase(Phase::FAILED, Status::TOUCH_TIMEOUT)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; + } + + last_status_ = Status::TOUCHING; + return true; +} + +bool TouchScreenApp::stepDwelling() { + const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); + if (elapsed < options_.dwell_time_s) { + last_status_ = Status::TOUCH_TRIGGERED; + return true; + } + + if (!startRetractPhase(Phase::DONE, Status::DONE)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + return true; +} + +bool TouchScreenApp::stepRetracting() { + if (!retract_command_started_) { + if (!startRetractPhase(phase_after_retract_, final_status_after_retract_)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + } + + const double elapsed = std::chrono::duration(Clock::now() - phase_start_time_).count(); + if (elapsed < options_.retract_duration_s) { + last_status_ = Status::RETRACTING; + return true; + } + + try { + robot_->stopSpeedL(); + } catch (...) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + holdCurrentControlledPosition(); + phase_ = phase_after_retract_; + last_status_ = final_status_after_retract_; + return phase_ != Phase::FAILED; +} + +bool TouchScreenApp::readControlledJointPositions(std::vector& q_out) const { + if (!robot_) { + return false; + } + + std::vector states; + robot_->getJointsState(states); + std::unordered_map q_map; + q_map.reserve(states.size()); + for (const auto& state : states) { + q_map[state.name] = state.position; + } + + q_out.resize(options_.control_joint_names.size()); + for (size_t i = 0; i < options_.control_joint_names.size(); ++i) { + const auto it = q_map.find(options_.control_joint_names[i]); + if (it == q_map.end()) { + return false; + } + q_out[i] = it->second; + } + return true; +} + +bool TouchScreenApp::sendJointVelocity(const std::vector& qdot) const { + if (!robot_ || qdot.size() != options_.control_joint_names.size()) { + return false; + } + + std::vector cmd; + cmd.reserve(qdot.size()); + for (size_t i = 0; i < qdot.size(); ++i) { + cmd.push_back({options_.control_joint_names[i], qdot[i]}); + } + + try { + robot_->speedJ(cmd); + } catch (...) { + return false; + } + return true; +} + +bool TouchScreenApp::sendZeroJointVelocity() const { + std::vector zero(options_.control_joint_names.size(), 0.0); + return sendJointVelocity(zero); +} + +bool TouchScreenApp::holdCurrentControlledPosition() const { + if (!robot_) { + return false; + } + + std::vector states; + robot_->getJointsState(states); + std::unordered_map q_map; + q_map.reserve(states.size()); + for (const auto& state : states) { + q_map[state.name] = state.position; + } + + std::vector joints; + joints.reserve(options_.control_joint_names.size()); + for (const auto& name : options_.control_joint_names) { + const auto it = q_map.find(name); + if (it == q_map.end()) { + return false; + } + joints.emplace_back(name, it->second, 0.0); + } + + try { + robot_->servoJ(joints, 0.02); + } catch (...) { + return false; + } + return true; +} + +bool TouchScreenApp::startTouchPhase() { + if (!robot_) { + return false; + } + try { + if (!robot_->speedL(toStdVector6(options_.touch_twist_base), + options_.touch_acceleration, + 0.0)) { + return false; + } + } catch (...) { + return false; + } + phase_ = Phase::TOUCHING; + phase_start_time_ = Clock::now(); + touch_command_started_ = true; + retract_command_started_ = false; + last_status_ = Status::TOUCHING; + return true; +} + +bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract, + const Status final_status_after_retract) { + if (!robot_) { + return false; + } + try { + if (!robot_->speedL(toStdVector6(options_.retract_twist_base), + options_.retract_acceleration, + 0.0)) { + return false; + } + } catch (...) { + return false; + } + phase_ = Phase::RETRACTING; + phase_after_retract_ = next_phase_after_retract; + final_status_after_retract_ = final_status_after_retract; + phase_start_time_ = Clock::now(); + retract_command_started_ = true; + last_status_ = Status::RETRACTING; + return true; +} + +void TouchScreenApp::enterFailed(const Status status) { + try { + if (robot_) { + robot_->stopSpeedL(); + } + } catch (...) { + } + sendZeroJointVelocity(); + holdCurrentControlledPosition(); + phase_ = Phase::FAILED; + touch_command_started_ = false; + retract_command_started_ = false; + last_status_ = status; +} + +bool TouchScreenApp::updateTouchPressure() { + if (!dexhand_) { + last_touch_pressure_sum_ = 0.0; + last_touch_pressure_peak_ = 0.0; + return false; + } + + const auto& sensors = dexhand_->getSensorData(); + double sum = 0.0; + double peak = 0.0; + + auto accumulate_finger = [&](const auto& finger_sensor) { + switch (options_.tactile_region) { + case TactileRegion::TIP: + accumulateMatrixStats(finger_sensor.tip.data, sum, peak); + break; + case TactileRegion::FINGER: + accumulateMatrixStats(finger_sensor.finger.data, sum, peak); + break; + case TactileRegion::PAD: + accumulateMatrixStats(finger_sensor.pad.data, sum, peak); + break; + case TactileRegion::TIP_AND_FINGER: + accumulateMatrixStats(finger_sensor.tip.data, sum, peak); + accumulateMatrixStats(finger_sensor.finger.data, sum, peak); + break; + case TactileRegion::THUMB_MIDDLE: + break; + } + }; + + switch (options_.tactile_finger) { + case device::FingerType::PINKY: + accumulate_finger(sensors.pinky); + break; + case device::FingerType::RING: + accumulate_finger(sensors.ring); + break; + case device::FingerType::MIDDLE: + accumulate_finger(sensors.middle); + break; + case device::FingerType::INDEX: + accumulate_finger(sensors.index); + break; + case device::FingerType::THUMB: + switch (options_.tactile_region) { + case TactileRegion::TIP: + accumulateMatrixStats(sensors.thumb.tip.data, sum, peak); + break; + case TactileRegion::FINGER: + accumulateMatrixStats(sensors.thumb.finger.data, sum, peak); + break; + case TactileRegion::PAD: + accumulateMatrixStats(sensors.thumb.pad.data, sum, peak); + break; + case TactileRegion::TIP_AND_FINGER: + accumulateMatrixStats(sensors.thumb.tip.data, sum, peak); + accumulateMatrixStats(sensors.thumb.finger.data, sum, peak); + break; + case TactileRegion::THUMB_MIDDLE: + accumulateMatrixStats(sensors.thumb.middle.data, sum, peak); + break; + } + break; + } + + last_touch_pressure_sum_ = sum; + last_touch_pressure_peak_ = peak; + return true; +} + +} // namespace cmvr::app diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp new file mode 100644 index 00000000..63d02fa9 --- /dev/null +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -0,0 +1,112 @@ +#include "gtest/gtest.h" + +#include +#include +#include + +#include "applications/include/touch_screen_app.h" +#include "device_manager/include/device_manager.h" + +namespace { + +constexpr const char* kConfigPath = + "/home/lgv/cmvr/0-workspace/cmvr-es/cmvr-es/common/config/cabin_robot.xml"; +// 这些 id 需要与现场配置一致；保持为示例调用中的写法。 +constexpr const char* kRobotId = "hc01"; +constexpr const char* kDexhandId = "dexhand1"; +constexpr const char* kCameraId = "cam1"; +constexpr const char* kUrdfPath = + "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf"; +constexpr const char* kBaseLink = "PELVIS_S"; +constexpr const char* kFlangeLink = "R_WRIST_R_S"; +constexpr const char* kCameraLink = "R_CAM"; +constexpr int kTargetU = 320; +constexpr int kTargetV = 240; + +void run_touch_once(int u, int v) { + const XmlNode config(kConfigPath); + if (!config.hasChild("DeviceManager")) { + std::cerr << "DeviceManager node not found\n"; + return; + } + + auto& dm = cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager")); + + auto robot = dm.getDevice(kRobotId); + auto dexhand = dm.getDevice(kDexhandId); + auto camera = dm.getDevice(kCameraId); + + + cmvr::app::TouchScreenApp app; + cmvr::app::TouchScreenApp::Options opt; + + opt.urdf_path = kUrdfPath; + opt.base_link = kBaseLink; + opt.flange_link = kFlangeLink; + opt.camera_link = kCameraLink; + opt.tag_size_m = 0.02; + + opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.12); + opt.pause_after_align_reached = true; + + // 只测试视觉对齐阶段：禁止进入真实下压。 + opt.touch_twist_base << + 0.0, 0.0, 0.0, + 0.0, 0.0, 0.0; + opt.touch_acceleration = 0.6; + + opt.retract_twist_base << + 0.0, 0.0, 0.03, + 0.0, 0.0, 0.0; + opt.retract_duration_s = 0.20; + + opt.tactile_finger = cmvr::device::FingerType::INDEX; + opt.tactile_region = cmvr::app::TouchScreenApp::TactileRegion::FINGER; + opt.tactile_pressure_sum_threshold = 1e12; + + if (!app.init(robot, dexhand, camera, opt)) { + std::cerr << "TouchScreenApp init failed\n"; + return; + } + + if (!app.startFromPixel(u, v)) { + std::cerr << "startFromPixel failed\n"; + return; + } + + while (app.isBusy()) { + if (!app.step()) { + std::cerr << "touch failed, status=" + << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) + << "\n"; + break; + } + std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase()) + << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) + << ", active_tag=" << app.lastActiveTagId() + << ", err_c=[" << app.lastAlignErrorCamera().x() << ", " + << app.lastAlignErrorCamera().y() << ", " + << app.lastAlignErrorCamera().z() << "]\n"; + + if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) { + std::cout << "align reached\n"; + app.stop(); + break; + } + std::this_thread::sleep_for(std::chrono::milliseconds(10)); + } + + if (app.isFinished()) { + std::cout << "touch done\n"; + } else if (app.isFailed()) { + std::cout << "touch failed: " + << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) + << "\n"; + } +} + +} // namespace + +TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) { + run_touch_once(kTargetU, kTargetV); +} diff --git a/cmvr-es/controller/include/ibvs_controller.h b/cmvr-es/controller/include/ibvs_controller.h index f32af6c7..ce841b02 100644 --- a/cmvr-es/controller/include/ibvs_controller.h +++ b/cmvr-es/controller/include/ibvs_controller.h @@ -241,6 +241,13 @@ public: // 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。 const Eigen::Matrix& lastCameraTwistVisp() const { return last_v_camera_visp_; } + /** + * @brief 获取当前 IK 链的关节名称（base->camera 顺序）。 + * @param joint_names 输出关节名称列表。 + * @return 已初始化且成功读取时返回 `true`。 + */ + bool getChainJointNames(std::vector& joint_names) const; + private: /** * @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。 diff --git a/cmvr-es/controller/src/controller_test.cpp b/cmvr-es/controller/src/controller_test.cpp index 579230e0..61d58dd9 100644 --- a/cmvr-es/controller/src/controller_test.cpp +++ b/cmvr-es/controller/src/controller_test.cpp @@ -540,7 +540,7 @@ protected: ibvs_controller_->setDepthZGain(1.0); ibvs_controller_->setVelocityLimit6(vmax6_); ibvs_controller_->setTrackedTagId(tracked_tag_id_); - ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.0, 0), + ibvs_controller_->setTargetFromPointInTag(Eigen::Vector3d(0.08, 0.05, 0), Eigen::Vector3d(0.0, 0.0, 0.4)); ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25); diff --git a/cmvr-es/controller/src/ibvs_controller.cpp b/cmvr-es/controller/src/ibvs_controller.cpp index 31b31d2c..e6ed12f6 100644 --- a/cmvr-es/controller/src/ibvs_controller.cpp +++ b/cmvr-es/controller/src/ibvs_controller.cpp @@ -146,6 +146,14 @@ bool IbvsController::compute(const std::vector& joints_angle, return computeInternal(joints_angle, qdot_out); } +bool IbvsController::getChainJointNames(std::vector& joint_names) const { + if (!dls_solver_) { + joint_names.clear(); + return false; + } + return dls_solver_->getChainJointNames(joint_names); +} + bool IbvsController::computeInternal(const std::vector& joints_angle, std::vector& qdot_out) { last_depth_usage_ = DepthUsage::NONE; @@ -284,9 +292,15 @@ bool IbvsController::computeInternal(const std::vector& joints_angle, return false; } + Eigen::Map q_chain(joints_angle.data(), static_cast(joints_angle.size())); + Eigen::Map qdot_vec(qdot.data(), static_cast(qdot.size())); + const Eigen::VectorXd qdot_soft_limited = dls_solver_->applyJointSoftLimitVelocity(q_chain, qdot_vec); + qdot_out.resize(qdot.size()); for (size_t i = 0; i < qdot.size(); ++i) { - qdot_out[i] = SupportFunctions::clamp(qdot[i], -qdot_max_, qdot_max_); + qdot_out[i] = SupportFunctions::clamp(qdot_soft_limited[static_cast(i)], + -qdot_max_, + qdot_max_); } last_compute_status_ = ComputeStatus::OK; diff --git a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h index 35c9d013..8b75c623 100644 --- a/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h +++ b/cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h @@ -90,6 +90,22 @@ public: double damping = -1.0, double qdot_abs_max = std::numeric_limits::infinity()); + /** + * @brief 对靠近关节位置限位且仍继续向外运动的关节速度做软压缩。 + * + * 该接口不改变主任务的求解方式，只在得到关节速度命令后做逐轴后处理。 + * 软/硬边界参数来自当前 speedL 配置中的： + * - `joint_soft_limit_margin` + * - `joint_hard_limit_margin` + * - `enable_joint_soft_limit_velocity` + * + * @param q_chain 当前链关节位置，size=chain_dof_，单位 rad。 + * @param qdot_des 待后处理的关节速度命令，size=chain_v_dof_，单位 rad/s。 + * @return 经过软限位速度压缩后的关节速度命令。 + */ + Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, + const Eigen::VectorXd& qdot_des); + void setJointLimitAvoidance(bool enable, double gain = 0.2, double margin_ratio = 0.15, @@ -245,8 +261,6 @@ private: Eigen::VectorXd* q_full_out = nullptr); Eigen::VectorXd applyJointVelocityLimits(const Eigen::VectorXd& qdot_des) const; - Eigen::VectorXd applyJointSoftLimitVelocity(const Eigen::VectorXd& q_chain, - const Eigen::VectorXd& qdot_des); Eigen::VectorXd applyJointAccelerationLimits(const Eigen::VectorXd& qdot_des, const Eigen::VectorXd& qdot_reference,