feat: add touch app
This commit is contained in:
parent
bf385669fc
commit
e82a90237d
6
.vscode/extensions.json
vendored
Normal file
6
.vscode/extensions.json
vendored
Normal file
@ -0,0 +1,6 @@
|
||||
{
|
||||
"recommendations": [
|
||||
"ms-vscode.cpptools",
|
||||
"ms-vscode.cmake-tools"
|
||||
]
|
||||
}
|
||||
32
.vscode/launch.json
vendored
Normal file
32
.vscode/launch.json
vendored
Normal file
@ -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"
|
||||
}
|
||||
12
.vscode/settings.json
vendored
Normal file
12
.vscode/settings.json
vendored
Normal file
@ -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"
|
||||
}
|
||||
25
.vscode/tasks.json
vendored
Normal file
25
.vscode/tasks.json
vendored
Normal file
@ -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"
|
||||
}
|
||||
@ -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)
|
||||
|
||||
@ -11,6 +11,7 @@
|
||||
|
||||
#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"
|
||||
@ -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<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.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<double, 6> 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<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.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<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 方向的允许误差，单位米。
|
||||
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<double, 6, 1> touch_twist_base{
|
||||
(Eigen::Matrix<double, 6, 1>() << 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<double, 6, 1>() << 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<double, 6, 1> retract_twist_base{
|
||||
(Eigen::Matrix<double, 6, 1>() << 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<double, 6, 1>() << 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<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 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<double>& 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);
|
||||
|
||||
@ -3,8 +3,13 @@
|
||||
#include <algorithm>
|
||||
#include <cmath>
|
||||
#include <exception>
|
||||
#include <limits>
|
||||
#include <unordered_map>
|
||||
|
||||
#include "common/utils/config_helper/include/config_helper.h"
|
||||
#include "device_manager/include/device_manager.h"
|
||||
#include <visp3/core/vpRotationMatrix.h>
|
||||
|
||||
namespace cmvr::app {
|
||||
|
||||
namespace {
|
||||
@ -29,15 +34,250 @@ void accumulateMatrixStats(const std::vector<std::vector<device::TactilePoint>>&
|
||||
}
|
||||
}
|
||||
|
||||
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<double>::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<double, 6, 1>& 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<device::AbstractRobot>(config.robot_id());
|
||||
auto dexhand = dm.getDevice<device::AbstractDexHand>(config.dexhand_id());
|
||||
auto camera = dm.getDevice<device::AbstractCamera>(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<device::AbstractRobot>& robot,
|
||||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||||
const std::shared_ptr<device::AbstractCamera>& 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<device::AbstractRobot>& robot,
|
||||
const std::shared_ptr<device::AbstractDexHand>& dexhand,
|
||||
const std::shared_ptr<device::AbstractCamera>& 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<device::AbstractRobot>& 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<device::AbstractRobot>& 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<size_t>(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<double, 6, 1> 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<size_t>(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<double>(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;
|
||||
|
||||
|
||||
@ -5,21 +5,13 @@
|
||||
#include <thread>
|
||||
|
||||
#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<cmvr::device::AbstractRobot>(kRobotId);
|
||||
// auto dexhand = dm.getDevice<cmvr::device::AbstractDexHand>(kDexhandId);
|
||||
auto camera = dm.getDevice<cmvr::device::AbstractCamera>(kCameraId);
|
||||
std::shared_ptr<cmvr::device::AbstractDexHand> dexhand = nullptr;
|
||||
|
||||
camera->start();
|
||||
|
||||
std::vector<cmvr::device::JointPoint> 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);
|
||||
}
|
||||
|
||||
@ -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
|
||||
}
|
||||
@ -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
|
||||
@ -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 <class T>
|
||||
|
||||
@ -5,3 +5,4 @@ DECLARE_string(camera_config_file);
|
||||
DECLARE_string(dexhand_config_file);
|
||||
DECLARE_string(microphone_config_file);
|
||||
DECLARE_string(speaker_config_file);
|
||||
DECLARE_string(touch_screen_app_config_file);
|
||||
|
||||
@ -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");
|
||||
|
||||
@ -75,6 +75,15 @@ namespace cmvr::device{
|
||||
THUMB // 大拇指
|
||||
};
|
||||
|
||||
// 触觉区域枚举（设备层）。
|
||||
enum class TactileRegion {
|
||||
TIP = 0,
|
||||
FINGER,
|
||||
PAD,
|
||||
TIP_AND_FINGER,
|
||||
THUMB_MIDDLE
|
||||
};
|
||||
|
||||
// 整只手的触觉传感器数据
|
||||
struct HandTactileSensors {
|
||||
// 五指的触觉数据（每根手指包含指端、指尖、指腹）
|
||||
@ -219,6 +228,11 @@ namespace cmvr::device{
|
||||
virtual void execPresetAct(int action_id) {}
|
||||
virtual void setForce(const std::vector<int>& 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_;
|
||||
|
||||
@ -61,6 +61,7 @@ namespace cmvr::device {
|
||||
void execPresetAct(int action_id) override;//这个无用
|
||||
void setForce(const std::vector<int>& finger_joint_force) override;
|
||||
HandTactileSensors& getSensorData() override;
|
||||
HandTactileSensors& getSensorData(FingerType finger_type, TactileRegion tactile_region) override;
|
||||
private:
|
||||
void updateState();
|
||||
void updateSensorData();
|
||||
|
||||
@ -35,6 +35,21 @@ const int TOUCH_SENSOR_END_ADDR_PALM = 5123;
|
||||
// Modbus 每次最多读取寄存器的数量
|
||||
const int MAX_REGISTERS_PER_READ = 125;
|
||||
|
||||
namespace {
|
||||
|
||||
std::pair<int, int> 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_;
|
||||
}
|
||||
|
||||
@ -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() ;
|
||||
|
||||
|
||||
|
||||
|
||||
@ -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<double> &xd, double acceleration = 0.25, double time = 0.0) { throw std::runtime_error("Not implemented"); }
|
||||
virtual bool speedL(const std::vector<double> &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"); }
|
||||
|
||||
@ -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<double> &xd, double acceleration = 0.25, double time = 0.0) override;
|
||||
bool speedL(const std::vector<double> &xd,
|
||||
double acceleration = 0.25,
|
||||
double time = 0.0,
|
||||
cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override;
|
||||
void stopSpeedL() override;
|
||||
Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase();
|
||||
|
||||
@ -215,6 +218,7 @@ namespace cmvr::device{
|
||||
Eigen::Matrix<double, 6, 1> speedl_target_twist_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
Eigen::Matrix<double, 6, 1> speedl_last_command_twist_base_{Eigen::Matrix<double, 6, 1>::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};
|
||||
|
||||
|
||||
@ -87,7 +87,10 @@ void HumanoidRobot<DOF>::init() {
|
||||
|
||||
|
||||
template <int DOF>
|
||||
bool HumanoidRobot<DOF>::speedL(const std::vector<double>& xd, double acceleration, double time)
|
||||
bool HumanoidRobot<DOF>::speedL(const std::vector<double>& 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<DOF>::speedL(const std::vector<double>& xd, double accelerati
|
||||
std::lock_guard<std::mutex> 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<DOF>::speedL(const std::vector<double>& xd, double accelerati
|
||||
std::lock_guard<std::mutex> 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<DOF>::stopSpeedL()
|
||||
{
|
||||
std::lock_guard<std::mutex> 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<DOF>::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<DOF>::speedLWorkerLoop_() {
|
||||
while (true) {
|
||||
Eigen::Matrix<double, 6, 1> target_twist = Eigen::Matrix<double, 6, 1>::Zero();
|
||||
double acceleration = 0.25;
|
||||
cmvr::CartesianFrame target_frame = cmvr::CartesianFrame::Base;
|
||||
{
|
||||
std::unique_lock<std::mutex> lock(speedl_mutex_);
|
||||
speedl_cv_.wait(lock, [&]() {
|
||||
@ -283,6 +291,7 @@ void HumanoidRobot<DOF>::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<DOF>::speedLWorkerLoop_() {
|
||||
}
|
||||
target_twist = speedl_target_twist_;
|
||||
acceleration = speedl_target_acceleration_;
|
||||
target_frame = speedl_target_frame_;
|
||||
}
|
||||
|
||||
if (!updateSpeedLAccelerationConfig_(acceleration)) {
|
||||
@ -323,7 +333,7 @@ void HumanoidRobot<DOF>::speedLWorkerLoop_() {
|
||||
dt,
|
||||
q_now,
|
||||
qd_cmd,
|
||||
cmvr::CartesianFrame::Base,
|
||||
target_frame,
|
||||
true)) {
|
||||
LOG(ERROR) << "speedL worker: speedLStep() failed";
|
||||
send_zero();
|
||||
|
||||
@ -22,8 +22,11 @@
|
||||
#include "../../../../device_manager/include/device_manager.h"
|
||||
#include <libgen.h>
|
||||
|
||||
#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<std::string, double> makeRightArmQMap() {
|
||||
return q_map;
|
||||
}
|
||||
|
||||
constexpr std::array<double, 7> 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<double, 7> 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<AbstractRobot>("hc01");
|
||||
robot->torqueOff();
|
||||
std::vector<cmvr::device::JointState> 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<AbstractRobot>("hc01");
|
||||
robot->calibrateZeroQ("R_WRIST_R");
|
||||
|
||||
// std::vector<JointPoint> 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<AbstractRobot>("hc01");
|
||||
|
||||
auto q_map_now = makeRightArmQMap();
|
||||
std::vector<double> 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<JointPoint> 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<AbstractRobot>("hc01");
|
||||
std::vector<JointVelocityCommand> 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<JointPoint> 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<double> twist_pos = {0.08, 0, 0, 0.00, 0, 0};
|
||||
const std::vector<double> twist_neg = {-0.08, 0, 0, 0.00, 0, 0};
|
||||
const std::vector<double> twist_pos = {0.00, -0.08, 0, 0.00, 0, 0};
|
||||
const std::vector<double> 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<int>(std::ceil(duration / sample_dt));
|
||||
for (int i = 0; i < steps; ++i) {
|
||||
const Eigen::Matrix<double, 6, 1> 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<double>(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<double> 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<int>(std::ceil(total_time / dt));
|
||||
const int settle_steps = static_cast<int>(std::ceil(settle_time / dt));
|
||||
const int total_steps = active_steps + settle_steps;
|
||||
|
||||
int ok_steps = 0;
|
||||
std::vector<double> t_trace;
|
||||
std::vector<double> target_vy_trace;
|
||||
std::vector<double> target_vz_trace;
|
||||
std::vector<double> command_vx_trace;
|
||||
std::vector<double> command_vy_trace;
|
||||
std::vector<double> command_vz_trace;
|
||||
std::vector<std::vector<double>> 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<double>(step) * dt;
|
||||
|
||||
Eigen::Matrix<double, 6, 1> target_twist = Eigen::Matrix<double, 6, 1>::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<double> 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<std::string> 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<double> 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<AbstractRobot>("hc01");
|
||||
auto camera = dmgr.getDevice<AbstractCamera>("cam2");
|
||||
auto camera = dmgr.getDevice<AbstractCamera>("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<cmvr::perception::AprilTagPerception>(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<double> qdot_lpf(kRightArmJointNames.size(), 0.0);
|
||||
std::vector<double> 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<double>(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<double> 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<JointVelocityCommand> 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<AbstractRobot>("hc01");
|
||||
auto camera = dmgr.getDevice<AbstractCamera>("right_hand_cam");
|
||||
ASSERT_NE(robot, nullptr);
|
||||
ASSERT_NE(camera, nullptr);
|
||||
|
||||
std::vector<JointPoint> 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<AbstractCamera> camera;
|
||||
std::shared_ptr<AbstractRobot> 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<cmvr::perception::AprilTagPerception>(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<double> 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<int>(std::lround(uv.x()));
|
||||
v = static_cast<int>(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<double> 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<JointVelocityCommand> 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<int>(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.";
|
||||
}
|
||||
|
||||
@ -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
|
||||
)
|
||||
|
||||
|
||||
@ -3,7 +3,10 @@
|
||||
//
|
||||
#pragma once
|
||||
|
||||
#include "device_manager/include/device_manager.h"
|
||||
#include <mutex>
|
||||
#include <string>
|
||||
|
||||
#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};
|
||||
};
|
||||
|
||||
|
||||
|
||||
@ -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);
|
||||
|
||||
|
||||
|
||||
@ -4,37 +4,137 @@
|
||||
|
||||
|
||||
#include "../include/grpc_hlc_service.h"
|
||||
|
||||
#include <chrono>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
|
||||
#include <glog/logging.h>
|
||||
#include <google/protobuf/util/time_util.h>
|
||||
#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<AbstractRobot> robot = nullptr;
|
||||
// auto cam = dmgr_.getDevice<AbstractCamera>("cam4");
|
||||
// auto hand = dmgr_.getDevice<AbstractDexHand>("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<std::mutex> 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());
|
||||
}
|
||||
}
|
||||
|
||||
BIN
data/plot1.png
BIN
data/plot1.png
Binary file not shown.
|
Before Width: | Height: | Size: 56 KiB After Width: | Height: | Size: 61 KiB |
@ -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;
|
||||
}
|
||||
Loading…
Reference in New Issue
Block a user