diff --git a/.vscode/extensions.json b/.vscode/extensions.json new file mode 100644 index 00000000..c03d5b9f --- /dev/null +++ b/.vscode/extensions.json @@ -0,0 +1,6 @@ +{ + "recommendations": [ + "ms-vscode.cpptools", + "ms-vscode.cmake-tools" + ] +} diff --git a/.vscode/launch.json b/.vscode/launch.json new file mode 100644 index 00000000..4c435193 --- /dev/null +++ b/.vscode/launch.json @@ -0,0 +1,32 @@ +{ + "configurations": [ + + { + "name": "C/C++: g++ 构建和调试活动文件", + "type": "cppdbg", + "request": "launch", + "program": "${fileDirname}/${fileBasenameNoExtension}", + "args": [], + "stopAtEntry": false, + "cwd": "${fileDirname}", + "environment": [], + "externalConsole": false, + "MIMode": "gdb", + "setupCommands": [ + { + "description": "为 gdb 启用整齐打印", + "text": "-enable-pretty-printing", + "ignoreFailures": true + }, + { + "description": "将反汇编风格设置为 Intel", + "text": "-gdb-set disassembly-flavor intel", + "ignoreFailures": true + } + ], + "preLaunchTask": "C/C++: g++ 生成活动文件", + "miDebuggerPath": "/usr/bin/gdb" + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/.vscode/settings.json b/.vscode/settings.json new file mode 100644 index 00000000..f2daafe2 --- /dev/null +++ b/.vscode/settings.json @@ -0,0 +1,12 @@ +{ + "cmake.configureOnOpen": true, + "cmake.buildDirectory": "${workspaceFolder}/cmake-build-debug", + "cmake.copyCompileCommands": "${workspaceFolder}/compile_commands.json", + "cmake.configureSettings": { + "CMAKE_EXPORT_COMPILE_COMMANDS": "ON" + }, + "C_Cpp.default.configurationProvider": "ms-vscode.cmake-tools", + "C_Cpp.default.compileCommands": "${workspaceFolder}/compile_commands.json", + "C_Cpp.default.cppStandard": "c++17", + "C_Cpp.default.intelliSenseMode": "linux-gcc-x64" +} diff --git a/.vscode/tasks.json b/.vscode/tasks.json new file mode 100644 index 00000000..e9fb5f49 --- /dev/null +++ b/.vscode/tasks.json @@ -0,0 +1,25 @@ +{ + "tasks": [ + { + "type": "cppbuild", + "label": "C/C++: g++ 生成活动文件", + "command": "/usr/bin/g++", + "args": [ + "-fdiagnostics-color=always", + "-g", + "${file}", + "-o", + "${fileDirname}/${fileBasenameNoExtension}" + ], + "options": { + "cwd": "${fileDirname}" + }, + "problemMatcher": [ + "$gcc" + ], + "group": "build", + "detail": "调试器生成的任务。" + } + ], + "version": "2.0.0" +} \ No newline at end of file diff --git a/cmvr-es/applications/CMakeLists.txt b/cmvr-es/applications/CMakeLists.txt index 7d4b7a1f..331fd67c 100644 --- a/cmvr-es/applications/CMakeLists.txt +++ b/cmvr-es/applications/CMakeLists.txt @@ -4,8 +4,12 @@ add_library(applications target_include_directories(applications PUBLIC ${CMAKE_CURRENT_SOURCE_DIR}) -target_link_libraries(applications PUBLIC +target_link_libraries(applications + PUBLIC cmvr_es::controller + cmvr_es::common + PRIVATE + cmvr_es::device_manager ) add_library(cmvr_es::applications ALIAS applications) diff --git a/cmvr-es/applications/include/touch_screen_app.h b/cmvr-es/applications/include/touch_screen_app.h index ad405585..b0fc5f9d 100644 --- a/cmvr-es/applications/include/touch_screen_app.h +++ b/cmvr-es/applications/include/touch_screen_app.h @@ -11,6 +11,7 @@ #include +#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" #include "controller/include/ibvs_controller.h" #include "devices/camera/abstract_camera.h" #include "devices/dexhand/abstract_dexhand.h" @@ -23,36 +24,37 @@ namespace cmvr::app { class TouchScreenApp { public: enum class Phase { - IDLE = 0, - ALIGNING, - ALIGN_REACHED, - TOUCHING, - DWELLING, - RETRACTING, - DONE, - FAILED + IDLE = 0, // 空闲，尚未开始任务。 + ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。 + 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 + IDLE = 0, // 空闲状态。 + NOT_INITIALIZED, // 尚未调用 init() 完成初始化。 + INVALID_CONFIG, // 配置非法，无法启动或应用参数。 + CONTROL_JOINT_MISMATCH, // 控制关节顺序与 IK 链不一致。 + ALIGN_WAITING_PERCEPTION, // 对准阶段等待相机/AprilTag 感知结果。 + ALIGN_WAITING_TRACK, // 对准阶段等待目标点跟踪恢复成功。 + ALIGN_TARGET_SETUP_FAILED,// 视觉目标设置失败，setTargetFromPointInTag 失败。 + ALIGN_COMPUTE_FAILED, // 对准阶段 IBVS 或 IK 计算失败。 + ALIGN_TIMEOUT, // 对准阶段超时仍未收敛。 + ALIGNING, // 正在执行视觉对准。 + ALIGN_REACHED, // 视觉对准完成。 + TOUCHING, // 正在向前触控。 + TACTILE_UNAVAILABLE, // 触觉数据不可用。 + TOUCH_TRIGGERED, // 已检测到接触触发。 + TOUCH_FORWARD_TIMEOUT, // 前进触控时间到，但未触发接触。 + RETRACTING, // 正在回退离开屏幕。 + DONE, // 流程成功完成。 + STOPPED, // 被外部 stop() 主动停止。 + ROBOT_STATE_FAILED, // 读取机器人状态失败。 + ROBOT_COMMAND_FAILED // 向机器人下发控制命令失败。 }; enum class TactileRegion { @@ -64,80 +66,141 @@ public: }; struct Options { + // 是否在触控流程开始前先回到指定初始关节位姿。 + bool move_to_init_position_before_start{false}; + // 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。 + bool move_to_init_position{false}; + // 初始关节位姿目标，在前置回位或结束后回位开启时使用。 + std::vector init_joint_positions{}; + // 回到初始位姿时的 moveJ 主导速度，单位 rad/s。 + double init_movej_vel{1.0}; + // 回到初始位姿时的 moveJ 主导加速度，单位 rad/s^2。 + double init_movej_acc{2.0}; + // IBVS / IK 初始化参数。 + // URDF 文件路径，用于初始化 IbvsController 内部 IK 求解器。 std::string urdf_path; + // IK 链基座 link 名称。 std::string base_link{"PELVIS_S"}; + // IK 链末端法兰 link 名称。 std::string flange_link{"R_WRIST_R_S"}; + // URDF 中相机 link 名称。 std::string camera_link; // 视觉感知参数。 + // AprilTag 实际边长，单位米。 double tag_size_m{0.12}; + // 感知更新时如何使用深度图：不用 / 尽量用 / 必须用。 perception::AprilTagPerception::DepthPolicy depth_policy{ perception::AprilTagPerception::DepthPolicy::NONE}; + // 从像素恢复目标点时采用 tag 平面求交，还是深度图反投影。 perception::TagRelativeTarget3D::TargetPointMethod target_point_method{ perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE}; // 视觉阶段目标：触控点在相机坐标系中的 hover 位置。 - Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.12}; + // 目标点在相机坐标系中的期望位置，单位米。 + Eigen::Vector3d hover_target_in_camera{0.0, 0.0, 0.40}; + // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 double target_rx{3.14159265358979323846}; + // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 double target_ry{0.0}; + // 目标 tag 姿态旋转参数，直接传给 vpRotationMatrix::buildFrom。 double target_rz{0.0}; // IBVS 参数。 + // 视觉伺服增益 lambda。 double ibvs_lambda{0.6}; + // DLS IK 阻尼系数 mu。 double ibvs_mu{0.1}; - double ibvs_qdot_max{0.5}; - std::array ibvs_vmax6{{0.04, 0.04, 0.04, 0.04, 0.04, 0.04}}; + // 单关节最大速度，单位 rad/s。 + double ibvs_qdot_max{0.15}; + // 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。 + std::array ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}}; + // 是否启用关节限位回避。 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_margin_ratio{0.15}; + // 单关节限位回避最大推回速度。 double joint_limit_avoidance_max_push{0.25}; + // `AbstractCamera` 相机坐标系到 ViSP 相机坐标系的旋转矩阵。 Eigen::Matrix3d R_camera_to_visp{Eigen::Matrix3d::Identity()}; + // `AbstractCamera` 相机坐标系到 URDF 相机坐标系的旋转矩阵。 Eigen::Matrix3d R_camera_to_urdf{Eigen::Matrix3d::Identity()}; // 关节控制链，默认右臂 7 轴。 + // 顺序必须与 IbvsController 内部 IK 链顺序一致。 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"}; // 视觉对准收敛判据。 + // 目标点在相机坐标系 x/y 方向的允许误差，单位米。 double align_xy_threshold_m{0.003}; + // 目标点在相机坐标系 z 方向的允许误差，单位米。 double align_z_threshold_m{0.010}; + // tag 当前姿态与目标姿态的允许夹角误差，单位弧度。 + double align_rot_threshold_rad{0.08726646259971647}; + // 连续多少帧都满足阈值，才认为对准完成。 int align_stable_frames{5}; + // 对准阶段超时时间，单位秒。 double align_timeout_s{10.0}; + // 为 true 时对准完成后暂停，不自动进入触控阶段。 bool pause_after_align_reached{false}; - // 触控阶段：speedL 目标 twist（base_link 系）。 + // 触控阶段：speedL 目标 twist。当前按末端 Tool 坐标系解释，字段名保留兼容。 + // 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。 + // 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。 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}; + (Eigen::Matrix() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()}; + // 触控阶段 speedL 的加速度参数。 + double touch_acceleration{3.0}; + // 前进触控阶段的最大持续时间，单位秒。 + // 该时间由 TouchScreenApp 状态机自行计时，不直接传给 robot->speedL(time)。 + // 大于 0 时，达到该时间后无论压力是否达阈值，都会立即进入回退阶段。 + // 小于等于 0 时，表示不启用这条限制。 + double touch_forward_duration_s{1.0}; - // 接触后停留与回退。 + // 接触后停留与回退。当前按末端 Tool 坐标系解释。 + // 检测到接触后在当前位置停留的时间，单位秒。 + // 当该值小于 0 时，表示不做停留，直接把 speedL 切换为回退。 double dwell_time_s{0.05}; + // 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。 + // 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。 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}; + (Eigen::Matrix() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()}; + // 回退阶段 speedL 的加速度参数。 + double retract_acceleration{3.0}; + // 回退阶段持续时间，单位秒。 + double retract_duration_s{0.8}; // 指尖触觉判据。 + // 使用哪根手指的触觉阵列判断是否接触。 device::FingerType tactile_finger{device::FingerType::INDEX}; - TactileRegion tactile_region{TactileRegion::FINGER}; - double tactile_pressure_sum_threshold{3000.0}; + // 使用该手指的哪个触觉区域。 + TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER}; + // 触觉压力和阈值；总和超过该值认为已经接触。 + double tactile_pressure_sum_threshold{100.0}; + // 触觉峰值阈值；为 0 时表示不使用峰值判据。 double tactile_pressure_peak_threshold{0.0}; }; TouchScreenApp(); ~TouchScreenApp() = default; + bool init(); + bool init(const std::shared_ptr& robot, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera); bool init(const std::shared_ptr& robot, const std::shared_ptr& dexhand, const std::shared_ptr& camera, - const Options& options); + const cmvr::config::TouchScreenAppConfig& config); - void setOptions(const Options& options); - const Options& options() const { return options_; } + bool setOptionsFromConfig(); + bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config); bool startFromPixel(int u, int v); bool step(); @@ -168,6 +231,9 @@ public: private: using Clock = std::chrono::steady_clock; + static bool optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config, + Options& options_out); + void setOptions(const Options& options); bool applyOptions(); bool validateControlJointNames() const; bool stepAligning(); @@ -179,6 +245,7 @@ private: bool sendJointVelocity(const std::vector& qdot) const; bool sendZeroJointVelocity() const; bool holdCurrentControlledPosition() const; + bool moveToInitPositionIfEnabled() const; bool startTouchPhase(); bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract); diff --git a/cmvr-es/applications/src/touch_screen_app.cpp b/cmvr-es/applications/src/touch_screen_app.cpp index 90b2a703..66e53dbf 100644 --- a/cmvr-es/applications/src/touch_screen_app.cpp +++ b/cmvr-es/applications/src/touch_screen_app.cpp @@ -3,8 +3,13 @@ #include #include #include +#include #include +#include "common/utils/config_helper/include/config_helper.h" +#include "device_manager/include/device_manager.h" +#include + namespace cmvr::app { namespace { @@ -29,15 +34,250 @@ void accumulateMatrixStats(const std::vector>& } } +bool requiresReinitForOptionChange(const TouchScreenApp::Options& current, + const TouchScreenApp::Options& requested) { + const auto init_joints_differ = [&]() { + if (current.init_joint_positions.size() != requested.init_joint_positions.size()) { + return true; + } + for (size_t i = 0; i < current.init_joint_positions.size(); ++i) { + if (current.init_joint_positions[i].joint_name != requested.init_joint_positions[i].joint_name || + current.init_joint_positions[i].rad != requested.init_joint_positions[i].rad) { + return true; + } + } + return false; + }; + return current.move_to_init_position_before_start != requested.move_to_init_position_before_start || + current.move_to_init_position != requested.move_to_init_position || + init_joints_differ() || + current.init_movej_vel != requested.init_movej_vel || + current.init_movej_acc != requested.init_movej_acc || + current.urdf_path != requested.urdf_path || + current.base_link != requested.base_link || + current.flange_link != requested.flange_link || + current.camera_link != requested.camera_link || + current.control_joint_names != requested.control_joint_names; +} + +Eigen::Matrix3d rotationFromTargetRotvec(double rx, double ry, double rz) { + vpRotationMatrix R_visp; + R_visp.buildFrom(rx, ry, rz); + + Eigen::Matrix3d R = Eigen::Matrix3d::Identity(); + for (int r = 0; r < 3; ++r) { + for (int c = 0; c < 3; ++c) { + R(r, c) = R_visp[r][c]; + } + } + return R; +} + +double rotationErrorRad(const Eigen::Matrix3d& R_current, + const Eigen::Matrix3d& R_target) { + if (!R_current.allFinite() || !R_target.allFinite()) { + return std::numeric_limits::infinity(); + } + + const Eigen::Matrix3d R_err = R_current * R_target.transpose(); + const double cos_angle = std::clamp(0.5 * (R_err.trace() - 1.0), -1.0, 1.0); + return std::acos(cos_angle); +} + +device::TactileRegion toDeviceTactileRegion(const TouchScreenApp::TactileRegion region) { + switch (region) { + case TouchScreenApp::TactileRegion::TIP: return device::TactileRegion::TIP; + case TouchScreenApp::TactileRegion::FINGER: return device::TactileRegion::FINGER; + case TouchScreenApp::TactileRegion::PAD: return device::TactileRegion::PAD; + case TouchScreenApp::TactileRegion::TIP_AND_FINGER: + return device::TactileRegion::TIP_AND_FINGER; + case TouchScreenApp::TactileRegion::THUMB_MIDDLE: + return device::TactileRegion::THUMB_MIDDLE; + } + return device::TactileRegion::TIP; +} + +perception::AprilTagPerception::DepthPolicy toDepthPolicy( + const cmvr::config::TouchScreenDepthPolicy policy) { + switch (policy) { + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_NONE: + return perception::AprilTagPerception::DepthPolicy::NONE; + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_PREFER: + return perception::AprilTagPerception::DepthPolicy::PREFER; + case cmvr::config::TOUCH_SCREEN_DEPTH_POLICY_REQUIRE: + return perception::AprilTagPerception::DepthPolicy::REQUIRE; + } + return perception::AprilTagPerception::DepthPolicy::NONE; +} + +perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod( + const cmvr::config::TouchScreenTargetPointMethod method) { + switch (method) { + case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE: + return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; + case cmvr::config::TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE: + return perception::TagRelativeTarget3D::TargetPointMethod::DEPTH_IMAGE; + } + return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE; +} + +device::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) { + switch (finger) { + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY: + return device::FingerType::PINKY; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING: + return device::FingerType::RING; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE: + return device::FingerType::MIDDLE; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX: + return device::FingerType::INDEX; + case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB: + return device::FingerType::THUMB; + } + return device::FingerType::INDEX; +} + +TouchScreenApp::TactileRegion toTactileRegion( + const cmvr::config::TouchScreenTactileRegion region) { + switch (region) { + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP: + return TouchScreenApp::TactileRegion::TIP; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_FINGER: + return TouchScreenApp::TactileRegion::FINGER; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_PAD: + return TouchScreenApp::TactileRegion::PAD; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER: + return TouchScreenApp::TactileRegion::TIP_AND_FINGER; + case cmvr::config::TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE: + return TouchScreenApp::TactileRegion::THUMB_MIDDLE; + } + return TouchScreenApp::TactileRegion::TIP; +} + +void applyVec3FromConfig(const cmvr::config::TouchScreenVec3& src, + Eigen::Vector3d& dst) { + if (src.has_x()) { + dst.x() = src.x(); + } + if (src.has_y()) { + dst.y() = src.y(); + } + if (src.has_z()) { + dst.z() = src.z(); + } +} + +void applyTwist6FromConfig(const cmvr::config::TouchScreenTwist6& src, + Eigen::Matrix& dst) { + if (src.has_vx()) { + dst[0] = src.vx(); + } + if (src.has_vy()) { + dst[1] = src.vy(); + } + if (src.has_vz()) { + dst[2] = src.vz(); + } + if (src.has_wx()) { + dst[3] = src.wx(); + } + if (src.has_wy()) { + dst[4] = src.wy(); + } + if (src.has_wz()) { + dst[5] = src.wz(); + } +} + +void applyMatrix3dFromConfig(const cmvr::config::TouchScreenMatrix3d& src, + Eigen::Matrix3d& dst) { + if (src.has_m00()) { + dst(0, 0) = src.m00(); + } + if (src.has_m01()) { + dst(0, 1) = src.m01(); + } + if (src.has_m02()) { + dst(0, 2) = src.m02(); + } + if (src.has_m10()) { + dst(1, 0) = src.m10(); + } + if (src.has_m11()) { + dst(1, 1) = src.m11(); + } + if (src.has_m12()) { + dst(1, 2) = src.m12(); + } + if (src.has_m20()) { + dst(2, 0) = src.m20(); + } + if (src.has_m21()) { + dst(2, 1) = src.m21(); + } + if (src.has_m22()) { + dst(2, 2) = src.m22(); + } +} + } // namespace TouchScreenApp::TouchScreenApp() : tracker_(nullptr) {} +bool TouchScreenApp::init() { + cmvr::config::TouchScreenAppConfig config; + if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if (!config.has_robot_id() || config.robot_id().empty() || + !config.has_camera_id() || config.camera_id().empty() || + !config.has_dexhand_id() || config.dexhand_id().empty()) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + try { + auto& dm = device::DeviceManager::getInstance(); + auto robot = dm.getDevice(config.robot_id()); + auto dexhand = dm.getDevice(config.dexhand_id()); + auto camera = dm.getDevice(config.camera_id()); + camera->start(); + return init(robot, dexhand, camera, config); + } catch (...) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } +} + +bool TouchScreenApp::init(const std::shared_ptr& robot, + const std::shared_ptr& dexhand, + const std::shared_ptr& camera) { + cmvr::config::TouchScreenAppConfig config; + if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + return init(robot, dexhand, camera, config); +} + bool TouchScreenApp::init(const std::shared_ptr& robot, const std::shared_ptr& dexhand, const std::shared_ptr& camera, - const Options& options) { + const cmvr::config::TouchScreenAppConfig& config) { + Options options; + if (!optionsFromConfig(config, options)) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + robot_ = robot; dexhand_ = dexhand; camera_ = camera; @@ -49,6 +289,24 @@ bool TouchScreenApp::init(const std::shared_ptr& robot, return false; } + if ((options_.move_to_init_position_before_start || options_.move_to_init_position) && + options_.init_joint_positions.empty()) { + initialized_ = false; + last_status_ = Status::INVALID_CONFIG; + return false; + } + + if (options_.move_to_init_position_before_start) { + try { + auto init_cmd = options_.init_joint_positions; + robot_->moveJ(init_cmd, options_.init_movej_vel, options_.init_movej_acc); + } catch (...) { + initialized_ = false; + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } + } + if (options_.urdf_path.empty() || options_.camera_link.empty() || options_.control_joint_names.empty()) { initialized_ = false; last_status_ = Status::INVALID_CONFIG; @@ -102,10 +360,53 @@ bool TouchScreenApp::init(const std::shared_ptr& robot, } void TouchScreenApp::setOptions(const Options& options) { - options_ = options; - if (initialized_) { - applyOptions(); + if (!initialized_) { + options_ = options; + return; } + + const Options previous = options_; + Options applied = options; + if (requiresReinitForOptionChange(options_, options)) { + std::cerr << "[TouchScreenApp] setOptions() ignored IK-chain option changes after init; " + "call init() again to update device/init pose/urdf/link/joint chain\n"; + applied.move_to_init_position_before_start = previous.move_to_init_position_before_start; + applied.move_to_init_position = previous.move_to_init_position; + applied.init_joint_positions = previous.init_joint_positions; + applied.init_movej_vel = previous.init_movej_vel; + applied.init_movej_acc = previous.init_movej_acc; + applied.urdf_path = previous.urdf_path; + applied.base_link = previous.base_link; + applied.flange_link = previous.flange_link; + applied.camera_link = previous.camera_link; + applied.control_joint_names = previous.control_joint_names; + } + + options_ = applied; + if (!applyOptions()) { + options_ = previous; + applyOptions(); + last_status_ = Status::INVALID_CONFIG; + } +} + +bool TouchScreenApp::setOptionsFromConfig() { + cmvr::config::TouchScreenAppConfig config; + if (!cmvr::ConfigHelper::getTouchScreenAppConfig(config)) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + return setOptionsFromConfig(config); +} + +bool TouchScreenApp::setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config) { + Options options; + if (!optionsFromConfig(config, options)) { + last_status_ = Status::INVALID_CONFIG; + return false; + } + setOptions(options); + return last_status_ != Status::INVALID_CONFIG; } bool TouchScreenApp::startFromPixel(int u, int v) { @@ -148,6 +449,10 @@ bool TouchScreenApp::step() { return false; } + if (dexhand_) { + updateTouchPressure(); + } + switch (phase_) { case Phase::IDLE: last_status_ = Status::IDLE; @@ -234,8 +539,9 @@ const char* TouchScreenApp::statusToString(const Status status) { case Status::ALIGNING: return "ALIGNING"; case Status::ALIGN_REACHED: return "ALIGN_REACHED"; case Status::TOUCHING: return "TOUCHING"; + case Status::TACTILE_UNAVAILABLE: return "TACTILE_UNAVAILABLE"; case Status::TOUCH_TRIGGERED: return "TOUCH_TRIGGERED"; - case Status::TOUCH_TIMEOUT: return "TOUCH_TIMEOUT"; + case Status::TOUCH_FORWARD_TIMEOUT: return "TOUCH_FORWARD_TIMEOUT"; case Status::RETRACTING: return "RETRACTING"; case Status::DONE: return "DONE"; case Status::STOPPED: return "STOPPED"; @@ -245,10 +551,181 @@ const char* TouchScreenApp::statusToString(const Status status) { return "UNKNOWN"; } +bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig& config, + Options& options_out) { + Options options; + + if (config.has_move_to_init_position_before_start()) { + options.move_to_init_position_before_start = config.move_to_init_position_before_start(); + } + if (config.has_move_to_init_position()) { + options.move_to_init_position = config.move_to_init_position(); + } + if (config.init_joint_positions_size() > 0) { + options.init_joint_positions.clear(); + options.init_joint_positions.reserve(static_cast(config.init_joint_positions_size())); + for (const auto& joint : config.init_joint_positions()) { + options.init_joint_positions.emplace_back( + joint.has_joint_name() ? joint.joint_name() : std::string{}, + joint.has_rad() ? joint.rad() : 0.0); + } + } + if (config.has_init_movej_vel()) { + options.init_movej_vel = config.init_movej_vel(); + } + if (config.has_init_movej_acc()) { + options.init_movej_acc = config.init_movej_acc(); + } + + if (config.has_urdf_path()) { + options.urdf_path = config.urdf_path(); + } + if (config.has_base_link()) { + options.base_link = config.base_link(); + } + if (config.has_flange_link()) { + options.flange_link = config.flange_link(); + } + if (config.has_camera_link()) { + options.camera_link = config.camera_link(); + } + + if (config.has_tag_size_m()) { + options.tag_size_m = config.tag_size_m(); + } + if (config.has_depth_policy()) { + options.depth_policy = toDepthPolicy(config.depth_policy()); + } + if (config.has_target_point_method()) { + options.target_point_method = toTargetPointMethod(config.target_point_method()); + } + + if (config.has_hover_target_in_camera()) { + applyVec3FromConfig(config.hover_target_in_camera(), options.hover_target_in_camera); + } + if (config.has_target_rx()) { + options.target_rx = config.target_rx(); + } + if (config.has_target_ry()) { + options.target_ry = config.target_ry(); + } + if (config.has_target_rz()) { + options.target_rz = config.target_rz(); + } + + if (config.has_ibvs_lambda()) { + options.ibvs_lambda = config.ibvs_lambda(); + } + if (config.has_ibvs_mu()) { + options.ibvs_mu = config.ibvs_mu(); + } + if (config.has_ibvs_qdot_max()) { + options.ibvs_qdot_max = config.ibvs_qdot_max(); + } + if (config.has_ibvs_vmax6()) { + Eigen::Matrix ibvs_vmax; + ibvs_vmax << options.ibvs_vmax6[0], options.ibvs_vmax6[1], options.ibvs_vmax6[2], + options.ibvs_vmax6[3], options.ibvs_vmax6[4], options.ibvs_vmax6[5]; + applyTwist6FromConfig(config.ibvs_vmax6(), ibvs_vmax); + for (int i = 0; i < 6; ++i) { + options.ibvs_vmax6[static_cast(i)] = ibvs_vmax[i]; + } + } + if (config.has_enable_joint_limit_avoidance()) { + options.enable_joint_limit_avoidance = config.enable_joint_limit_avoidance(); + } + if (config.has_joint_limit_avoidance_gain()) { + options.joint_limit_avoidance_gain = config.joint_limit_avoidance_gain(); + } + if (config.has_joint_limit_avoidance_margin_ratio()) { + options.joint_limit_avoidance_margin_ratio = config.joint_limit_avoidance_margin_ratio(); + } + if (config.has_joint_limit_avoidance_max_push()) { + options.joint_limit_avoidance_max_push = config.joint_limit_avoidance_max_push(); + } + + if (config.has_r_camera_to_visp()) { + applyMatrix3dFromConfig(config.r_camera_to_visp(), options.R_camera_to_visp); + } + if (config.has_r_camera_to_urdf()) { + applyMatrix3dFromConfig(config.r_camera_to_urdf(), options.R_camera_to_urdf); + } + + if (config.control_joint_names_size() > 0) { + options.control_joint_names.assign(config.control_joint_names().begin(), + config.control_joint_names().end()); + } + + if (config.has_align_xy_threshold_m()) { + options.align_xy_threshold_m = config.align_xy_threshold_m(); + } + if (config.has_align_z_threshold_m()) { + options.align_z_threshold_m = config.align_z_threshold_m(); + } + if (config.has_align_rot_threshold_rad()) { + options.align_rot_threshold_rad = config.align_rot_threshold_rad(); + } + if (config.has_align_stable_frames()) { + options.align_stable_frames = config.align_stable_frames(); + } + if (config.has_align_timeout_s()) { + options.align_timeout_s = config.align_timeout_s(); + } + if (config.has_pause_after_align_reached()) { + options.pause_after_align_reached = config.pause_after_align_reached(); + } + + if (config.has_touch_twist_base()) { + applyTwist6FromConfig(config.touch_twist_base(), options.touch_twist_base); + } + if (config.has_touch_acceleration()) { + options.touch_acceleration = config.touch_acceleration(); + } + if (config.has_touch_forward_duration_s()) { + options.touch_forward_duration_s = config.touch_forward_duration_s(); + } + + if (config.has_dwell_time_s()) { + options.dwell_time_s = config.dwell_time_s(); + } + if (config.has_retract_twist_base()) { + applyTwist6FromConfig(config.retract_twist_base(), options.retract_twist_base); + } + if (config.has_retract_acceleration()) { + options.retract_acceleration = config.retract_acceleration(); + } + if (config.has_retract_duration_s()) { + options.retract_duration_s = config.retract_duration_s(); + } + + if (config.has_tactile_finger()) { + options.tactile_finger = toFingerType(config.tactile_finger()); + } + if (config.has_tactile_region()) { + options.tactile_region = toTactileRegion(config.tactile_region()); + } + if (config.has_tactile_pressure_sum_threshold()) { + options.tactile_pressure_sum_threshold = config.tactile_pressure_sum_threshold(); + } + if (config.has_tactile_pressure_peak_threshold()) { + options.tactile_pressure_peak_threshold = config.tactile_pressure_peak_threshold(); + } + + options_out = options; + return true; +} + bool TouchScreenApp::applyOptions() { if (!perception_) { return false; } + if (!options_.R_camera_to_visp.allFinite() || !options_.R_camera_to_urdf.allFinite()) { + return false; + } + if (!std::isfinite(options_.touch_forward_duration_s) || + options_.touch_forward_duration_s < 0.0) { + return false; + } perception_->setTagSize(options_.tag_size_m); tracker_.setTargetPointMethod(options_.target_point_method); @@ -260,15 +737,8 @@ bool TouchScreenApp::applyOptions() { 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); - - Eigen::Matrix3d RxPi; - RxPi << 1,0,0, - 0,-1,0, - 0,0,-1; - ibvs_.setAlignCameraToUrdf(RxPi); - ibvs_.setAlignCameraToVisp(RxPi); + ibvs_.setAlignCameraToVisp(options_.R_camera_to_visp); + ibvs_.setAlignCameraToUrdf(options_.R_camera_to_urdf); return true; } @@ -395,9 +865,22 @@ bool TouchScreenApp::stepAligning() { } last_align_error_camera_ = tracker_.lastTargetInCamera() - options_.hover_target_in_camera; + const auto* current_tag = perception_->findTag(tag_id); + if (!current_tag) { + sendZeroJointVelocity(); + align_stable_count_ = 0; + last_status_ = Status::ALIGN_WAITING_TRACK; + return true; + } + + const Eigen::Matrix3d R_current = current_tag->T_c_t.block<3, 3>(0, 0); + const Eigen::Matrix3d R_target = + rotationFromTargetRotvec(options_.target_rx, options_.target_ry, options_.target_rz); + const double rot_error_rad = rotationErrorRad(R_current, R_target); 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) { + std::abs(last_align_error_camera_.z()) <= options_.align_z_threshold_m && + rot_error_rad <= options_.align_rot_threshold_rad) { ++align_stable_count_; } else { align_stable_count_ = 0; @@ -424,10 +907,38 @@ bool TouchScreenApp::stepTouching() { } } - 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)) { + if (!dexhand_) { + enterFailed(Status::TACTILE_UNAVAILABLE); + return false; + } + const auto touchTriggered = [&]() { + return 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); + }; + const auto handleTouchTriggered = [&]() { + if (options_.dwell_time_s < 0.0) { + try { + if (!robot_->speedL(toStdVector6(options_.retract_twist_base), + options_.retract_acceleration, + 0.0, + cmvr::CartesianFrame::Tool)) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + } catch (...) { + enterFailed(Status::ROBOT_COMMAND_FAILED); + return false; + } + phase_ = Phase::RETRACTING; + phase_after_retract_ = Phase::DONE; + final_status_after_retract_ = Status::DONE; + phase_start_time_ = Clock::now(); + retract_command_started_ = true; + last_status_ = Status::TOUCH_TRIGGERED; + return true; + } + try { robot_->stopSpeedL(); } catch (...) { @@ -438,17 +949,23 @@ bool TouchScreenApp::stepTouching() { phase_start_time_ = Clock::now(); last_status_ = Status::TOUCH_TRIGGERED; return true; + }; + + if (touchTriggered()) { + return handleTouchTriggered(); } 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); + if (options_.touch_forward_duration_s > 0.0 && + elapsed >= options_.touch_forward_duration_s) { + if (!updateTouchPressure()) { + enterFailed(Status::TACTILE_UNAVAILABLE); return false; } - if (!startRetractPhase(Phase::FAILED, Status::TOUCH_TIMEOUT)) { + if (touchTriggered()) { + return handleTouchTriggered(); + } + if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) { enterFailed(Status::ROBOT_COMMAND_FAILED); return false; } @@ -494,6 +1011,12 @@ bool TouchScreenApp::stepRetracting() { return false; } holdCurrentControlledPosition(); + if ((phase_after_retract_ == Phase::DONE || phase_after_retract_ == Phase::FAILED) && + !moveToInitPositionIfEnabled()) { + phase_ = Phase::FAILED; + last_status_ = Status::ROBOT_COMMAND_FAILED; + return false; + } phase_ = phase_after_retract_; last_status_ = final_status_after_retract_; return phase_ != Phase::FAILED; @@ -578,6 +1101,23 @@ bool TouchScreenApp::holdCurrentControlledPosition() const { return true; } +bool TouchScreenApp::moveToInitPositionIfEnabled() const { + if (!options_.move_to_init_position) { + return true; + } + if (!robot_ || options_.init_joint_positions.empty()) { + return false; + } + + try { + auto init_cmd = options_.init_joint_positions; + robot_->moveJ(init_cmd, options_.init_movej_vel, options_.init_movej_acc); + } catch (...) { + return false; + } + return true; +} + bool TouchScreenApp::startTouchPhase() { if (!robot_) { return false; @@ -585,7 +1125,8 @@ bool TouchScreenApp::startTouchPhase() { try { if (!robot_->speedL(toStdVector6(options_.touch_twist_base), options_.touch_acceleration, - 0.0)) { + 0.0, + cmvr::CartesianFrame::Tool)) { return false; } } catch (...) { @@ -607,7 +1148,8 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract, try { if (!robot_->speedL(toStdVector6(options_.retract_twist_base), options_.retract_acceleration, - 0.0)) { + 0.0, + cmvr::CartesianFrame::Tool)) { return false; } } catch (...) { @@ -634,7 +1176,7 @@ void TouchScreenApp::enterFailed(const Status status) { phase_ = Phase::FAILED; touch_command_started_ = false; retract_command_started_ = false; - last_status_ = status; + last_status_ = moveToInitPositionIfEnabled() ? status : Status::ROBOT_COMMAND_FAILED; } bool TouchScreenApp::updateTouchPressure() { @@ -644,7 +1186,8 @@ bool TouchScreenApp::updateTouchPressure() { return false; } - const auto& sensors = dexhand_->getSensorData(); + const auto& sensors = dexhand_->getSensorData(options_.tactile_finger, + toDeviceTactileRegion(options_.tactile_region)); double sum = 0.0; double peak = 0.0; diff --git a/cmvr-es/applications/src/touch_screen_app_test.cpp b/cmvr-es/applications/src/touch_screen_app_test.cpp index 9d2cb1eb..bccb1dfd 100644 --- a/cmvr-es/applications/src/touch_screen_app_test.cpp +++ b/cmvr-es/applications/src/touch_screen_app_test.cpp @@ -5,21 +5,13 @@ #include #include "applications/include/touch_screen_app.h" -#include "device_manager/include/device_manager.h" + +#include "include/device_manager.h" #include "service/grpc/include/server_runner.h" namespace { constexpr const char* kConfigPath = "/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"; -// 这些 id 需要与现场配置一致；保持为示例调用中的写法。 -constexpr const char* kRobotId = "hc01"; -constexpr const char* kDexhandId = "hand2"; -constexpr const char* kCameraId = "right_hand_cam"; -constexpr const char* kUrdfPath = - "/home/lgv/cmvr/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 = 1280 / 2.0; constexpr int kTargetV = 720 / 2.0; @@ -27,117 +19,75 @@ constexpr int kTargetV = 720 / 2.0; void run_touch_once(int u, int v) { const XmlNode config(kConfigPath); - cmvr::service::ServerRunner runner; - runner.start(config); - 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); - std::shared_ptr dexhand = nullptr; - - camera->start(); - - std::vector init_cmd{}; - init_cmd = { - {"R_SHOULDER_P", -0.2423}, - {"R_SHOULDER_R", 1.2929}, - {"R_SHOULDER_Y", 1.61}, - {"R_ELBOW_R", 1.58}, - {"R_WRIST_P", -2.8792}, - {"R_WRIST_Y", 0.1150}, - {"R_WRIST_R", -0.08}, - }; - robot->moveJ(init_cmd,1.0,2.0); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - + // auto& dm = cmvr::device::DeviceManager::getInstance(); + cmvr::device::DeviceManager::getInstance(config.getChild("DeviceManager")); + // cmvr::service::ServerRunner runner; + // runner.start(config); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; cmvr::app::TouchScreenApp app; - cmvr::app::TouchScreenApp::Options opt; + ASSERT_TRUE(app.init()) + << "TouchScreenApp init failed"; - opt.urdf_path = kUrdfPath; - opt.base_link = kBaseLink; - opt.flange_link = kFlangeLink; - opt.camera_link = kCameraLink; - opt.tag_size_m = 0.02; - opt.ibvs_vmax6 = {0.04, 0.04, 0.04, 0.04, 0.04, 0.04}; - - opt.hover_target_in_camera = Eigen::Vector3d(0.0, 0.0, 0.30); - 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; - opt.align_timeout_s = 100.0; - - 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; - } + ASSERT_TRUE(app.startFromPixel(u, v)) << "startFromPixel failed"; + bool align_reached = false; + bool touch_triggered = false; while (app.isBusy()) { - if (!app.step()) { - std::cerr << "touch failed, status=" - << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) - << "\n"; - break; - } + const bool step_ok = app.step(); + const auto& p_c_target = app.tracker().lastTargetInCamera(); std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase()) << ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus()) << ", active_tag=" << app.lastActiveTagId() + << ", target_c=[" << p_c_target.x() << ", " + << p_c_target.y() << ", " + << p_c_target.z() << "]" + << ", pressure_sum=" << app.lastTouchPressureSum() + << ", pressure_peak=" << app.lastTouchPressurePeak() << ", 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::cout << "align reached, target_c=[" << p_c_target.x() << ", " + << p_c_target.y() << ", " + << p_c_target.z() << "]\n"; + align_reached = true; } - 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"; - } - - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + if (app.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) { + std::cout << "touch triggered, pressure_sum=" << app.lastTouchPressureSum() + << ", pressure_peak=" << app.lastTouchPressurePeak() << "\n"; + touch_triggered = true; + } + if (!step_ok) { + const auto failed_status = app.lastStatus(); + app.stop(); + FAIL() << "touch flow failed, status=" + << cmvr::app::TouchScreenApp::statusToString(failed_status); + } + std::this_thread::sleep_for(std::chrono::milliseconds(1)); } + const auto final_status = app.lastStatus(); + ASSERT_TRUE(align_reached) + << "align was never reached, final status=" + << cmvr::app::TouchScreenApp::statusToString(final_status); + ASSERT_TRUE(touch_triggered) + << "touch was never triggered, final status=" + << cmvr::app::TouchScreenApp::statusToString(final_status); + ASSERT_TRUE(app.isFinished()) + << "touch did not finish successfully, final status=" + << cmvr::app::TouchScreenApp::statusToString(final_status); + std::cout << "touch done, final status=" + << cmvr::app::TouchScreenApp::statusToString(final_status) << "\n"; + app.stop(); } } // namespace TEST(TouchScreenAppTest, RunTouchOnceOnRealRobot) { - run_touch_once(kTargetU, kTargetV); } diff --git a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt index e2d2d969..57ff09fa 100644 --- a/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt +++ b/cmvr-es/common/config/dexhand_config/dexhand_config.pb.txt @@ -7,7 +7,7 @@ rh56dftp_dexhands { rh56dftp_dexhands { id: "hand2" - ip: "192.168.1.214" + ip: "192.168.1.224" port: 6000 - enable: false + enable: true } \ No newline at end of file diff --git a/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt new file mode 100644 index 00000000..f7e040f7 --- /dev/null +++ b/cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt @@ -0,0 +1,123 @@ +robot_id: "hc01" +dexhand_id: "hand2" +camera_id: "right_hand_cam" +move_to_init_position_before_start: true +move_to_init_position: true +init_joint_positions { + joint_name: "R_SHOULDER_P" + rad: -0.2423 +} +init_joint_positions { + joint_name: "R_SHOULDER_R" + rad: 1.2929 +} +init_joint_positions { + joint_name: "R_SHOULDER_Y" + rad: 1.61 +} +init_joint_positions { + joint_name: "R_ELBOW_R" + rad: 1.58 +} +init_joint_positions { + joint_name: "R_WRIST_P" + rad: -2.8792 +} +init_joint_positions { + joint_name: "R_WRIST_Y" + rad: 0.1150 +} +init_joint_positions { + joint_name: "R_WRIST_R" + rad: -0.08 +} +init_movej_vel: 1.0 +init_movej_acc: 2.0 + +urdf_path: "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf" +base_link: "PELVIS_S" +flange_link: "R_WRIST_R_S" +camera_link: "R_CAM" + +tag_size_m: 0.02 +depth_policy: TOUCH_SCREEN_DEPTH_POLICY_NONE +target_point_method: TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE + +hover_target_in_camera { + x: 0.0 + y: 0.0 + z: 0.20 +} +target_rx: 3.14159265358979323846 +target_ry: 0.0 +target_rz: 0.0 + +ibvs_lambda: 0.6 +ibvs_mu: 0.1 +ibvs_qdot_max: 0.15 +ibvs_vmax6 { + vx: 0.15 + vy: 0.15 + vz: 0.20 + wx: 0.6 + wy: 0.6 + wz: 0.6 +} +enable_joint_limit_avoidance: true +joint_limit_avoidance_gain: 0.2 +joint_limit_avoidance_margin_ratio: 0.15 +joint_limit_avoidance_max_push: 0.25 + +r_camera_to_visp { + m00: 1.0 + m11: 1.0 + m22: 1.0 +} +r_camera_to_urdf { + m00: 1.0 + m11: 1.0 + m22: 1.0 +} + +control_joint_names: "R_SHOULDER_P" +control_joint_names: "R_SHOULDER_R" +control_joint_names: "R_SHOULDER_Y" +control_joint_names: "R_ELBOW_R" +control_joint_names: "R_WRIST_P" +control_joint_names: "R_WRIST_Y" +control_joint_names: "R_WRIST_R" + +align_xy_threshold_m: 0.003 +align_z_threshold_m: 0.010 +align_rot_threshold_rad: 0.08726646259971647 +align_stable_frames: 5 +align_timeout_s: 100.0 +pause_after_align_reached: false + +touch_twist_base { + vx: 0.0 + vy: -0.08 + vz: 0.0 + wx: 0.0 + wy: 0.0 + wz: 0.0 +} +touch_acceleration: 3.0 +touch_forward_duration_s: 1.2 + +dwell_time_s: -1.0 +retract_twist_base { + vx: 0.0 + vy: 0.08 + vz: 0.0 + wx: 0.0 + wy: 0.0 + wz: 0.0 +} +retract_acceleration: 3.0 +retract_duration_s: 1.0 + +tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX +tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP +tactile_pressure_sum_threshold: 300.0 +tactile_pressure_peak_threshold: 0.0 diff --git a/cmvr-es/common/utils/config_helper/include/config_helper.h b/cmvr-es/common/utils/config_helper/include/config_helper.h index 6f575d4c..7d189128 100644 --- a/cmvr-es/common/utils/config_helper/include/config_helper.h +++ b/cmvr-es/common/utils/config_helper/include/config_helper.h @@ -9,6 +9,7 @@ #include "cmvr/config/dexhand_config/dexhand_config.pb.h" #include "cmvr/config/microphone_config/microphone_config.pb.h" #include "cmvr/config/speaker_config/speaker_conifg.pb.h" +#include "cmvr/config/touch_screen_app_config/touch_screen_app_config.pb.h" #define GET_CONFIG(file, para) \ ([&]() -> bool { \ @@ -71,6 +72,16 @@ namespace cmvr return GET_CONFIG(speaker_config_file, config); } + static bool getTouchScreenAppConfig(config::TouchScreenAppConfig& config) + { + return GET_CONFIG(touch_screen_app_config_file, config); + } + + static bool setTouchScreenAppConfig(const config::TouchScreenAppConfig& config) + { + return SET_CONFIG(config, touch_screen_app_config_file); + } + private: // Make macros able to call these (macros call ::cmvr::ConfigHelper::xxx) template diff --git a/cmvr-es/common/utils/config_helper/include/config_setting.h b/cmvr-es/common/utils/config_helper/include/config_setting.h index 25080311..5d58fb9e 100644 --- a/cmvr-es/common/utils/config_helper/include/config_setting.h +++ b/cmvr-es/common/utils/config_helper/include/config_setting.h @@ -4,4 +4,5 @@ DECLARE_string(pinocchio_qp_ik_solver_config_file); DECLARE_string(camera_config_file); DECLARE_string(dexhand_config_file); DECLARE_string(microphone_config_file); -DECLARE_string(speaker_config_file); \ No newline at end of file +DECLARE_string(speaker_config_file); +DECLARE_string(touch_screen_app_config_file); diff --git a/cmvr-es/common/utils/config_helper/src/config_setting.cpp b/cmvr-es/common/utils/config_helper/src/config_setting.cpp index 2e32d6ed..a0690df6 100644 --- a/cmvr-es/common/utils/config_helper/src/config_setting.cpp +++ b/cmvr-es/common/utils/config_helper/src/config_setting.cpp @@ -35,3 +35,6 @@ DEFINE_string(speaker_config_file, basePath() + "speaker_config/speaker_config.pb.txt", "The configuration file for speaker"); +DEFINE_string(touch_screen_app_config_file, + basePath() + "touch_screen_app_config/touch_screen_app_config.pb.txt", + "The configuration file for TouchScreenApp"); diff --git a/cmvr-es/devices/dexhand/abstract_dexhand.h b/cmvr-es/devices/dexhand/abstract_dexhand.h index 803b322b..9f5d502c 100644 --- a/cmvr-es/devices/dexhand/abstract_dexhand.h +++ b/cmvr-es/devices/dexhand/abstract_dexhand.h @@ -75,6 +75,15 @@ namespace cmvr::device{ THUMB // 大拇指 }; + // 触觉区域枚举（设备层）。 + enum class TactileRegion { + TIP = 0, + FINGER, + PAD, + TIP_AND_FINGER, + THUMB_MIDDLE + }; + // 整只手的触觉传感器数据 struct HandTactileSensors { // 五指的触觉数据（每根手指包含指端、指尖、指腹） @@ -219,10 +228,15 @@ namespace cmvr::device{ virtual void execPresetAct(int action_id) {} virtual void setForce(const std::vector& finger_joint_force) {} virtual HandTactileSensors& getSensorData() { return hand_tactile_sensors_;} + virtual HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) { + (void)finger_type; + (void)tactile_region; + return getSensorData(); + } protected: DexHandState status_; HandTactileSensors hand_tactile_sensors_; }; } -#endif //CMVR_ES_ABSTRACT_DEXHAND_H \ No newline at end of file +#endif //CMVR_ES_ABSTRACT_DEXHAND_H diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h index 43a59ddb..9455d103 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h @@ -61,6 +61,7 @@ namespace cmvr::device { void execPresetAct(int action_id) override;//这个无用 void setForce(const std::vector& finger_joint_force) override; HandTactileSensors& getSensorData() override; + HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) override; private: void updateState(); void updateSensorData(); diff --git a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp index 764ee0c6..da2dfb83 100644 --- a/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp +++ b/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp @@ -35,6 +35,21 @@ const int TOUCH_SENSOR_END_ADDR_PALM = 5123; // Modbus 每次最多读取寄存器的数量 const int MAX_REGISTERS_PER_READ = 125; +namespace { + +std::pair fingerSensorRegisterRange(const FingerType finger_type) { + switch (finger_type) { + case PINKY: return {TOUCH_SENSOR_BASE_ADDR_PINKY, TOUCH_SENSOR_END_ADDR_PINKY}; + case RING: return {TOUCH_SENSOR_BASE_ADDR_RING, TOUCH_SENSOR_END_ADDR_RING}; + case MIDDLE: return {TOUCH_SENSOR_BASE_ADDR_MIDDLE, TOUCH_SENSOR_END_ADDR_MIDDLE}; + case INDEX: return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX}; + case THUMB: return {TOUCH_SENSOR_BASE_ADDR_THUMB, TOUCH_SENSOR_END_ADDR_THUMB}; + } + return {TOUCH_SENSOR_BASE_ADDR_INDEX, TOUCH_SENSOR_END_ADDR_INDEX}; +} + +} // namespace + ModbusController::ModbusController() { // 初始化寄存器字典 regdict = { @@ -443,3 +458,13 @@ HandTactileSensors& RH56DFTPDexhand::getSensorData() return hand_tactile_sensors_; } + +HandTactileSensors& RH56DFTPDexhand::getSensorData(const FingerType finger_type, + const TactileRegion tactile_region) +{ + (void)tactile_region; + const auto [start_addr, end_addr] = fingerSensorRegisterRange(finger_type); + const auto finger_register_values = controller_->readRegisterRange(start_addr, end_addr); + hand_tactile_sensors_.parseFinger(finger_type, finger_register_values); + return hand_tactile_sensors_; +} diff --git a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp index a119df4b..57f83aa3 100644 --- a/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp +++ b/cmvr-es/devices/motor/ti5_motor/canopen/protocol/ti5_motor_tpdo1.cpp @@ -26,12 +26,12 @@ void Ti5MotorTPDO1::Parse(const std::uint8_t *bytes, int32_t length, msgs::Robot statusword_t st{}; st.value = motor_status->status_word(); - if (st.op_mode_specific > 0) { - LOG(INFO) << st.op_mode_specific ; - } + // if (st.op_mode_specific > 0) { + // LOG(INFO) << st.op_mode_specific ; + // } - LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ; + // LOG(INFO) << " Motor ID " << int(this->node_id_) << " mode = " << motor_status->run_mode() ; diff --git a/cmvr-es/devices/robot/abstract_robot.h b/cmvr-es/devices/robot/abstract_robot.h index 09c5d62b..823caf44 100644 --- a/cmvr-es/devices/robot/abstract_robot.h +++ b/cmvr-es/devices/robot/abstract_robot.h @@ -9,6 +9,7 @@ #include "json/json.h" #include "../abstract_device.h" #include "../../utils/controller/include/cartesian_controller.h" +#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h" #include "cmvr/msgs/geometry.pb.h" #include "cmvr/msgs/motor.pb.h" @@ -108,7 +109,12 @@ namespace cmvr::device{ virtual void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } virtual void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) { throw std::runtime_error("Not implemented"); } - virtual bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0) { throw std::runtime_error("Not implemented"); } + virtual bool speedL(const std::vector &xd, + double acceleration = 0.25, + double time = 0.0, + cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) { + throw std::runtime_error("Not implemented"); + } virtual void stopSpeedL() { throw std::runtime_error("Not implemented"); } virtual void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc=0.5) { throw std::runtime_error("Not implemented"); } diff --git a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h index 0f881368..51bf2d4e 100644 --- a/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h +++ b/cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h @@ -94,7 +94,10 @@ namespace cmvr::device{ void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override; - bool speedL(const std::vector &xd, double acceleration = 0.25, double time = 0.0) override; + bool speedL(const std::vector &xd, + double acceleration = 0.25, + double time = 0.0, + cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override; void stopSpeedL() override; Eigen::Matrix getSpeedLCommandTwistBase(); @@ -215,6 +218,7 @@ namespace cmvr::device{ Eigen::Matrix speedl_target_twist_{Eigen::Matrix::Zero()}; Eigen::Matrix speedl_last_command_twist_base_{Eigen::Matrix::Zero()}; double speedl_target_acceleration_{0.25}; + cmvr::CartesianFrame speedl_target_frame_{cmvr::CartesianFrame::Base}; double speedl_applied_acceleration_{0.25}; std::uint64_t speedl_command_version_{0}; diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp index 140febcb..0e4341ce 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp @@ -87,7 +87,10 @@ void HumanoidRobot::init() { template -bool HumanoidRobot::speedL(const std::vector& xd, double acceleration, double time) +bool HumanoidRobot::speedL(const std::vector& xd, + double acceleration, + double time, + cmvr::CartesianFrame frame) { if (!ik_solver_) { throw runtime_error("speedL failed: ik_solver_ is not initialized"); @@ -115,6 +118,7 @@ bool HumanoidRobot::speedL(const std::vector& xd, double accelerati std::lock_guard lock(speedl_mutex_); speedl_target_twist_ = target_twist; speedl_target_acceleration_ = acceleration; + speedl_target_frame_ = frame; speedl_command_active_ = true; command_version = ++speedl_command_version_; } @@ -127,6 +131,7 @@ bool HumanoidRobot::speedL(const std::vector& xd, double accelerati std::lock_guard lock(speedl_mutex_); if (speedl_command_version_ == command_version) { speedl_target_twist_.setZero(); + speedl_target_frame_ = cmvr::CartesianFrame::Base; speedl_command_active_ = true; ++speedl_command_version_; should_stop = true; @@ -149,6 +154,7 @@ void HumanoidRobot::stopSpeedL() { std::lock_guard lock(speedl_mutex_); speedl_target_twist_.setZero(); + speedl_target_frame_ = cmvr::CartesianFrame::Base; speedl_command_active_ = true; ++speedl_command_version_; } @@ -215,6 +221,7 @@ void HumanoidRobot::stopSpeedLWorker_() { speedl_stop_requested_.store(true); speedl_command_active_ = false; speedl_target_twist_.setZero(); + speedl_target_frame_ = cmvr::CartesianFrame::Base; speedl_last_command_twist_base_.setZero(); } speedl_cv_.notify_all(); @@ -272,6 +279,7 @@ void HumanoidRobot::speedLWorkerLoop_() { while (true) { Eigen::Matrix target_twist = Eigen::Matrix::Zero(); double acceleration = 0.25; + cmvr::CartesianFrame target_frame = cmvr::CartesianFrame::Base; { std::unique_lock lock(speedl_mutex_); speedl_cv_.wait(lock, [&]() { @@ -283,6 +291,7 @@ void HumanoidRobot::speedLWorkerLoop_() { } target_twist = speedl_target_twist_; acceleration = speedl_target_acceleration_; + target_frame = speedl_target_frame_; } next_tick = std::chrono::steady_clock::now(); @@ -300,6 +309,7 @@ void HumanoidRobot::speedLWorkerLoop_() { } target_twist = speedl_target_twist_; acceleration = speedl_target_acceleration_; + target_frame = speedl_target_frame_; } if (!updateSpeedLAccelerationConfig_(acceleration)) { @@ -323,7 +333,7 @@ void HumanoidRobot::speedLWorkerLoop_() { dt, q_now, qd_cmd, - cmvr::CartesianFrame::Base, + target_frame, true)) { LOG(ERROR) << "speedL worker: speedLStep() failed"; send_zero(); diff --git a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp index 22c7e947..eb26e14c 100644 --- a/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp +++ b/cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp @@ -22,8 +22,11 @@ #include "../../../../device_manager/include/device_manager.h" #include +#include "common/utils/image/image_process.h" #include "controller/include/ibvs_controller.h" #include "ik_solver/include/pinocchio_dls_ik_solver.h" +#include "perception/include/apriltag_perception.h" +#include "perception/include/tag_relative_target_3d.h" #include "cmvr/msgs/can_card_parameter.grpc.pb.h" #include "../include/humanoid_robot.h" #include "motor/ti5_motor/canopen/ti5_motor_canopen_protocol.h" @@ -78,188 +81,16 @@ std::unordered_map makeRightArmQMap() { return q_map; } -constexpr std::array kRightArmQMin = { - -3.14, // R_SHOULDER_P - -0.78, // R_SHOULDER_R - -3.14, // R_SHOULDER_Y - 0.00, // R_ELBOW_R - -3.14, // R_WRIST_P - -0.78, // R_WRIST_Y - -0.55 // R_WRIST_R -}; - -constexpr std::array kRightArmQMax = { - 3.14, // R_SHOULDER_P - 1.57, // R_SHOULDER_R - 3.14, // R_SHOULDER_Y - 2.05, // R_ELBOW_R - 3.14, // R_WRIST_P - 0.78, // R_WRIST_Y - 1.57 // R_WRIST_R -}; - -double applySoftJointLimitVelocity(double q, - double v, - double q_min, - double q_max, - double soft_margin, - double hard_margin) { - if (q_max <= q_min) return 0.0; - const double hard = std::max(1e-4, hard_margin); - const double soft = std::max(hard + 1e-4, soft_margin); - - if (v < 0.0) { - const double q_hard = q_min + hard; - const double q_soft = q_min + soft; - if (q <= q_hard) return 0.0; - if (q < q_soft) { - const double s = std::clamp((q - q_hard) / (q_soft - q_hard), 0.0, 1.0); - return v * s; - } - } else if (v > 0.0) { - const double q_hard = q_max - hard; - const double q_soft = q_max - soft; - if (q >= q_hard) return 0.0; - if (q > q_soft) { - const double s = std::clamp((q_hard - q) / (q_hard - q_soft), 0.0, 1.0); - return v * s; - } - } - return v; -} - } // namespace -TEST(HumanoidRobotTest,GetState) { - std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; - const XmlNode config(config_path); - - if (!config.hasChild("DeviceManager")){ - LOG(ERROR) << "Device Manager node not found"; - } - auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - robot->torqueOff(); - std::vector joints; - - while (true) { - joints.clear(); - robot->getJointsState(joints); - for (const auto &js : joints) { - LOG(INFO) << "Joint: " << js.name - << ", Position: " << js.position - << ", Velocity: " << js.velocity - << std::endl; - } - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - } -} - - -TEST(HumanoidRobotTest,MyRobotTest) { - - std::string config_path = "/home/lgv/cmvr/cmvr-es/config/cabin_robot.xml"; - const XmlNode config(config_path); - - if (!config.hasChild("DeviceManager")){ - LOG(ERROR) << "Device Manager node not found"; - } - auto dmgr_cfg = config.getChild("DeviceManager"); - auto &dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - robot->calibrateZeroQ("R_WRIST_R"); - - // std::vector cmd{}; - -} - -TEST(HumanoidRobotTest,ServoJAndGetJointQSmokeTest) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - - auto q_map_now = makeRightArmQMap(); - std::vector q_now(kRightArmJointNames.size(), 0.0); - auto refreshRightArmQ = [&]() -> bool { - robot->getJointQ(q_map_now); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const auto it = q_map_now.find(kRightArmJointNames[i]); - if (it == q_map_now.end()) return false; - q_now[i] = it->second; - } - return true; - }; - ASSERT_TRUE(refreshRightArmQ()) << "Failed to read right-arm joint q"; - std::vector cmd{}; - cmd = { - // {"R_SHOULDER_P", 0.0}, - // {"R_SHOULDER_R", 0.0}, - // {"R_SHOULDER_Y", 0.0}, - // {"R_ELBOW_R", 0.0}, - // {"R_WRIST_P", 0.0}, - // {"R_WRIST_Y", 0.0}, - {"R_WRIST_R", 0.2}, - }; - // auto joint_cmd = buildRightArmJointCmd(q_now, 0.8); - ASSERT_NO_THROW(robot->servoJ(cmd, 0.8, 0.01)); - while (true) { - std::this_thread::sleep_for(std::chrono::milliseconds(10000)); - } - -} - - -TEST(HumanoidRobotTest,speedJTest) { - const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); - ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; - auto dmgr_cfg = config.getChild("DeviceManager"); - auto& dmgr = DeviceManager::getInstance(dmgr_cfg); - - auto robot = dmgr.getDevice("hc01"); - std::vector cmd{}; - cmd = { - // {"R_SHOULDER_P", 0.0}, - // {"R_SHOULDER_R", 0.0}, - // {"R_SHOULDER_Y", 0.0}, - // {"R_ELBOW_R", 0.0}, - // {"R_WRIST_P", 0.0}, - // {"R_WRIST_Y", 0.0}, - {"R_WRIST_R", 0.2}, - }; - robot->speedJ(cmd); - std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - // robot->speedJ(0); - // - std::vector cmd1{}; - cmd1 = { - // {"R_SHOULDER_P", 0.0}, - // {"R_SHOULDER_R", 0.0}, - // {"R_SHOULDER_Y", 0.0}, - // {"R_ELBOW_R", 0.0}, - // {"R_WRIST_P", 0.0}, - // {"R_WRIST_Y", 0.0}, - {"R_WRIST_R", 0}, - }; - robot->moveJ(cmd1,2.0,10.0); - // robot->eStop(); - - // -} - TEST(HumanoidRobotTest, speedLSmokeTest) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; @@ -283,8 +114,8 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { robot->moveJ(init_cmd,1.0,2.0); std::this_thread::sleep_for(std::chrono::milliseconds(1000)); - const std::vector twist_pos = {0.08, 0, 0, 0.00, 0, 0}; - const std::vector twist_neg = {-0.08, 0, 0, 0.00, 0, 0}; + const std::vector twist_pos = {0.00, -0.08, 0, 0.00, 0, 0}; + const std::vector twist_neg = {0.00, 0.08, 0, 0.00, 0, 0}; const double acceleration = 3.0; const double segment_time = 1.0; const double settle_time = 1.0; @@ -302,7 +133,7 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { double duration, const char* phase) { LOG(INFO) << "speedLSmokeTest phase: " << phase; - ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0)); + ASSERT_NO_THROW(robot->speedL(target_twist, acceleration, 0.0, cmvr::CartesianFrame::Tool)); const int steps = static_cast(std::ceil(duration / sample_dt)); for (int i = 0; i < steps; ++i) { const Eigen::Matrix cmd_twist = robot->getSpeedLCommandTwistBase(); @@ -379,177 +210,6 @@ TEST(HumanoidRobotTest, speedLSmokeTest) { // show(fig); } -TEST(HumanoidRobotTest, SpeedLOpenLoopPlannerPlot) { - cmvr::PinocchioDlsIKSolver solver( - "/home/lgv/cmvr/0-workspace/cmvr-es/model/xiaoyan_description/dual_arm.urdf", - "PELVIS_S", - "R_WRIST_R_S", - "R_FINGER_TIP_FIXED"); - ASSERT_TRUE(solver.init()) << "PinocchioDlsIKSolver init failed"; - - cmvr::PinocchioDlsIKSolver::SpeedLConfig speedl_config; - speedl_config.linear_velocity_max = 0.55; - speedl_config.linear_acceleration_max = 0.80; - speedl_config.linear_jerk_max = 3.30; - speedl_config.angular_velocity_max = 1.00; - speedl_config.angular_acceleration_max = 3.00; - speedl_config.angular_jerk_max = 12.0; - speedl_config.joint_acceleration_max = std::vector(7, 8.0); - speedl_config.linear_target_replan_threshold = 1e-4; - speedl_config.angular_target_replan_threshold = 1e-4; - speedl_config.linear_reverse_cos_threshold = -0.8660254037844386; - speedl_config.linear_reverse_switch_speed_threshold = 1e-3; - ASSERT_TRUE(solver.configureSpeedL(speedl_config)) << "configureSpeedL failed"; - - std::vector q_init = {0.25, 1.00, M_PI / 2, M_PI / 2, -M_PI / 2, 0, 0}; - solver.update_joints_state(q_init); - - const double dt = 0.002; - const int log_every = 50; - const double segment_time = 1.5; - const double stop_time = 1.0; - const double settle_time = 1.0; - const double linear_speed_cmd = 0.1; - const double z_speed_cmd = 0.10; - const double total_time = 2.0 * segment_time + stop_time; - const int active_steps = static_cast(std::ceil(total_time / dt)); - const int settle_steps = static_cast(std::ceil(settle_time / dt)); - const int total_steps = active_steps + settle_steps; - - int ok_steps = 0; - std::vector t_trace; - std::vector target_vy_trace; - std::vector target_vz_trace; - std::vector command_vx_trace; - std::vector command_vy_trace; - std::vector command_vz_trace; - std::vector> qd_cmd_trace(kRightArmJointNames.size()); - t_trace.reserve(total_steps); - target_vy_trace.reserve(total_steps); - target_vz_trace.reserve(total_steps); - command_vx_trace.reserve(total_steps); - command_vy_trace.reserve(total_steps); - command_vz_trace.reserve(total_steps); - for (auto& v : qd_cmd_trace) v.reserve(total_steps); - - for (int step = 0; step < total_steps; ++step) { - const double t = static_cast(step) * dt; - - Eigen::Matrix target_twist = Eigen::Matrix::Zero(); - if (t < segment_time) { - target_twist[1] = linear_speed_cmd; - target_twist[2] = z_speed_cmd; - - } else if (t < 2.0 * segment_time) { - target_twist[1] = -linear_speed_cmd; - target_twist[2] = -z_speed_cmd; - } else if (t < total_time) { - target_twist.setZero(); - } else { - target_twist.setZero(); - } - - std::vector qd_cmd; - ASSERT_TRUE(solver.speedLStep(target_twist, - dt, - qd_cmd, - cmvr::CartesianFrame::Base, - true)) - << "speedLStep failed at step " << step; - - t_trace.push_back(t); - target_vy_trace.push_back(target_twist[1]); - target_vz_trace.push_back(target_twist[2]); - command_vx_trace.push_back(solver.getSpeedLCommandTwistBase()[0]); - command_vy_trace.push_back(solver.getSpeedLCommandTwistBase()[1]); - command_vz_trace.push_back(solver.getSpeedLCommandTwistBase()[2]); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - qd_cmd_trace[i].push_back(i < qd_cmd.size() ? qd_cmd[i] : 0.0); - } - - ++ok_steps; - - if ((step % log_every) == 0) { - std::cout << "[SPEEDL_OPEN_LOOP] step=" << step - << " t=" << t - << " target_vy=" << target_twist[1] - << " target_vz=" << target_twist[2] - << " phase=" - << (t < segment_time ? "pos" : - (t < 2.0 * segment_time ? "neg" : - (t < total_time ? "stop" : "settle"))) - << " cmd_qd="; - for (const auto& v : qd_cmd) { - std::cout << v << " "; - } - std::cout << std::endl; - } - } - - if (!t_trace.empty()) { - using namespace matplot; - auto fig = figure(true); - fig->size(1600, 1200); - fig->font_size(16); - - auto ax1 = subplot(3, 1, 0); - hold(ax1, true); - auto l_target_y = plot(ax1, t_trace, target_vy_trace, "k--"); - l_target_y->line_width(2.0f); - auto l_target_z = plot(ax1, t_trace, target_vz_trace, "c--"); - l_target_z->line_width(2.0f); - auto l_cmd_x = plot(ax1, t_trace, command_vx_trace, "r-"); - l_cmd_x->line_width(2.0f); - auto l_cmd_y = plot(ax1, t_trace, command_vy_trace, "g-"); - l_cmd_y->line_width(2.0f); - auto l_cmd_z = plot(ax1, t_trace, command_vz_trace, "b-"); - l_cmd_z->line_width(2.0f); - title(ax1, "speedL target vy/vz vs command vxyz"); - xlabel(ax1, "time [s]"); - ylabel(ax1, "linear cmd [m/s]"); - legend(ax1, {"target vy", "target vz", "command vx", "command vy", "command vz"}); - grid(ax1, true); - - auto ax2 = subplot(3, 1, 1); - hold(ax2, true); - std::vector joint_labels; - joint_labels.reserve(kRightArmJointNames.size()); - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - auto line = plot(ax2, t_trace, qd_cmd_trace[i]); - line->line_width(1.8f); - joint_labels.emplace_back(kRightArmJointNames[i]); - } - title(ax2, "solver qd_cmd"); - xlabel(ax2, "time [s]"); - ylabel(ax2, "joint vel [rad/s]"); - legend(ax2, joint_labels); - grid(ax2, true); - - auto ax3 = subplot(3, 1, 2); - hold(ax3, true); - std::vector qd_norm_trace; - qd_norm_trace.reserve(t_trace.size()); - for (size_t k = 0; k < t_trace.size(); ++k) { - double norm = 0.0; - for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - const double v = qd_cmd_trace[i][k]; - norm += v * v; - } - qd_norm_trace.push_back(std::sqrt(norm)); - } - auto qd_norm_line = plot(ax3, t_trace, qd_norm_trace, "m-"); - qd_norm_line->line_width(2.0f); - title(ax3, "solver qd_cmd norm"); - xlabel(ax3, "time [s]"); - ylabel(ax3, "norm [rad/s]"); - legend(ax3, {"||qd_cmd||"}); - grid(ax3, true); - - show(fig); - } - - EXPECT_GT(ok_steps, 0) << "No successful open-loop speedL steps."; -} TEST(HumanoidRobotTest,IBVSWithRealRobot) { const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); @@ -558,7 +218,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { auto& dmgr = DeviceManager::getInstance(dmgr_cfg); auto robot = dmgr.getDevice("hc01"); - auto camera = dmgr.getDevice("cam2"); + auto camera = dmgr.getDevice("right_hand_cam"); ASSERT_NE(robot, nullptr); ASSERT_NE(camera, nullptr); @@ -595,28 +255,45 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { } } guard{camera, robot}; + const double tag_size_m = 0.12; + const int tracked_tag_id = 0; + const double target_x = 0.0; + const double target_y = 0.0; + const double target_z = 0.40; + const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR; + + auto perception = std::make_shared(camera); + perception->setTagSize(tag_size_m); + cmvr::perception::AprilTagPerception::Options perception_opt; + switch (ibvs_depth_mode) { + case cmvr::IbvsController::DepthMode::MONOCULAR: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; + break; + case cmvr::IbvsController::DepthMode::PREFER_DEPTH: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER; + break; + case cmvr::IbvsController::DepthMode::DEPTH_ONLY: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; + break; + } + cmvr::IbvsController ibvs_controller; ibvs_controller.setMu(0.1); ibvs_controller.setLambda(0.6); - ibvs_controller.setQdotMax(0.6); - ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); - ibvs_controller.setTrackedTagId(0); - ibvs_controller.setTarget(0.0, 0.0, 0.40); - ibvs_controller.setDepthMode(cmvr::IbvsController::DepthMode::MONOCULAR); + ibvs_controller.setTrackedTagId(tracked_tag_id); + ibvs_controller.setTarget(target_x, target_y, target_z); + ibvs_controller.setDepthMode(ibvs_depth_mode); ibvs_controller.setDepthZGain(1.0); - Eigen::Matrix3d I = Eigen::Matrix3d::Identity(); - // ibvs_controller.setAlignCameraToVisp(I); // RealSense optical -> ViSP - // ibvs_controller.setAlignCameraToUrdf(I); // optical -> URDF 的 R_CAM - Eigen::Matrix3d RxPi; - RxPi << 1,0,0, - 0,-1,0, - 0,0,-1; - ibvs_controller.setAlignCameraToUrdf(RxPi); - ibvs_controller.setAlignCameraToVisp(RxPi); + // Eigen::Matrix3d RxPi; + // RxPi << 1,0,0, + // 0,-1,0, + // 0,0,-1; + // ibvs_controller.setAlignCameraToUrdf(RxPi); + // ibvs_controller.setAlignCameraToVisp(RxPi); - // ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0}); + // ibvs_controller.setVelocityLimit6({0.3, 0.3, 0.3, 0.3, 0.3, 0.3}); // ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005}); ibvs_controller.setQdotMax(0.15); ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); @@ -629,6 +306,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { "R_WRIST_R_S", "R_CAM")) << "IbvsController init failed"; + ibvs_controller.setPerception(perception); auto q_map_now = makeRightArmQMap(); robot->getJointQ(q_map_now); @@ -653,23 +331,25 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { const int max_steps = 30000; const int log_every = 1; const int cycle_ms = 10; - const double soft_margin = 0.20; // rad - const double hard_margin = 0.04; // rad - const double qdot_deadband = 0.01; // rad/s - const double qdot_lpf_alpha = 0.75; - const double qdot_acc_limit = 1.5; // rad/s^2 - const double qdot_max_send = 0.25; // rad/s int ok_steps = 0; int fail_steps = 0; - std::vector qdot_lpf(kRightArmJointNames.size(), 0.0); - std::vector qdot_cmd_prev(kRightArmJointNames.size(), 0.0); - auto t_prev = std::chrono::steady_clock::now(); + int perception_fail_steps = 0; + int compute_fail_steps = 0; + auto sendZeroVelocity = [&]() { + robot->speedJ(0); + }; + + for (int i = 0; i < 10; ++i) { + perception->update(perception_opt); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + } for (int step = 0; step < max_steps; ++step) { robot->getJointQ(q_map_now); if (!refreshRightArmQ()) { ++fail_steps; + sendZeroVelocity(); if ((step % log_every) == 0) { std::cout << "[IBVS_REAL] step=" << step << " joint extract failed\n"; } @@ -677,33 +357,49 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { continue; } - const auto t_now = std::chrono::steady_clock::now(); - double dt = std::chrono::duration(t_now - t_prev).count(); - t_prev = t_now; - dt = std::clamp(dt, 0.001, 0.1); + const bool perception_ok = perception->update(perception_opt); + if (!perception_ok) { + ++perception_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL] step=" << step + << " perception failed: " + << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus()) + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } std::vector qdot_next; const bool ok = ibvs_controller.compute(q_now, qdot_next); - - + if (!ok) { + ++compute_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL] step=" << step + << " compute failed: " + << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus()); + if (perception->hasTags()) { + std::cout << " visible_tags="; + for (const auto& tag : perception->tags()) { + std::cout << tag.id << " "; + } + } + std::cout << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } std::vector qd_send; for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { - qdot_lpf[i] = qdot_lpf_alpha * qdot_lpf[i] + (1.0 - qdot_lpf_alpha) * qdot_next[i]; - double v = applySoftJointLimitVelocity(q_now[i], qdot_lpf[i], - kRightArmQMin[i], kRightArmQMax[i], - soft_margin, hard_margin); - if (std::abs(v) < qdot_deadband) { - v = 0.0; - } - const double dv_max = qdot_acc_limit * dt; - v = std::clamp(v, qdot_cmd_prev[i] - dv_max, qdot_cmd_prev[i] + dv_max); - v = std::clamp(v, -qdot_max_send, qdot_max_send); - qdot_cmd_prev[i] = v; - qd_send.push_back({kRightArmJointNames[i],v}); + qd_send.push_back({kRightArmJointNames[i], qdot_next[i]}); } - robot->speedJ(qd_send); + robot->speedJ(qd_send); ++ok_steps; if ((step % log_every) == 0) { @@ -728,9 +424,315 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) { std::cout << "[IBVS_REAL] finished steps=" << max_steps << " ok_steps=" << ok_steps << " fail_steps=" << fail_steps + << " perception_fail_steps=" << perception_fail_steps + << " compute_fail_steps=" << compute_fail_steps << std::endl; robot->speedJ(0); EXPECT_GT(ok_steps, 0) << "No successful IBVS control steps."; } + +TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) { + const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml"); + ASSERT_TRUE(config.hasChild("DeviceManager")) << "DeviceManager node not found"; + auto dmgr_cfg = config.getChild("DeviceManager"); + auto& dmgr = DeviceManager::getInstance(dmgr_cfg); + + auto robot = dmgr.getDevice("hc01"); + auto camera = dmgr.getDevice("right_hand_cam"); + ASSERT_NE(robot, nullptr); + ASSERT_NE(camera, nullptr); + + std::vector init_cmd{}; + init_cmd = { + {"R_SHOULDER_P", -0.2423}, + {"R_SHOULDER_R", 1.2929}, + {"R_SHOULDER_Y", 1.61}, + {"R_ELBOW_R", 1.58}, + {"R_WRIST_P", -2.8792}, + {"R_WRIST_Y", 0.1150}, + {"R_WRIST_R", -0.08}, + }; + robot->moveJ(init_cmd, 1.0, 2.0); + std::this_thread::sleep_for(std::chrono::milliseconds(1000)); + + ASSERT_NO_THROW(camera->start()); + struct RuntimeGuard { + std::shared_ptr camera; + std::shared_ptr robot; + ~RuntimeGuard() { + if (camera) { + try { + camera->stop(); + } catch (...) { + } + } + if (robot) { + try { + robot->eStop(); + } catch (...) { + } + } + } + } guard{camera, robot}; + + const double tag_size_m = 0.12; + const int preferred_tag_id = 0; + const Eigen::Vector3d hover_target_in_camera(0.0, 0.0, 0.40); + const cmvr::IbvsController::DepthMode ibvs_depth_mode = cmvr::IbvsController::DepthMode::MONOCULAR; + + auto perception = std::make_shared(camera); + perception->setTagSize(tag_size_m); + cmvr::perception::AprilTagPerception::Options perception_opt; + switch (ibvs_depth_mode) { + case cmvr::IbvsController::DepthMode::MONOCULAR: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::NONE; + break; + case cmvr::IbvsController::DepthMode::PREFER_DEPTH: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::PREFER; + break; + case cmvr::IbvsController::DepthMode::DEPTH_ONLY: + perception_opt.depth_policy = cmvr::perception::AprilTagPerception::DepthPolicy::REQUIRE; + break; + } + + cmvr::perception::TagRelativeTarget3D tracker(perception); + tracker.setTargetPointMethod(cmvr::perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE); + tracker.setActiveTagSwitchPolicy(4, 1.2); + tracker.setTrackingCandidateScoreWeights(1.0, 1.0, 0.08); + + cmvr::IbvsController ibvs_controller; + ibvs_controller.setMu(0.1); + ibvs_controller.setLambda(0.6); + ibvs_controller.setDepthMode(ibvs_depth_mode); + ibvs_controller.setDepthZGain(1.0); + + ibvs_controller.setQdotMax(0.15); + ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25); + + ASSERT_TRUE(ibvs_controller.init( + "/home/lgv/cmvr/cmvr-es/model/xiaoyan_description/dual_arm.urdf", + "PELVIS_S", + "R_WRIST_R_S", + "R_CAM")) + << "IbvsController init failed"; + ibvs_controller.setPerception(perception); + + auto q_map_now = makeRightArmQMap(); + robot->getJointQ(q_map_now); + std::vector q_now(kRightArmJointNames.size(), 0.0); + auto refreshRightArmQ = [&]() -> bool { + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + const auto it = q_map_now.find(kRightArmJointNames[i]); + if (it == q_map_now.end()) { + return false; + } + q_now[i] = it->second; + } + return true; + }; + + ASSERT_TRUE(refreshRightArmQ()) + << "Failed to extract right-arm 7 joints from robot state"; + ibvs_controller.reset(q_now); + + auto sendZeroVelocity = [&]() { + robot->speedJ(0); + }; + + auto projectPreferredTagCenter = [&](int& u, int& v) -> bool { + const auto* tag = perception->findTag(preferred_tag_id); + if (!tag) return false; + Eigen::Vector2d uv = Eigen::Vector2d::Zero(); + const Eigen::Vector3d p_c_tag = tag->T_c_t.block<3, 1>(0, 3); + if (!cmvr::ImageProcess::projectCameraPointToPixel(perception->intrinsics(), p_c_tag, uv)) { + return false; + } + u = static_cast(std::lround(uv.x())); + v = static_cast(std::lround(uv.y())); + const cv::Mat& color = perception->color(); + return !color.empty() && u >= 0 && v >= 0 && u < color.cols && v < color.rows; + }; + + for (int i = 0; i < 10; ++i) { + perception->update(perception_opt); + std::this_thread::sleep_for(std::chrono::milliseconds(30)); + } + + const int max_steps = 30000; + const int log_every = 1; + const int cycle_ms = 10; + + int ok_steps = 0; + int fail_steps = 0; + int perception_fail_steps = 0; + int tracking_fail_steps = 0; + int compute_fail_steps = 0; + bool target_locked = false; + bool ibvs_target_initialized = false; + int lock_u = -1; + int lock_v = -1; + + for (int step = 0; step < max_steps; ++step) { + robot->getJointQ(q_map_now); + if (!refreshRightArmQ()) { + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step << " joint extract failed\n"; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + + const bool perception_ok = perception->update(perception_opt); + if (!perception_ok) { + ++perception_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " perception failed: " + << cmvr::perception::AprilTagPerception::statusToString(perception->lastStatus()) + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + + bool tracking_ok = false; + if (!target_locked) { + if (projectPreferredTagCenter(lock_u, lock_v)) { + tracking_ok = tracker.startTrackingFromPixel(lock_u, lock_v); + if (tracking_ok) { + target_locked = true; + ibvs_target_initialized = false; + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " lock point success" + << " uv=(" << lock_u << ", " << lock_v << ")" + << " active_tag=" << tracker.activeTagId() + << std::endl; + } + } + } + } else { + tracking_ok = tracker.track(); + } + + if (!tracking_ok) { + ++tracking_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " tracker failed: " + << cmvr::perception::TagRelativeTarget3D::statusToString(tracker.lastStatus()); + if (perception->hasTags()) { + std::cout << " visible_tags="; + for (const auto& tag : perception->tags()) { + std::cout << tag.id << " "; + } + } + std::cout << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + + const int active_tag_id = tracker.activeTagId(); + Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero(); + if (active_tag_id < 0 || !tracker.getAnchorInTag(active_tag_id, p_t_target)) { + ++tracking_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " anchor unavailable, active_tag=" << active_tag_id + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + + ibvs_controller.setTrackedTagId(active_tag_id); + if (!ibvs_target_initialized || tracker.lastSwitched()) { + if (!ibvs_controller.setTargetFromPointInTag(p_t_target, hover_target_in_camera)) { + ++tracking_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " setTargetFromPointInTag failed" + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + ibvs_target_initialized = true; + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " setTargetFromPointInTag ok" + << " active_tag=" << active_tag_id + << " p_t_target=[" << p_t_target.x() << ", " + << p_t_target.y() << ", " << p_t_target.z() << "]" + << std::endl; + } + } + + std::vector qdot_next; + const bool ok = ibvs_controller.compute(q_now, qdot_next); + if (!ok) { + ++compute_fail_steps; + ++fail_steps; + sendZeroVelocity(); + if ((step % log_every) == 0) { + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " compute failed: " + << cmvr::IbvsController::statusToString(ibvs_controller.lastComputeStatus()) + << " active_tag=" << active_tag_id + << std::endl; + } + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + continue; + } + + std::vector qd_send; + for (size_t i = 0; i < kRightArmJointNames.size(); ++i) { + qd_send.push_back({kRightArmJointNames[i], qdot_next[i]}); + } + + robot->speedJ(qd_send); + ++ok_steps; + + if ((step % log_every) == 0) { + const auto& t_co = ibvs_controller.lastTagPositionVisp(); + const auto& v_c = ibvs_controller.lastCameraTwistVisp(); + const auto& p_c_target = tracker.lastTargetInCamera(); + std::cout << "[IBVS_REAL_TRACK] step=" << step + << " active_tag=" << active_tag_id + << " switched=" << static_cast(tracker.lastSwitched()) + << " target_p_c=[" << p_c_target.x() << ", " << p_c_target.y() << ", " << p_c_target.z() << "]" + << " tag=[" << t_co.x() << ", " << t_co.y() << ", " << t_co.z() << "]" + << " v_c=[" << v_c[0] << ", " << v_c[1] << ", " << v_c[2] + << ", " << v_c[3] << ", " << v_c[4] << ", " << v_c[5] << "]" + << " z_source=" << cmvr::IbvsController::depthUsageToString(ibvs_controller.lastDepthUsage()) + << std::endl; + } + + std::this_thread::sleep_for(std::chrono::milliseconds(cycle_ms)); + } + + std::cout << "[IBVS_REAL_TRACK] finished steps=" << max_steps + << " ok_steps=" << ok_steps + << " fail_steps=" << fail_steps + << " perception_fail_steps=" << perception_fail_steps + << " tracking_fail_steps=" << tracking_fail_steps + << " compute_fail_steps=" << compute_fail_steps + << std::endl; + + robot->speedJ(0); + + EXPECT_GT(ok_steps, 0) << "No successful tracked-point IBVS control steps."; +} diff --git a/cmvr-es/service/CMakeLists.txt b/cmvr-es/service/CMakeLists.txt index 27c95800..4ef50063 100644 --- a/cmvr-es/service/CMakeLists.txt +++ b/cmvr-es/service/CMakeLists.txt @@ -18,6 +18,7 @@ target_link_libraries(service PRIVATE osqp cmvr_es::device_manager cmvr_es::controller + cmvr_es::applications protobuf::libprotobuf ) @@ -81,4 +82,3 @@ target_link_libraries(grpc_hlc_client_test fcl cmvr_es::device_manager ) - diff --git a/cmvr-es/service/grpc/include/grpc_hlc_service.h b/cmvr-es/service/grpc/include/grpc_hlc_service.h index 01193c8c..14675e8f 100644 --- a/cmvr-es/service/grpc/include/grpc_hlc_service.h +++ b/cmvr-es/service/grpc/include/grpc_hlc_service.h @@ -3,7 +3,10 @@ // #pragma once -#include "device_manager/include/device_manager.h" +#include +#include + +#include "applications/include/touch_screen_app.h" #include "cmvr/api/hlc_service.grpc.pb.h" namespace cmvr { @@ -15,7 +18,11 @@ namespace cmvr { grpc::Status touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) override; private: - device::DeviceManager& dmgr_; + bool ensureTouchAppInitialized(std::string& error_message); + + std::mutex touch_mutex_; + app::TouchScreenApp touch_app_; + bool touch_app_initialized_{false}; }; diff --git a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp index 25df06ec..9fc65ef0 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_client_test.cpp @@ -26,8 +26,8 @@ TEST(GrpcHlcClientTest, MyTest) { *request.mutable_header()->mutable_timestamp() = google::protobuf::util::TimeUtil::GetCurrentTime(); - request.set_u(12); - request.set_v(13); + request.set_u(600); + request.set_v(360); request.set_max_force(1300); diff --git a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp index ebaf5b3e..bafd84d9 100644 --- a/cmvr-es/service/grpc/src/grpc_hlc_service.cpp +++ b/cmvr-es/service/grpc/src/grpc_hlc_service.cpp @@ -4,37 +4,137 @@ #include "../include/grpc_hlc_service.h" + +#include +#include +#include + +#include #include -#include "devices/robot/humanoid_robot/include/humanoid_robot.h" -#include "controller/include/touch_controller.h" +#include "applications/include/touch_screen_app.h" using namespace cmvr::service; -using namespace cmvr::device; using namespace cmvr::api; using google::protobuf::util::TimeUtil; -gRPCHlcServiceImpl::gRPCHlcServiceImpl():dmgr_(DeviceManager::getInstance()){} +namespace { -grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { - - grpc::Status ret = grpc::Status::OK; - - // try { - // std::shared_ptr robot = nullptr; - // auto cam = dmgr_.getDevice("cam4"); - // auto hand = dmgr_.getDevice("hand1"); - // ctrl::TouchController touch_controller(robot,hand,cam);; - // touch_controller.touch(request->u(),request->v(),request->max_force()); - // response->mutable_header()->set_success(true); - // response->mutable_header()->set_error_message(""); - // }catch (const std::exception& e) { - // response->mutable_header()->set_success(false); - // response->mutable_header()->set_error_message(e.what()); - // ret = grpc::Status(grpc::StatusCode::INTERNAL, e.what()); - // } - // *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); - return ret; +std::string buildTouchFailureMessage(const cmvr::app::TouchScreenApp& app, + const std::string& prefix) { + return prefix + ", phase=" + + cmvr::app::TouchScreenApp::phaseToString(app.phase()) + + ", status=" + + cmvr::app::TouchScreenApp::statusToString(app.lastStatus()); } +void fillTouchResponse(Touch_Response* response, + const bool success, + const std::string& error_message) { + response->mutable_header()->set_success(success); + response->mutable_header()->set_error_message(error_message); + *response->mutable_header()->mutable_timestamp() = TimeUtil::GetCurrentTime(); +} +} // namespace + +gRPCHlcServiceImpl::gRPCHlcServiceImpl() = default; + +bool gRPCHlcServiceImpl::ensureTouchAppInitialized(std::string& error_message) { + if (touch_app_initialized_) { + return true; + } + + LOG(INFO) << "[gRPCHlcServiceImpl] init TouchScreenApp from config"; + if (!touch_app_.init()) { + error_message = buildTouchFailureMessage(touch_app_, "TouchScreenApp init failed"); + touch_app_initialized_ = false; + return false; + } + + touch_app_initialized_ = true; + return true; +} + +grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr::api::Touch_Request *request, cmvr::api::Touch_Response *response) { + std::lock_guard lock(touch_mutex_); + + try { + LOG(INFO) << "[gRPCHlcServiceImpl] touch: request.device_id=" + << request->header().device_id() + << " (ignored, using TouchScreenApp config)" + << ", u=" << request->u() + << ", v=" << request->v() + << ", max_force ignored"; + + std::string init_error; + if (!ensureTouchAppInitialized(init_error)) { + fillTouchResponse(response, false, init_error); + return grpc::Status(grpc::StatusCode::INTERNAL, init_error); + } + + if (!touch_app_.startFromPixel(request->u(), request->v())) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "TouchScreenApp startFromPixel failed")); + } + + bool align_reached = false; + bool touch_triggered = false; + auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE; + while (touch_app_.isBusy()) { + if (context != nullptr && context->IsCancelled()) { + touch_app_.stop(); + const std::string error = "touch request cancelled"; + fillTouchResponse(response, false, error); + return grpc::Status(grpc::StatusCode::CANCELLED, error); + } + + if (!touch_app_.step()) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "touch flow failed")); + } + + if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::ALIGN_REACHED) { + align_reached = true; + } + if (touch_app_.lastStatus() == cmvr::app::TouchScreenApp::Status::TOUCH_TRIGGERED) { + touch_triggered = true; + } + + if (touch_app_.lastStatus() != last_logged_status) { + LOG(INFO) << "[gRPCHlcServiceImpl] touch status=" + << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus()) + << ", phase=" + << cmvr::app::TouchScreenApp::phaseToString(touch_app_.phase()) + << ", active_tag=" << touch_app_.lastActiveTagId() + << ", pressure_sum=" << touch_app_.lastTouchPressureSum() + << ", pressure_peak=" << touch_app_.lastTouchPressurePeak(); + last_logged_status = touch_app_.lastStatus(); + } + + std::this_thread::sleep_for(std::chrono::milliseconds(1)); + } + + if (!align_reached) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "touch exited without ALIGN_REACHED")); + } + if (!touch_triggered) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "touch exited without TOUCH_TRIGGERED")); + } + if (!touch_app_.isFinished()) { + throw std::runtime_error( + buildTouchFailureMessage(touch_app_, "touch did not finish successfully")); + } + + LOG(INFO) << "[gRPCHlcServiceImpl] touch done" + << ", final_status=" + << cmvr::app::TouchScreenApp::statusToString(touch_app_.lastStatus()); + fillTouchResponse(response, true, ""); + return grpc::Status::OK; + } catch (const std::exception& e) { + fillTouchResponse(response, false, e.what()); + return grpc::Status(grpc::StatusCode::INTERNAL, e.what()); + } +} diff --git a/data/plot1.png b/data/plot1.png index 29ea5ccd..11a4ca21 100644 Binary files a/data/plot1.png and b/data/plot1.png differ diff --git a/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto new file mode 100644 index 00000000..8b8bf178 --- /dev/null +++ b/protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto @@ -0,0 +1,122 @@ +syntax = "proto3"; + +package cmvr.config; + +message TouchScreenVec3 { + optional double x = 1; + optional double y = 2; + optional double z = 3; +} + +message TouchScreenTwist6 { + optional double vx = 1; + optional double vy = 2; + optional double vz = 3; + optional double wx = 4; + optional double wy = 5; + optional double wz = 6; +} + +message TouchScreenMatrix3d { + optional double m00 = 1; + optional double m01 = 2; + optional double m02 = 3; + optional double m10 = 4; + optional double m11 = 5; + optional double m12 = 6; + optional double m20 = 7; + optional double m21 = 8; + optional double m22 = 9; +} + +message TouchScreenInitJointPoint { + optional string joint_name = 1; + optional double rad = 2; +} + +enum TouchScreenDepthPolicy { + TOUCH_SCREEN_DEPTH_POLICY_NONE = 0; + TOUCH_SCREEN_DEPTH_POLICY_PREFER = 1; + TOUCH_SCREEN_DEPTH_POLICY_REQUIRE = 2; +} + +enum TouchScreenTargetPointMethod { + TOUCH_SCREEN_TARGET_POINT_METHOD_TAG_PLANE = 0; + TOUCH_SCREEN_TARGET_POINT_METHOD_DEPTH_IMAGE = 1; +} + +enum TouchScreenFingerType { + TOUCH_SCREEN_FINGER_TYPE_PINKY = 0; + TOUCH_SCREEN_FINGER_TYPE_RING = 1; + TOUCH_SCREEN_FINGER_TYPE_MIDDLE = 2; + TOUCH_SCREEN_FINGER_TYPE_INDEX = 3; + TOUCH_SCREEN_FINGER_TYPE_THUMB = 4; +} + +enum TouchScreenTactileRegion { + TOUCH_SCREEN_TACTILE_REGION_TIP = 0; + TOUCH_SCREEN_TACTILE_REGION_FINGER = 1; + TOUCH_SCREEN_TACTILE_REGION_PAD = 2; + TOUCH_SCREEN_TACTILE_REGION_TIP_AND_FINGER = 3; + TOUCH_SCREEN_TACTILE_REGION_THUMB_MIDDLE = 4; +} + +message TouchScreenAppConfig { + optional string robot_id = 40; + optional string dexhand_id = 41; + optional string camera_id = 42; + optional bool move_to_init_position = 43; + repeated TouchScreenInitJointPoint init_joint_positions = 44; + optional double init_movej_vel = 45; + optional double init_movej_acc = 46; + optional bool move_to_init_position_before_start = 47; + + optional string urdf_path = 1; + optional string base_link = 2; + optional string flange_link = 3; + optional string camera_link = 4; + + optional double tag_size_m = 5; + optional TouchScreenDepthPolicy depth_policy = 6; + optional TouchScreenTargetPointMethod target_point_method = 7; + + TouchScreenVec3 hover_target_in_camera = 8; + optional double target_rx = 9; + optional double target_ry = 10; + optional double target_rz = 11; + + optional double ibvs_lambda = 12; + optional double ibvs_mu = 13; + optional double ibvs_qdot_max = 14; + TouchScreenTwist6 ibvs_vmax6 = 15; + optional bool enable_joint_limit_avoidance = 16; + optional double joint_limit_avoidance_gain = 17; + optional double joint_limit_avoidance_margin_ratio = 18; + optional double joint_limit_avoidance_max_push = 19; + + TouchScreenMatrix3d r_camera_to_visp = 20; + TouchScreenMatrix3d r_camera_to_urdf = 21; + + repeated string control_joint_names = 22; + + optional double align_xy_threshold_m = 23; + optional double align_z_threshold_m = 24; + optional double align_rot_threshold_rad = 25; + optional int32 align_stable_frames = 26; + optional double align_timeout_s = 27; + optional bool pause_after_align_reached = 28; + + TouchScreenTwist6 touch_twist_base = 29; + optional double touch_acceleration = 30; + optional double touch_forward_duration_s = 31; + + optional double dwell_time_s = 32; + TouchScreenTwist6 retract_twist_base = 33; + optional double retract_acceleration = 34; + optional double retract_duration_s = 35; + + optional TouchScreenFingerType tactile_finger = 36; + optional TouchScreenTactileRegion tactile_region = 37; + optional double tactile_pressure_sum_threshold = 38; + optional double tactile_pressure_peak_threshold = 39; +}