324 lines
15 KiB
C++
324 lines
15 KiB
C++
#pragma once
|
||
|
||
#ifndef CMVR_ES_TOUCH_SCREEN_APP_H
|
||
#define CMVR_ES_TOUCH_SCREEN_APP_H
|
||
|
||
#include <array>
|
||
#include <chrono>
|
||
#include <memory>
|
||
#include <string>
|
||
#include <vector>
|
||
|
||
#include <Eigen/Dense>
|
||
|
||
#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"
|
||
#include "devices/robot/abstract_robot.h"
|
||
#include "perception/include/apriltag_perception.h"
|
||
#include "perception/include/tag_relative_target_3d.h"
|
||
|
||
namespace cmvr::app {
|
||
|
||
class TouchScreenApp {
|
||
public:
|
||
enum class Phase {
|
||
IDLE = 0, // 空闲，尚未开始任务。
|
||
ALIGNING, // 视觉对准阶段：持续 IBVS 对齐目标点。
|
||
ALIGN_REACHED, // 视觉对准已达到阈值，等待进入下一阶段。
|
||
TOUCHING, // 前进触控阶段：沿设定方向向屏幕推进。
|
||
DWELLING, // 已检测到接触，保持当前位置短暂停留。
|
||
RETRACTING, // 回退阶段：沿设定回退方向离开屏幕。
|
||
DONE, // 整个流程成功完成。
|
||
FAILED // 流程失败并已停止。
|
||
};
|
||
|
||
enum class Status {
|
||
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 {
|
||
TIP = 0,
|
||
FINGER,
|
||
PAD,
|
||
TIP_AND_FINGER,
|
||
THUMB_MIDDLE
|
||
};
|
||
|
||
enum class AlignMode {
|
||
POSE_AND_POSITION = 0, // 使用配置里的固定 rx/ry/rz 与位置一起对齐。
|
||
RX_RY_AND_POSITION, // 使用配置里的 rx/ry，保留锁定时看到的 tag 平面内 yaw，再与位置一起对齐。
|
||
POSITION_ONLY // 保留锁定时看到的完整 tag 姿态，只按位置对齐。
|
||
};
|
||
|
||
struct Options {
|
||
// 是否在触控流程开始前先回到指定初始关节位姿。
|
||
bool move_to_init_position_before_start{false};
|
||
// 是否在触控流程结束（DONE/FAILED）后回到指定初始关节位姿。
|
||
bool move_to_init_position{false};
|
||
// 初始关节位姿目标，在前置回位或结束后回位开启时使用。
|
||
std::vector<device::JointPoint> 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.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};
|
||
// 对齐模式：1) 固定姿态+位置；2) 固定 rx/ry + 锁定时 yaw + 位置；3) 仅位置。
|
||
AlignMode align_mode{AlignMode::POSE_AND_POSITION};
|
||
|
||
// IBVS 参数。
|
||
// 视觉伺服增益 lambda。
|
||
double ibvs_lambda{0.6};
|
||
// DLS IK 阻尼系数 mu。
|
||
double ibvs_mu{0.1};
|
||
// 单关节最大速度，单位 rad/s。
|
||
double ibvs_qdot_max{0.15};
|
||
// 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。
|
||
std::array<double, 6> 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.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<std::string> 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, z, rx, ry, rz]`。
|
||
// 其中位置误差单位米，旋转误差单位弧度；旋转部分使用目标姿态误差 rotvec 的三个分量分别比较。
|
||
std::array<double, 6> align_error_threshold6{{0.003, 0.003, 0.010,
|
||
0.08726646259971647,
|
||
0.08726646259971647,
|
||
0.08726646259971647}};
|
||
// 连续多少帧都满足阈值，才认为对准完成。
|
||
int align_stable_frames{5};
|
||
// 对准阶段超时时间，单位秒。
|
||
double align_timeout_s{10.0};
|
||
// 为 true 时对准完成后暂停，不自动进入触控阶段。
|
||
bool pause_after_align_reached{false};
|
||
|
||
// 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。
|
||
// 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。
|
||
// 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。
|
||
bool touch_use_speedl{true};
|
||
// 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。
|
||
// 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。
|
||
// 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。
|
||
Eigen::Matrix<double, 6, 1> touch_twist_base{
|
||
(Eigen::Matrix<double, 6, 1>() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()};
|
||
// 触控阶段 speedL 的加速度参数。
|
||
double touch_speedl_acceleration{3.0};
|
||
// 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。
|
||
double touch_forward_l{0.08};
|
||
// 触控阶段使用 moveL 时的末端速度，单位 m/s。
|
||
double touch_movel_speed{0.25};
|
||
// 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。
|
||
double touch_movel_acceleration{1.2};
|
||
// 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。
|
||
double touch_movel_jerk{5.0};
|
||
// 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。
|
||
std::vector<double> touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5};
|
||
// 前进触控阶段的最大累计位移，单位米。
|
||
// 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。
|
||
// 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。
|
||
// 小于等于 0 时，表示不启用这条限制。
|
||
double touch_speedl_forward_l{0.08};
|
||
|
||
// 接触后停留与回退。当前按末端 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 方向向后离屏。
|
||
// 回退阶段统一使用 speedL。
|
||
Eigen::Matrix<double, 6, 1> retract_twist_base{
|
||
(Eigen::Matrix<double, 6, 1>() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()};
|
||
// 回退阶段 speedL 的加速度参数。
|
||
double retract_acceleration{3.0};
|
||
// 回退阶段 speedL 持续时间，单位秒。
|
||
double retract_duration_s{0.8};
|
||
|
||
// 指尖触觉判据。
|
||
// 使用哪根手指的触觉阵列判断是否接触。
|
||
device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
|
||
// 使用该手指的哪个触觉区域。
|
||
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<device::AbstractRobot>& robot,
|
||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||
const std::shared_ptr<device::AbstractCamera>& camera);
|
||
bool init(const std::shared_ptr<device::AbstractRobot>& robot,
|
||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||
const std::shared_ptr<device::AbstractCamera>& camera,
|
||
const cmvr::config::TouchScreenAppConfig& config);
|
||
|
||
bool setOptionsFromConfig();
|
||
bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config);
|
||
bool setTouchSpeedlForwardL(double forward_l);
|
||
|
||
bool startFromPixel(int u, int v);
|
||
bool step();
|
||
void stop();
|
||
|
||
Phase phase() const { return phase_; }
|
||
Status lastStatus() const { return last_status_; }
|
||
static const char* phaseToString(Phase phase);
|
||
static const char* statusToString(Status status);
|
||
|
||
bool isBusy() const { return phase_ == Phase::ALIGNING || phase_ == Phase::ALIGN_REACHED ||
|
||
phase_ == Phase::TOUCHING || phase_ == Phase::DWELLING ||
|
||
phase_ == Phase::RETRACTING; }
|
||
bool isFinished() const { return phase_ == Phase::DONE; }
|
||
bool isFailed() const { return phase_ == Phase::FAILED; }
|
||
|
||
int targetU() const { return target_u_; }
|
||
int targetV() const { return target_v_; }
|
||
double lastTouchPressureSum() const { return last_touch_pressure_sum_; }
|
||
double lastTouchPressurePeak() const { return last_touch_pressure_peak_; }
|
||
int lastActiveTagId() const { return last_active_tag_id_; }
|
||
const Eigen::Vector3d& lastAlignErrorCamera() const { return last_align_error_camera_; }
|
||
|
||
const std::shared_ptr<perception::AprilTagPerception>& perception() const { return perception_; }
|
||
const perception::TagRelativeTarget3D& tracker() const { return tracker_; }
|
||
const IbvsController& ibvs() const { return ibvs_; }
|
||
|
||
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();
|
||
bool stepTouching();
|
||
bool stepDwelling();
|
||
bool stepRetracting();
|
||
|
||
bool readControlledJointPositions(std::vector<double>& q_out) const;
|
||
bool sendJointVelocity(const std::vector<double>& qdot) const;
|
||
bool sendZeroJointVelocity() const;
|
||
bool holdCurrentControlledPosition() const;
|
||
bool moveToInitPositionIfEnabled() const;
|
||
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
|
||
void logTouchingSpeedLState() const;
|
||
|
||
bool startTouchPhase();
|
||
bool handleTouchTriggered(bool stop_forward_motion);
|
||
bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
|
||
void enterFailed(Status status);
|
||
|
||
bool updateTouchPressure();
|
||
|
||
private:
|
||
std::shared_ptr<device::AbstractRobot> robot_{nullptr};
|
||
std::shared_ptr<device::AbstractDexHand> dexhand_{nullptr};
|
||
std::shared_ptr<device::AbstractCamera> camera_{nullptr};
|
||
|
||
std::shared_ptr<perception::AprilTagPerception> perception_{nullptr};
|
||
perception::TagRelativeTarget3D tracker_;
|
||
IbvsController ibvs_;
|
||
Options options_{};
|
||
|
||
Phase phase_{Phase::IDLE};
|
||
Phase phase_after_retract_{Phase::DONE};
|
||
Status last_status_{Status::NOT_INITIALIZED};
|
||
|
||
bool initialized_{false};
|
||
bool target_locked_{false};
|
||
bool ibvs_target_initialized_{false};
|
||
bool touch_command_started_{false};
|
||
bool retract_command_started_{false};
|
||
|
||
int target_u_{-1};
|
||
int target_v_{-1};
|
||
int align_stable_count_{0};
|
||
int last_active_tag_id_{-1};
|
||
|
||
double last_touch_pressure_sum_{0.0};
|
||
double last_touch_pressure_peak_{0.0};
|
||
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
|
||
bool locked_target_rotation_valid_{false};
|
||
Eigen::Matrix3d locked_target_rotation_{Eigen::Matrix3d::Identity()};
|
||
bool touch_start_position_valid_{false};
|
||
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
|
||
|
||
Clock::time_point phase_start_time_{};
|
||
Status final_status_after_retract_{Status::DONE};
|
||
};
|
||
|
||
using touch_screen_app = TouchScreenApp;
|
||
|
||
} // namespace cmvr::app
|
||
|
||
#endif // CMVR_ES_TOUCH_SCREEN_APP_H
|