SmartBench/cmvr-es
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

fix: add touch by distance

Browse Source
This commit is contained in:
lgv 2026-03-26 15:35:00 +08:00
parent 96aca18525
commit baf883bc27
24 changed files with 2395 additions and 1653 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

37
cmvr-es/applications/include/touch_screen_app.h
View File

@ -150,18 +150,32 @@ public:
// 为 true 时对准完成后暂停，不自动进入触控阶段。
bool pause_after_align_reached{false};
// 触控阶段：speedL 目标 twist。当前按末端 Tool 坐标系解释，字段名保留兼容。
// 触控阶段前进方向。当前按末端 Tool 坐标系解释，字段名保留兼容。
// 为 true 时，TOUCHING 阶段使用 speedL；为 false 时使用 moveL。
// 当前 moveL 路径为同步前进，执行完成后直接结束流程，并按配置决定是否回初始位姿。
bool touch_use_speedl{true};
// 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`，单位 m/s 和 rad/s。
// 当前机器人上 `[0, -0.08, 0, 0, 0, 0]` 表示沿 Tool -Y 方向向前触屏。
// 当 TOUCHING 使用 moveL 时，会取其线速度方向并按 touch_forward_l 构造位移目标。
Eigen::Matrix<double, 6, 1> touch_twist_base{
(Eigen::Matrix<double, 6, 1>() << 0.0, -0.08, 0.0, 0.0, 0.0, 0.0).finished()};
// 触控阶段 speedL 的加速度参数。
double touch_acceleration{3.0};
// 前进触控阶段的最大持续时间，单位秒。
// 该时间由 TouchScreenApp 状态机自行计时，不直接传给 robot->speedL(time)。
// 大于 0 时，达到该时间后无论压力是否达阈值，都会立即进入回退阶段。
double touch_speedl_acceleration{3.0};
// 触控阶段使用 moveL 时，沿 touch_twist_base 线速度方向前进的距离，单位米。
double touch_forward_l{0.08};
// 触控阶段使用 moveL 时的末端速度，单位 m/s。
double touch_movel_speed{0.25};
// 触控阶段使用 moveL 时的末端加速度，单位 m/s^2。
double touch_movel_acceleration{1.2};
// 触控阶段使用 moveL 时的末端 jerk，单位 m/s^3。
double touch_movel_jerk{5.0};
// 触控阶段使用 moveL 时的关节速度上限；为空时退回 robot->moveL 默认值。
std::vector<double> touch_movel_qd_max{2.5, 2.5, 2.5, 2.5, 2.5, 2.5, 2.5};
// 前进触控阶段的最大累计位移，单位米。
// 该距离仅在 TOUCHING 使用 speedL 时生效，由 TouchScreenApp 根据末端相对触控起点的累计位移判断。
// 大于 0 时，达到该距离后无论压力是否达阈值，都会立即进入回退阶段。
// 小于等于 0 时，表示不启用这条限制。
double touch_forward_duration_s{1.0};
double touch_speedl_forward_l{0.08};
// 接触后停留与回退。当前按末端 Tool 坐标系解释。
// 检测到接触后在当前位置停留的时间，单位秒。
@ -169,16 +183,17 @@ public:
double dwell_time_s{0.05};
// 回退阶段的 6 维速度命令 `[vx, vy, vz, wx, wy, wz]`。
// 当前机器人上 `[0, +0.08, 0, 0, 0, 0]` 表示沿 Tool +Y 方向向后离屏。
// 回退阶段统一使用 speedL。
Eigen::Matrix<double, 6, 1> retract_twist_base{
(Eigen::Matrix<double, 6, 1>() << 0.0, 0.08,0.0, 0.0, 0.0, 0.0).finished()};
// 回退阶段 speedL 的加速度参数。
double retract_acceleration{3.0};
// 回退阶段持续时间，单位秒。
// 回退阶段 speedL 持续时间，单位秒。
double retract_duration_s{0.8};
// 指尖触觉判据。
// 使用哪根手指的触觉阵列判断是否接触。
device::FingerType tactile_finger{device::FingerType::INDEX};
device::AbstractDexHand::FingerType tactile_finger{device::AbstractDexHand::FingerType::INDEX};
// 使用该手指的哪个触觉区域。
TactileRegion tactile_region{TactileRegion::TIP_AND_FINGER};
// 触觉压力和阈值；总和超过该值认为已经接触。
@ -201,6 +216,7 @@ public:
bool setOptionsFromConfig();
bool setOptionsFromConfig(const cmvr::config::TouchScreenAppConfig& config);
bool setTouchSpeedlForwardL(double forward_l);
bool startFromPixel(int u, int v);
bool step();
@ -246,8 +262,11 @@ private:
bool sendZeroJointVelocity() const;
bool holdCurrentControlledPosition() const;
bool moveToInitPositionIfEnabled() const;
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
void logTouchingSpeedLState() const;
bool startTouchPhase();
bool handleTouchTriggered(bool stop_forward_motion);
bool startRetractPhase(Phase next_phase_after_retract, Status final_status_after_retract);
void enterFailed(Status status);
@ -281,6 +300,8 @@ private:
double last_touch_pressure_sum_{0.0};
double last_touch_pressure_peak_{0.0};
Eigen::Vector3d last_align_error_camera_{Eigen::Vector3d::Zero()};
bool touch_start_position_valid_{false};
Eigen::Vector3d touch_start_position_base_{Eigen::Vector3d::Zero()};
Clock::time_point phase_start_time_{};
Status final_status_after_retract_{Status::DONE};

476
cmvr-es/applications/src/touch_screen_app.cpp
View File

@ -3,6 +3,7 @@
#include <algorithm>
#include <cmath>
#include <exception>
#include <iostream>
#include <limits>
#include <unordered_map>
@ -22,14 +23,48 @@ std::vector<double> toStdVector6(const Eigen::Matrix<double, 6, 1>& twist) {
return out;
}
void accumulateMatrixStats(const std::vector<std::vector<device::TactilePoint>>& matrix,
bool computeLinearMoveDeltaTool(const Eigen::Matrix<double, 6, 1>& twist_base,
const double move_length,
Eigen::Vector3d& delta_out) {
if (!std::isfinite(move_length) || move_length <= 0.0) {
return false;
}
const Eigen::Vector3d linear = twist_base.head<3>();
if (!linear.allFinite()) {
return false;
}
const double linear_norm = linear.norm();
if (!std::isfinite(linear_norm) || linear_norm <= 1e-9) {
return false;
}
delta_out = linear / linear_norm * move_length;
return delta_out.allFinite();
}
bool isTouchTriggered(const TouchScreenApp::Options& options,
const double pressure_sum,
const double pressure_peak) {
return pressure_sum >= options.tactile_pressure_sum_threshold ||
(options.tactile_pressure_peak_threshold > 0.0 &&
pressure_peak >= options.tactile_pressure_peak_threshold);
}
void accumulateMatrixStats(const device::AbstractDexHand::TactileMatrixView& matrix,
double& sum_out,
double& peak_out) {
for (const auto& row : matrix) {
for (const auto value : row) {
const double v = static_cast<double>(value);
sum_out += v;
peak_out = std::max(peak_out, v);
if (!matrix.valid()) {
return;
}
for (int row = 0; row < matrix.rows; ++row) {
const auto* row_data = matrix.rowData(row);
for (int col = 0; col < matrix.cols; ++col) {
const double value = static_cast<double>(row_data[col]);
sum_out += value;
peak_out = std::max(peak_out, value);
}
}
}
@ -84,17 +119,34 @@ double rotationErrorRad(const Eigen::Matrix3d& R_current,
return std::acos(cos_angle);
}
device::TactileRegion toDeviceTactileRegion(const TouchScreenApp::TactileRegion region) {
bool appendRequestedTactileRegions(
const device::AbstractDexHand::FingerType finger,
const TouchScreenApp::TactileRegion region,
std::vector<device::AbstractDexHand::TactileRegionKey>& regions_out) {
using DeviceTactileRegion = device::AbstractDexHand::TactileRegion;
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:
regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
return true;
case TouchScreenApp::TactileRegion::FINGER:
regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
return true;
case TouchScreenApp::TactileRegion::PAD:
regions_out.emplace_back(finger, DeviceTactileRegion::PAD);
return true;
case TouchScreenApp::TactileRegion::TIP_AND_FINGER:
return device::TactileRegion::TIP_AND_FINGER;
regions_out.emplace_back(finger, DeviceTactileRegion::TIP);
regions_out.emplace_back(finger, DeviceTactileRegion::FINGER);
return true;
case TouchScreenApp::TactileRegion::THUMB_MIDDLE:
return device::TactileRegion::THUMB_MIDDLE;
if (finger != device::AbstractDexHand::FingerType::THUMB) {
return false;
}
return device::TactileRegion::TIP;
regions_out.emplace_back(finger, DeviceTactileRegion::THUMB_MIDDLE);
return true;
}
return false;
}
perception::AprilTagPerception::DepthPolicy toDepthPolicy(
@ -121,20 +173,20 @@ perception::TagRelativeTarget3D::TargetPointMethod toTargetPointMethod(
return perception::TagRelativeTarget3D::TargetPointMethod::TAG_PLANE;
}
device::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) {
device::AbstractDexHand::FingerType toFingerType(const cmvr::config::TouchScreenFingerType finger) {
switch (finger) {
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_PINKY:
return device::FingerType::PINKY;
return device::AbstractDexHand::FingerType::PINKY;
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_RING:
return device::FingerType::RING;
return device::AbstractDexHand::FingerType::RING;
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_MIDDLE:
return device::FingerType::MIDDLE;
return device::AbstractDexHand::FingerType::MIDDLE;
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_INDEX:
return device::FingerType::INDEX;
return device::AbstractDexHand::FingerType::INDEX;
case cmvr::config::TOUCH_SCREEN_FINGER_TYPE_THUMB:
return device::FingerType::THUMB;
return device::AbstractDexHand::FingerType::THUMB;
}
return device::FingerType::INDEX;
return device::AbstractDexHand::FingerType::INDEX;
}
TouchScreenApp::TactileRegion toTactileRegion(
@ -409,6 +461,18 @@ bool TouchScreenApp::setOptionsFromConfig(const cmvr::config::TouchScreenAppConf
return last_status_ != Status::INVALID_CONFIG;
}
bool TouchScreenApp::setTouchSpeedlForwardL(double forward_l) {
if (!std::isfinite(forward_l) || forward_l < 0.0) {
last_status_ = Status::INVALID_CONFIG;
return false;
}
Options options = options_;
options.touch_speedl_forward_l = forward_l;
setOptions(options);
return last_status_ != Status::INVALID_CONFIG;
}
bool TouchScreenApp::startFromPixel(int u, int v) {
if (!initialized_) {
last_status_ = Status::NOT_INITIALIZED;
@ -435,6 +499,8 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
last_touch_pressure_peak_ = 0.0;
last_active_tag_id_ = -1;
last_align_error_camera_.setZero();
touch_start_position_valid_ = false;
touch_start_position_base_.setZero();
phase_ = Phase::ALIGNING;
phase_after_retract_ = Phase::DONE;
final_status_after_retract_ = Status::DONE;
@ -465,7 +531,11 @@ bool TouchScreenApp::step() {
return true;
}
if (!startTouchPhase()) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
last_status_ == Status::INVALID_CONFIG ||
last_status_ == Status::ROBOT_STATE_FAILED
? last_status_
: Status::ROBOT_COMMAND_FAILED);
return false;
}
return true;
@ -508,6 +578,8 @@ void TouchScreenApp::stop() {
last_touch_pressure_sum_ = 0.0;
last_touch_pressure_peak_ = 0.0;
last_align_error_camera_.setZero();
touch_start_position_valid_ = false;
touch_start_position_base_.setZero();
last_status_ = Status::STOPPED;
}
@ -678,11 +750,30 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
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_use_speedl()) {
options.touch_use_speedl = config.touch_use_speedl();
}
if (config.has_touch_forward_duration_s()) {
options.touch_forward_duration_s = config.touch_forward_duration_s();
if (config.has_touch_speedl_acceleration()) {
options.touch_speedl_acceleration = config.touch_speedl_acceleration();
}
if (config.has_touch_forward_l()) {
options.touch_forward_l = config.touch_forward_l();
}
if (config.has_touch_movel_speed()) {
options.touch_movel_speed = config.touch_movel_speed();
}
if (config.has_touch_movel_acceleration()) {
options.touch_movel_acceleration = config.touch_movel_acceleration();
}
if (config.has_touch_movel_jerk()) {
options.touch_movel_jerk = config.touch_movel_jerk();
}
if (config.touch_movel_qd_max_size() > 0) {
options.touch_movel_qd_max.assign(config.touch_movel_qd_max().begin(),
config.touch_movel_qd_max().end());
}
if (config.has_touch_speedl_forward_l()) {
options.touch_speedl_forward_l = config.touch_speedl_forward_l();
}
if (config.has_dwell_time_s()) {
@ -697,7 +788,6 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
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());
}
@ -722,10 +812,49 @@ bool TouchScreenApp::applyOptions() {
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) {
if (!std::isfinite(options_.touch_speedl_forward_l) ||
options_.touch_speedl_forward_l < 0.0) {
return false;
}
if (!options_.touch_use_speedl) {
Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
if (!computeLinearMoveDeltaTool(options_.touch_twist_base,
options_.touch_forward_l,
touch_forward_delta)) {
return false;
}
if (!std::isfinite(options_.touch_movel_speed) || options_.touch_movel_speed <= 0.0 ||
!std::isfinite(options_.touch_movel_acceleration) ||
options_.touch_movel_acceleration <= 0.0 ||
!std::isfinite(options_.touch_movel_jerk) || options_.touch_movel_jerk <= 0.0) {
return false;
}
if (!options_.touch_movel_qd_max.empty()) {
if (options_.touch_movel_qd_max.size() != options_.control_joint_names.size()) {
return false;
}
for (const double qd_max_i : options_.touch_movel_qd_max) {
if (!std::isfinite(qd_max_i) || qd_max_i <= 0.0) {
return false;
}
}
}
}
if (dexhand_) {
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
if (!appendRequestedTactileRegions(options_.tactile_finger,
options_.tactile_region,
tactile_regions)) {
return false;
}
try {
dexhand_->setTactilePollingRegions(tactile_regions);
} catch (...) {
return false;
}
}
perception_->setTagSize(options_.tag_size_m);
tracker_.setTargetPointMethod(options_.target_point_method);
@ -902,7 +1031,11 @@ bool TouchScreenApp::stepAligning() {
bool TouchScreenApp::stepTouching() {
if (!touch_command_started_) {
if (!startTouchPhase()) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
enterFailed(last_status_ == Status::TACTILE_UNAVAILABLE ||
last_status_ == Status::INVALID_CONFIG ||
last_status_ == Status::ROBOT_STATE_FAILED
? last_status_
: Status::ROBOT_COMMAND_FAILED);
return false;
}
}
@ -911,59 +1044,49 @@ bool TouchScreenApp::stepTouching() {
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)) {
if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
if (options_.touch_use_speedl) {
logTouchingSpeedLState();
}
if (!handleTouchTriggered(true)) {
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 (...) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
if (!options_.touch_use_speedl) {
last_status_ = Status::TOUCHING;
return true;
}
if (options_.touch_speedl_forward_l > 0.0) {
if (!touch_start_position_valid_) {
enterFailed(Status::ROBOT_STATE_FAILED);
return false;
}
phase_ = Phase::DWELLING;
phase_start_time_ = Clock::now();
last_status_ = Status::TOUCH_TRIGGERED;
return true;
};
if (touchTriggered()) {
return handleTouchTriggered();
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
if (!readCurrentTouchPointPositionBase(current_position_base)) {
enterFailed(Status::ROBOT_STATE_FAILED);
return false;
}
const double elapsed = std::chrono::duration<double>(Clock::now() - phase_start_time_).count();
if (options_.touch_forward_duration_s > 0.0 &&
elapsed >= options_.touch_forward_duration_s) {
const double traveled_distance =
(current_position_base - touch_start_position_base_).norm();
if (traveled_distance >= options_.touch_speedl_forward_l) {
logTouchingSpeedLState();
if (!updateTouchPressure()) {
enterFailed(Status::TACTILE_UNAVAILABLE);
return false;
}
if (touchTriggered()) {
return handleTouchTriggered();
if (isTouchTriggered(options_, last_touch_pressure_sum_, last_touch_pressure_peak_)) {
if (!handleTouchTriggered(true)) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
return false;
}
return true;
}
if (!startRetractPhase(Phase::FAILED, Status::TOUCH_FORWARD_TIMEOUT)) {
enterFailed(Status::ROBOT_COMMAND_FAILED);
@ -971,6 +1094,7 @@ bool TouchScreenApp::stepTouching() {
}
return true;
}
}
last_status_ = Status::TOUCHING;
return true;
@ -1070,6 +1194,53 @@ bool TouchScreenApp::sendZeroJointVelocity() const {
return sendJointVelocity(zero);
}
bool TouchScreenApp::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const {
if (!robot_) {
return false;
}
try {
const auto pose = robot_->fk(true);
p_out << pose.position().x(), pose.position().y(), pose.position().z();
return p_out.allFinite();
} catch (...) {
return false;
}
}
void TouchScreenApp::logTouchingSpeedLState() const {
if (!robot_ || !options_.touch_use_speedl || phase_ != Phase::TOUCHING) {
return;
}
const Eigen::Matrix<double, 6, 1> cmd_twist_base = robot_->getSpeedLCommandTwistBase();
Eigen::Vector3d current_position_base = Eigen::Vector3d::Zero();
const bool have_current_position = readCurrentTouchPointPositionBase(current_position_base);
const bool have_delta = touch_start_position_valid_ && have_current_position;
Eigen::Vector3d cumulative_delta_base = Eigen::Vector3d::Zero();
if (have_delta) {
cumulative_delta_base = current_position_base - touch_start_position_base_;
}
std::cout << "[TouchScreenApp][TOUCHING] speedl_cmd_base=["
<< cmd_twist_base[0] << ", "
<< cmd_twist_base[1] << ", "
<< cmd_twist_base[2] << ", "
<< cmd_twist_base[3] << ", "
<< cmd_twist_base[4] << ", "
<< cmd_twist_base[5] << "]";
if (have_delta) {
std::cout << ", cum_tcp_delta_base=["
<< cumulative_delta_base.x() << ", "
<< cumulative_delta_base.y() << ", "
<< cumulative_delta_base.z() << "]"
<< ", cum_tcp_dist=" << cumulative_delta_base.norm();
} else {
std::cout << ", cum_tcp_delta_base=[unavailable]";
}
std::cout << std::endl;
}
bool TouchScreenApp::holdCurrentControlledPosition() const {
if (!robot_) {
return false;
@ -1118,25 +1289,107 @@ bool TouchScreenApp::moveToInitPositionIfEnabled() const {
return true;
}
bool TouchScreenApp::startTouchPhase() {
if (!robot_) {
bool TouchScreenApp::handleTouchTriggered(const bool stop_forward_motion) {
if (options_.dwell_time_s < 0.0) {
if (!startRetractPhase(Phase::DONE, Status::DONE)) {
return false;
}
if (phase_ == Phase::RETRACTING) {
last_status_ = Status::TOUCH_TRIGGERED;
}
return true;
}
if (stop_forward_motion) {
try {
if (!robot_->speedL(toStdVector6(options_.touch_twist_base),
options_.touch_acceleration,
0.0,
cmvr::CartesianFrame::Tool)) {
return false;
}
robot_->stopSpeedL();
} catch (...) {
return false;
}
}
phase_ = Phase::DWELLING;
phase_start_time_ = Clock::now();
last_status_ = Status::TOUCH_TRIGGERED;
return true;
}
bool TouchScreenApp::startTouchPhase() {
if (!robot_) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
phase_ = Phase::TOUCHING;
phase_start_time_ = Clock::now();
touch_command_started_ = true;
retract_command_started_ = false;
last_status_ = Status::TOUCHING;
if (options_.touch_use_speedl) {
touch_start_position_valid_ = readCurrentTouchPointPositionBase(touch_start_position_base_);
if (options_.touch_speedl_forward_l > 0.0 && !touch_start_position_valid_) {
std::cerr << "[TouchScreenApp] startTouchPhase failed: cannot read touch start pose "
<< "for speedL distance-based touching" << std::endl;
last_status_ = Status::ROBOT_STATE_FAILED;
return false;
}
try {
if (!robot_->speedL(toStdVector6(options_.touch_twist_base),
options_.touch_speedl_acceleration,
0.0,
cmvr::CartesianFrame::Tool)) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
} catch (...) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
return true;
}
Eigen::Vector3d touch_forward_delta = Eigen::Vector3d::Zero();
if (!computeLinearMoveDeltaTool(options_.touch_twist_base,
options_.touch_forward_l,
touch_forward_delta)) {
last_status_ = Status::INVALID_CONFIG;
return false;
}
const std::vector<double> pose_cmd = {
touch_forward_delta.x(),
touch_forward_delta.y(),
touch_forward_delta.z(),
0.0,
0.0,
0.0
};
try {
if (!robot_->moveL(pose_cmd,
options_.touch_movel_speed,
options_.touch_movel_acceleration,
options_.touch_movel_jerk,
options_.touch_movel_qd_max,
false)) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
} catch (...) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
if (!moveToInitPositionIfEnabled()) {
last_status_ = Status::ROBOT_COMMAND_FAILED;
return false;
}
phase_ = Phase::DONE;
touch_command_started_ = false;
retract_command_started_ = false;
last_status_ = Status::DONE;
return true;
}
@ -1145,6 +1398,7 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract,
if (!robot_) {
return false;
}
try {
if (!robot_->speedL(toStdVector6(options_.retract_twist_base),
options_.retract_acceleration,
@ -1155,6 +1409,7 @@ bool TouchScreenApp::startRetractPhase(const Phase next_phase_after_retract,
} catch (...) {
return false;
}
phase_ = Phase::RETRACTING;
phase_after_retract_ = next_phase_after_retract;
final_status_after_retract_ = final_status_after_retract;
@ -1180,70 +1435,31 @@ void TouchScreenApp::enterFailed(const Status status) {
}
bool TouchScreenApp::updateTouchPressure() {
if (!dexhand_) {
last_touch_pressure_sum_ = 0.0;
last_touch_pressure_peak_ = 0.0;
if (!dexhand_) {
return false;
}
std::vector<device::AbstractDexHand::TactileRegionKey> tactile_regions;
if (!appendRequestedTactileRegions(options_.tactile_finger,
options_.tactile_region,
tactile_regions)) {
return false;
}
const auto& sensors = dexhand_->getSensorData(options_.tactile_finger,
toDeviceTactileRegion(options_.tactile_region));
double sum = 0.0;
double peak = 0.0;
auto accumulate_finger = [&](const auto& finger_sensor) {
switch (options_.tactile_region) {
case TactileRegion::TIP:
accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
break;
case TactileRegion::FINGER:
accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
break;
case TactileRegion::PAD:
accumulateMatrixStats(finger_sensor.pad.data, sum, peak);
break;
case TactileRegion::TIP_AND_FINGER:
accumulateMatrixStats(finger_sensor.tip.data, sum, peak);
accumulateMatrixStats(finger_sensor.finger.data, sum, peak);
break;
case TactileRegion::THUMB_MIDDLE:
break;
try {
for (const auto& tactile_region : tactile_regions) {
const auto sensor = dexhand_->getSensorData(tactile_region.first, tactile_region.second);
if (!sensor.valid()) {
return false;
}
};
switch (options_.tactile_finger) {
case device::FingerType::PINKY:
accumulate_finger(sensors.pinky);
break;
case device::FingerType::RING:
accumulate_finger(sensors.ring);
break;
case device::FingerType::MIDDLE:
accumulate_finger(sensors.middle);
break;
case device::FingerType::INDEX:
accumulate_finger(sensors.index);
break;
case device::FingerType::THUMB:
switch (options_.tactile_region) {
case TactileRegion::TIP:
accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
break;
case TactileRegion::FINGER:
accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
break;
case TactileRegion::PAD:
accumulateMatrixStats(sensors.thumb.pad.data, sum, peak);
break;
case TactileRegion::TIP_AND_FINGER:
accumulateMatrixStats(sensors.thumb.tip.data, sum, peak);
accumulateMatrixStats(sensors.thumb.finger.data, sum, peak);
break;
case TactileRegion::THUMB_MIDDLE:
accumulateMatrixStats(sensors.thumb.middle.data, sum, peak);
break;
accumulateMatrixStats(sensor.view, sum, peak);
}
break;
} catch (...) {
return false;
}
last_touch_pressure_sum_ = sum;

54
cmvr-es/common/config/touch_screen_app_config/touch_screen_app_config.pb.txt
View File

@ -5,31 +5,31 @@ move_to_init_position_before_start: true
move_to_init_position: true
init_joint_positions {
joint_name: "R_SHOULDER_P"
rad: -0.2423
rad: -0.3678
}
init_joint_positions {
joint_name: "R_SHOULDER_R"
rad: 1.2929
rad: 1.1127
}
init_joint_positions {
joint_name: "R_SHOULDER_Y"
rad: 1.61
rad: 1.6084
}
init_joint_positions {
joint_name: "R_ELBOW_R"
rad: 1.58
rad: 1.69508
}
init_joint_positions {
joint_name: "R_WRIST_P"
rad: -2.8792
rad: -2.5718
}
init_joint_positions {
joint_name: "R_WRIST_Y"
rad: 0.1150
rad: 0.1276
}
init_joint_positions {
joint_name: "R_WRIST_R"
rad: -0.08
rad: 0.1297
}
init_movej_vel: 1.0
init_movej_acc: 2.0
@ -39,20 +39,20 @@ base_link: "PELVIS_S"
flange_link: "R_WRIST_R_S"
camera_link: "R_CAM"
tag_size_m: 0.02
tag_size_m: 0.012
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
x: 0.005
y: 0.075
z: 0.15
}
target_rx: 3.14159265358979323846
target_ry: 0.0
target_rz: 0.0
ibvs_lambda: 0.6
ibvs_lambda: 0.3
ibvs_mu: 0.1
ibvs_qdot_max: 0.15
ibvs_vmax6 {
@ -88,10 +88,10 @@ 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
align_z_threshold_m: 0.003
align_rot_threshold_rad: 0.1026646259971647
align_stable_frames: 3
align_timeout_s: 30.0
pause_after_align_reached: false
touch_twist_base {
@ -102,10 +102,22 @@ touch_twist_base {
wy: 0.0
wz: 0.0
}
touch_acceleration: 3.0
touch_forward_duration_s: 1.2
touch_use_speedl: true
touch_speedl_acceleration: 3.0
touch_forward_l: 0.064
touch_movel_speed: 0.1
touch_movel_acceleration: 5.0
touch_movel_jerk: 5.0
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_movel_qd_max: 2.5
touch_speedl_forward_l: 0.0255
dwell_time_s: -1.0
dwell_time_s: -1
retract_twist_base {
vx: 0.0
vy: 0.08
@ -115,9 +127,9 @@ retract_twist_base {
wz: 0.0
}
retract_acceleration: 3.0
retract_duration_s: 1.0
retract_duration_s: 0.45
tactile_finger: TOUCH_SCREEN_FINGER_TYPE_INDEX
tactile_region: TOUCH_SCREEN_TACTILE_REGION_TIP
tactile_pressure_sum_threshold: 300.0
tactile_pressure_sum_threshold: 300000.0
tactile_pressure_peak_threshold: 0.0

72
cmvr-es/common/utils/config_helper/src/config_setting.cpp
View File

@ -1,19 +1,69 @@
#include "common/utils/config_helper/include/config_setting.h"
#include <array>
#include <filesystem>
#include <limits.h>
#include <unistd.h>
namespace {
static std::string basePath()
{
char* cwd = ::getcwd(nullptr, 0);
if (!cwd) return {};
std::string s(cwd);
std::free(cwd);
return s + "/config/";
std::string normalizeDirectory(std::filesystem::path path) {
auto text = path.lexically_normal().string();
if (!text.empty() && text.back() != '/') {
text.push_back('/');
}
return text;
}
std::string probeConfigDirectory(std::filesystem::path start) {
static const std::array<std::filesystem::path, 4> candidates = {
std::filesystem::path("output/bin/config"),
std::filesystem::path("cmvr-es/common/config"),
std::filesystem::path("config"),
std::filesystem::path("common/config")
};
start = start.lexically_normal();
while (!start.empty()) {
for (const auto& candidate : candidates) {
const auto path = start / candidate;
if (std::filesystem::exists(path) && std::filesystem::is_directory(path)) {
return normalizeDirectory(path);
}
}
const auto parent = start.parent_path();
if (parent == start) {
break;
}
start = parent;
}
return {};
}
std::filesystem::path executableDirectory() {
char exe_path[PATH_MAX] = {0};
const auto count = ::readlink("/proc/self/exe", exe_path, sizeof(exe_path) - 1);
if (count <= 0) {
return {};
}
exe_path[count] = '\0';
return std::filesystem::path(exe_path).parent_path();
}
} // namespace
static std::string basePath() {
if (const auto resolved = probeConfigDirectory(executableDirectory()); !resolved.empty()) {
return resolved;
}
if (const auto resolved = probeConfigDirectory(std::filesystem::current_path()); !resolved.empty()) {
return resolved;
}
return "config/";
}
// static std::string basePath()
// {
// return "/home/lgv/cmvr/cmvr-es/output/bin/config/";
// }
DEFINE_string(pinocchio_qp_ik_solver_config_file,
basePath() + "ik_solver_config/pinocchio_qp_ik_solver_config.pb.txt",

4
cmvr-es/devices/biohead/biohead_esp32/CMakeLists.txt
View File

@ -3,7 +3,9 @@ add_library(head_esp32 SHARED src/biohead_esp32.cpp)
# 设置头文件包含路径
target_include_directories(head_esp32 PUBLIC ${CMAKE_CURRENT_SOURCE_DIR})
target_link_libraries(head_esp32 PRIVATE
cmvr_es::hardware
)
# 创建命名空间化的 ALIAS（建议使用项目命名空间）
add_library(cmvr::device::head_esp32 ALIAS head_esp32)

2
cmvr-es/devices/camera/realsense_camera/src/realsense_camera.cpp
View File

@ -249,7 +249,7 @@ void RealsenseCamera::start() {
return;
}
constexpr int kStartMaxRetry = 3;
constexpr int kStartMaxRetry = 5;
constexpr int kRetrySleepMs = 300;
constexpr unsigned int kProbeTimeoutMs = 2000;
std::string last_error;

318
cmvr-es/devices/dexhand/abstract_dexhand.h
View File

@ -6,236 +6,144 @@
#define CMVR_ES_ABSTRACT_DEXHAND_H
#pragma once
#include <cstdint>
#include <memory>
#include <stdexcept>
#include <string>
#include <utility>
#include <vector>
#include "devices/abstract_device.h"
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
namespace cmvr::device{
// 单个触觉点数据（16位无符号整数）
namespace cmvr::device {
class AbstractDexHand : public AbstractDevice {
public:
using TactilePoint = uint16_t;
// 手指触觉数据结构体（支持从数组任意位置解析）
struct FingerTactileData {
std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
int rows; // 行数
int cols; // 列数
int byteSize; // 总字节数（rows * cols * 2字节/点）
std::string name; // 部位名称
// 初始化数据数组
void init() {
data.resize(rows, std::vector<TactilePoint>(cols, 0));
}
// 从 std::vector<uint16_t> 中赋值数据
void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
int index = startIndex;
for (int i = 0; i < rows; ++i) {
for (int j = 0; j < cols; ++j) {
if (index < values.size()) {
data[i][j] = values[index];
index++;
}
}
}
}
enum class FingerType {
PINKY,
RING,
MIDDLE,
INDEX,
THUMB,
PALM
};
// 手掌触觉数据结构体（支持从数组任意位置解析）
struct PalmTactileData {
std::vector<std::vector<TactilePoint>> data; // 触觉数据二维数组
int rows = 8; // 行数（掌心固定为8行）
int cols = 14; // 列数（掌心固定为14列）
int byteSize = 224; // 总字节数（8*14*2=224）
std::string name = "掌心"; // 部位名称
// 初始化数据数组
void init() {
data.resize(rows, std::vector<TactilePoint>(cols, 0));
}
// 从 std::vector<uint16_t> 中赋值数据
void assignFromVector(const std::vector<uint16_t>& values, int startIndex = 0) {
int index = startIndex;
for (int j = 0; j < cols; ++j) {
for (int i = rows - 1; i >= 0; --i) {
if (index < values.size()) {
data[i][j] = values[index];
index++;
}
}
}
}
};
// 手指枚举（方便按手指类型解析）
enum FingerType {
PINKY, // 小拇指
RING, // 无名指
MIDDLE, // 中指
INDEX, // 食指
THUMB // 大拇指
};
// 触觉区域枚举（设备层）。
enum class TactileRegion {
TIP = 0,
TIP,
FINGER,
PAD,
TIP_AND_FINGER,
THUMB_MIDDLE
THUMB_MIDDLE,
PALM_PAD
};
// 整只手的触觉传感器数据
struct HandTactileSensors {
// 五指的触觉数据（每根手指包含指端、指尖、指腹）
struct {
FingerTactileData tip; // 指端
FingerTactileData finger; // 指尖
FingerTactileData pad; // 指腹
} pinky, ring, middle, index; // 小拇指、无名指、中指、食指
using TactileRegionKey = std::pair<FingerType, TactileRegion>;
// 大拇指（特殊：多一个指中）
struct {
FingerTactileData tip; // 指端
FingerTactileData finger; // 指尖
FingerTactileData middle; // 指中
FingerTactileData pad; // 指腹
} thumb;
struct TactileMatrixView {
const TactilePoint* data{nullptr};
int rows{0};
int cols{0};
// 掌心触觉数据
PalmTactileData palm;
// 初始化所有传感器数据（设置相对偏移量）
void init() {
// 小拇指（总370byte）
pinky.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "小拇指指端"};
pinky.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "小拇指指尖"};
pinky.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "小拇指指腹"};
// 无名指（总370byte）
ring.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "无名指指端"};
ring.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "无名指指尖"};
ring.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "无名指指腹"};
// 中指（总370byte）
middle.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "中指指端"};
middle.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "中指指尖"};
middle.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "中指指腹"};
// 食指（总370byte）
index.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "食指指端"};
index.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "食指指尖"};
index.pad = {.rows = 10, .cols = 8, .byteSize = 160, .name = "食指指腹"};
// 大拇指（总420byte）
thumb.tip = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指端"};
thumb.finger = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指尖"};
thumb.middle = {.rows = 3, .cols = 3, .byteSize = 18, .name = "大拇指指中"};
thumb.pad = {.rows = 12, .cols = 8, .byteSize = 192, .name = "大拇指指腹"};
// 掌心（总224byte）
palm = { .name = "掌心"};
// 初始化所有数据数组
pinky.tip.init();
pinky.finger.init();
pinky.pad.init();
ring.tip.init();
ring.finger.init();
ring.pad.init();
middle.tip.init();
middle.finger.init();
middle.pad.init();
index.tip.init();
index.finger.init();
index.pad.init();
thumb.tip.init();
thumb.finger.init();
thumb.middle.init();
thumb.pad.init();
palm.init();
}
// 按手指类型解析整个手指的数据
void parseFinger(FingerType fingerType, const std::vector<uint16_t>& values) {
switch (fingerType) {
case PINKY:
pinky.tip.assignFromVector(values);
pinky.finger.assignFromVector(values, 9);
pinky.pad.assignFromVector(values, 9 + 96);
break;
case RING:
ring.tip.assignFromVector(values);
ring.finger.assignFromVector(values, 9);
ring.pad.assignFromVector(values, 9 + 96);
break;
case MIDDLE:
middle.tip.assignFromVector(values);
middle.finger.assignFromVector(values, 9);
middle.pad.assignFromVector(values, 9 + 96);
break;
case INDEX:
index.tip.assignFromVector(values);
index.finger.assignFromVector(values, 9);
index.pad.assignFromVector(values, 9 + 96);
break;
case THUMB:
thumb.tip.assignFromVector(values);
thumb.finger.assignFromVector(values, 9);
thumb.middle.assignFromVector(values, 9 + 96);
thumb.pad.assignFromVector(values, 9 + 96 + 9);
break;
}
bool valid() const {
return data != nullptr && rows > 0 && cols > 0;
}
// 解析手心（掌心）数据
void parsePalm(const std::vector<uint16_t>& values) {
palm.assignFromVector(values);
int pointCount() const {
return rows * cols;
}
// 获取手指名称（用于调试）
std::string getFingerName(FingerType fingerType) {
switch (fingerType) {
case PINKY: return "小拇指";
case RING: return "无名指";
case MIDDLE: return "中指";
case INDEX: return "食指";
case THUMB: return "大拇指";
default: return "未知";
const TactilePoint* rowData(int row) const {
return data + row * cols;
}
TactilePoint at(int row, int col) const {
return data[row * cols + col];
}
};
class AbstractDexHand: public AbstractDevice{
class TactileRegionData {
public:
TactileRegionData() = default;
TactileRegionData(FingerType finger_in,
TactileRegion region_in,
TactileMatrixView view_in,
const char* name_in,
std::shared_ptr<const void> lifetime = {})
: finger(finger_in),
region(region_in),
view(view_in),
name(name_in),
lifetime_(std::move(lifetime)) {}
bool valid() const {
return view.valid();
}
int byteSize() const {
return view.pointCount() * static_cast<int>(sizeof(TactilePoint));
}
FingerType finger{FingerType::PINKY};
TactileRegion region{TactileRegion::TIP};
TactileMatrixView view{};
const char* name{nullptr};
private:
std::shared_ptr<const void> lifetime_;
};
enum class Status {
CREATED,
INITIALIZED,
STREAMING,
STOPPED,
FAULT
};
AbstractDexHand() = default;
explicit AbstractDexHand(const XmlNode& cfg): AbstractDevice(cfg) {}
explicit AbstractDexHand(const XmlNode& cfg) : AbstractDevice(cfg) {}
~AbstractDexHand() override = default;
virtual void getState(DexHandState &state) {}
virtual Status state() const = 0;
virtual std::string lastError() const = 0;
// control
virtual void setPositions(const std::vector<int>& finger_joint_targets) {}
virtual void setAngles(const std::vector<int>& finger_joint_angles) {}
virtual void setVelocities(const std::vector<int>& finger_joint_velocities) {}
virtual void setPresetAct(int action_id) {}
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();
virtual void getState(DexHandState& state) {
state = DexHandState{};
const auto lifecycle = this->state();
state.is_initialized =
lifecycle == Status::INITIALIZED ||
lifecycle == Status::STREAMING;
}
virtual void setAngles(const std::vector<int>& finger_joint_angles) = 0;
virtual void setTactilePollingRegion(FingerType finger, TactileRegion region) {
setTactilePollingRegions({TactileRegionKey{finger, region}});
}
virtual void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) = 0;
virtual std::vector<TactileRegionData> getSensorData() = 0;
virtual TactileRegionData getSensorData(FingerType finger, TactileRegion region) = 0;
virtual void setPositions(const std::vector<int>&) {
throw std::logic_error("setPositions is not supported by this dexhand abstraction.");
}
virtual void setVelocities(const std::vector<int>&) {
throw std::logic_error("setVelocities is not supported by this dexhand abstraction.");
}
virtual void setPresetAct(int) {
throw std::logic_error("setPresetAct is not supported by this dexhand abstraction.");
}
virtual void execPresetAct(int) {
throw std::logic_error("execPresetAct is not supported by this dexhand abstraction.");
}
virtual void setForce(const std::vector<int>&) {
throw std::logic_error("setForce is not supported by this dexhand abstraction.");
}
protected:
DexHandState status_;
HandTactileSensors hand_tactile_sensors_;
};
}

14
cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeLists.txt
View File

@ -7,3 +7,17 @@ add_library(cmvr_es::device::rh56dftp_dexhand ALIAS rh56dftp_dexhand)
target_link_libraries(rh56dftp_dexhand PRIVATE cmvr_es::hardware -lmodbus)
install(TARGETS rh56dftp_dexhand LIBRARY DESTINATION lib)
add_executable(rh56dftp_dexhand_test
src/rh56dftp_dexhand_test.cpp
)
target_link_libraries(rh56dftp_dexhand_test PRIVATE
cmvr_es::device::rh56dftp_dexhand
cmvr_es::common
cmvr_es::proto
glog
gtest
gtest_main
pthread
)

126
cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h
View File

@ -5,78 +5,124 @@
#ifndef RH56DFTP_DEXHAND_H
#define RH56DFTP_DEXHAND_H
#include <thread>
#include <mutex>
#include <queue>
#include <array>
#include <atomic>
#include <bitset>
#include <chrono>
#include <condition_variable>
#include <memory>
#include <mutex>
#include <shared_mutex>
#include <string>
#include <thread>
#include <vector>
#include <modbus/modbus.h>
#include <boost/lockfree/spsc_queue.hpp>
#include "../../../../utils/base/include/os.h"
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
#include "../../abstract_dexhand.h"
#include "../../../../hardware/include/serial_interface.h"
#include "rh56dftp_tactile_buffer.h"
namespace cmvr::device {
class ModbusController {
private:
modbus_t *ctx;
std::map<std::string, int> regdict;
static constexpr int MAX_REGISTERS_PER_READ = 64; // 根据Modbus协议限制设置
public:
ModbusController();
ModbusController() = default;
~ModbusController();
bool open(const std::string& ip, int port);
void open(const std::string& ip, int port);
void close();
bool isOpen() const;
bool writeRegisters(int address, const std::vector<uint16_t>& values);
std::vector<uint16_t> readRegisters(int address, int count);
std::vector<uint16_t> readRegisterRange(int start_addr, int end_addr);
void writeRegisters(int address, const uint16_t* values, int count);
void readRegisterBlock(int start_addr, int count, std::vector<uint16_t>& values);
bool write6(const std::string& reg_name, const std::vector<int>& val);
bool read6(const std::string& reg_name, std::vector<int>& result);
private:
void closeUnlocked();
static void sleep(float seconds);
modbus_t* ctx_{nullptr};
mutable std::mutex io_mutex_;
};
class RH56DFTPDexhand final : public AbstractDexHand{
class RH56DFTPDexhand final : public AbstractDexHand {
public:
using FingerType = AbstractDexHand::FingerType;
using TactileRegion = AbstractDexHand::TactileRegion;
using TactileRegionKey = AbstractDexHand::TactileRegionKey;
using TactileRegionData = AbstractDexHand::TactileRegionData;
using Status = AbstractDexHand::Status;
static constexpr size_t ANGLE_COMMAND_COUNT = 6;
static constexpr size_t TACTILE_REGION_SLOT_COUNT = RH56TactileBuffer::REGION_COUNT;
using RegionMask = std::bitset<TACTILE_REGION_SLOT_COUNT>;
explicit RH56DFTPDexhand(const XmlNode& cfg);
RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg);
explicit RH56DFTPDexhand(const config::RH56DFTPDexHandConfig& cfg);
~RH56DFTPDexhand() override;
void init() override;
void start() override;
void stop() override;
Status state() const override;
std::string lastError() const override;
void getState(DexHandState& state) override;
void getState(DexHandState &state) override;
// control
void setPositions(const std::vector<int>& finger_joint_targets) override;
void setAngles(const std::vector<int>& finger_joint_angles) override;
void setVelocities(const std::vector<int>& finger_joint_velocities) override;
void setPresetAct(int action_id) override;//这个接口暂时不可用，未开放寄存器设置预设
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();
void setTactilePollingRegions(const std::vector<TactileRegionKey>& regions) override;
std::vector<TactileRegionData> getSensorData() override;
TactileRegionData getSensorData(FingerType finger, TactileRegion region) override;
int default_force_;//上电的力控阈值
int default_speed_;//上电的自由角转动速度
private:
struct TactileBufferLease {
std::shared_lock<std::shared_mutex> lock;
const RH56TactileBuffer* buffer{nullptr};
};
void parseXmlConfig();
void initializeTactileBuffers();
void ensureConnected();
void refreshTactileData(const RegionMask& mask);
void tactilePollingLoop();
std::shared_ptr<const TactileBufferLease> acquireActiveBufferLease() const;
std::vector<TactileRegionData> acquireAllRegionData() const;
TactileRegionData acquireRegionData(FingerType finger, TactileRegion region) const;
bool isRequestedMaskCovered(const RegionMask& mask) const;
bool isActiveBufferReady(const RegionMask& mask) const;
void ensureTactileMaskReady(const RegionMask& mask, bool allow_background);
void transitionTo(Status next_state);
void enterFault(const std::string& error);
bool isOperationalState(Status lifecycle) const;
std::string ip_address_;
int port_;
std::shared_ptr<ModbusController> controller_;
int port_{6000};
std::unique_ptr<ModbusController> controller_;
config::RH56DFTPDexHandConfig dexhandCfg_;
mutable std::mutex lifecycle_mutex_;
Status lifecycle_state_{Status::CREATED};
std::string last_error_;
mutable std::mutex command_mutex_;
std::array<int, ANGLE_COMMAND_COUNT> last_commanded_angles_{};
std::array<RH56TactileBuffer, 2> tactile_buffers_;
std::array<RegionMask, 2> tactile_buffer_masks_{};
mutable std::shared_mutex tactile_buffer_mutex_;
mutable std::mutex tactile_refresh_mutex_;
mutable std::mutex polling_mutex_;
std::condition_variable polling_cv_;
RegionMask requested_polling_mask_{};
std::thread tactile_thread_;
std::atomic<bool> tactile_thread_running_{false};
std::atomic<int> active_buffer_index_{0};
std::chrono::milliseconds tactile_poll_interval_{20};
};
}
#endif //RH56DFTP_DEXHAND_H

136
cmvr-es/devices/dexhand/rh56dftp_dexhand/include/rh56dftp_tactile_buffer.h Normal file
View File

@ -0,0 +1,136 @@
#ifndef RH56DFTP_TACTILE_BUFFER_H
#define RH56DFTP_TACTILE_BUFFER_H
#pragma once
#include <array>
#include <cstddef>
#include <stdexcept>
#include "../../abstract_dexhand.h"
namespace cmvr::device {
enum class RH56TactileFillOrder {
ROW_MAJOR,
COLUMN_MAJOR_BOTTOM_TO_TOP
};
struct RH56TactileRegionLayout {
AbstractDexHand::FingerType finger{AbstractDexHand::FingerType::PINKY};
AbstractDexHand::TactileRegion region{AbstractDexHand::TactileRegion::TIP};
const char* name{nullptr};
int rows{0};
int cols{0};
size_t offset{0};
size_t point_count{0};
RH56TactileFillOrder fill_order{RH56TactileFillOrder::ROW_MAJOR};
AbstractDexHand::TactileMatrixView view(const AbstractDexHand::TactilePoint* buffer) const {
return AbstractDexHand::TactileMatrixView{buffer + offset, rows, cols};
}
};
class RH56TactileBuffer {
public:
static constexpr size_t REGION_COUNT = 17;
static constexpr size_t POINT_COUNT = 1062;
void clear() {
values_.fill(0);
}
void assignRegionData(AbstractDexHand::FingerType finger,
AbstractDexHand::TactileRegion region,
const AbstractDexHand::TactilePoint* input,
int valueCount) {
const auto& layout = regionLayout(finger, region);
auto* destination = values_.data() + layout.offset;
const auto sanitize_point = [](const AbstractDexHand::TactilePoint value) {
return value > static_cast<AbstractDexHand::TactilePoint>(4096) ? 0 : value;
};
int input_index = 0;
if (layout.fill_order == RH56TactileFillOrder::ROW_MAJOR) {
for (size_t point = 0; point < layout.point_count && input_index < valueCount; ++point, ++input_index) {
destination[point] = sanitize_point(input[input_index]);
}
return;
}
for (int col = 0; col < layout.cols; ++col) {
for (int row = layout.rows - 1; row >= 0 && input_index < valueCount; --row, ++input_index) {
destination[static_cast<size_t>(row * layout.cols + col)] = sanitize_point(input[input_index]);
}
}
}
const RH56TactileRegionLayout& regionLayout(AbstractDexHand::FingerType finger,
AbstractDexHand::TactileRegion region) const {
return layouts()[findRegionIndex(finger, region)];
}
AbstractDexHand::TactileMatrixView regionView(AbstractDexHand::FingerType finger,
AbstractDexHand::TactileRegion region) const {
const auto& layout = regionLayout(finger, region);
return layout.view(values_.data());
}
const std::array<RH56TactileRegionLayout, REGION_COUNT>& layouts() const {
return staticLayouts();
}
const AbstractDexHand::TactilePoint* data() const {
return values_.data();
}
private:
static const std::array<RH56TactileRegionLayout, REGION_COUNT>& staticLayouts() {
static const std::array<RH56TactileRegionLayout, REGION_COUNT> layouts = {{
{AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::TIP, "小拇指指端", 3, 3, 0, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::FINGER, "小拇指指尖", 12, 8, 9, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::PINKY, AbstractDexHand::TactileRegion::PAD, "小拇指指腹", 10, 8, 105, 80, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::TIP, "无名指指端", 3, 3, 185, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::FINGER, "无名指指尖", 12, 8, 194, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::RING, AbstractDexHand::TactileRegion::PAD, "无名指指腹", 10, 8, 290, 80, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::TIP, "中指指端", 3, 3, 370, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::FINGER, "中指指尖", 12, 8, 379, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::MIDDLE, AbstractDexHand::TactileRegion::PAD, "中指指腹", 10, 8, 475, 80, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::TIP, "食指指端", 3, 3, 555, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::FINGER, "食指指尖", 12, 8, 564, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::INDEX, AbstractDexHand::TactileRegion::PAD, "食指指腹", 10, 8, 660, 80, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::TIP, "大拇指指端", 3, 3, 740, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::FINGER, "大拇指尖", 12, 8, 749, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::THUMB_MIDDLE, "大拇指指中", 3, 3, 845, 9, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::THUMB, AbstractDexHand::TactileRegion::PAD, "大拇指指腹", 12, 8, 854, 96, RH56TactileFillOrder::ROW_MAJOR},
{AbstractDexHand::FingerType::PALM,
AbstractDexHand::TactileRegion::PALM_PAD,
"掌心",
8,
14,
950,
112,
RH56TactileFillOrder::COLUMN_MAJOR_BOTTOM_TO_TOP}
}};
return layouts;
}
static size_t findRegionIndex(AbstractDexHand::FingerType finger, AbstractDexHand::TactileRegion region) {
const auto& region_layouts = staticLayouts();
for (size_t index = 0; index < region_layouts.size(); ++index) {
if (region_layouts[index].finger == finger && region_layouts[index].region == region) {
return index;
}
}
throw std::invalid_argument("Invalid tactile region for selected finger.");
}
std::array<AbstractDexHand::TactilePoint, POINT_COUNT> values_{};
};
}
#endif //RH56DFTP_TACTILE_BUFFER_H

983
cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp
View File

File diff suppressed because it is too large Load Diff

137
cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp Normal file
View File

@ -0,0 +1,137 @@
#include "gtest/gtest.h"
#include "../include/rh56dftp_dexhand.h"
#include "cmvr/config/dexhand_config/dexhand_config.pb.h"
#include "common/utils/config_helper/include/config_helper.h"
#include <chrono>
#include <iomanip>
#include <iostream>
#include <stdexcept>
#include <string>
#include <thread>
#include <vector>
namespace {
using DexHand = cmvr::device::AbstractDexHand;
using RH56DexHand = cmvr::device::RH56DFTPDexhand;
const char* tactileRegionToString(const DexHand::TactileRegion region) {
switch (region) {
case DexHand::TactileRegion::TIP: return "TIP";
case DexHand::TactileRegion::FINGER: return "FINGER";
case DexHand::TactileRegion::PAD: return "PAD";
case DexHand::TactileRegion::THUMB_MIDDLE: return "THUMB_MIDDLE";
case DexHand::TactileRegion::PALM_PAD: return "PALM_PAD";
}
return "UNKNOWN";
}
struct StopGuard {
std::shared_ptr<RH56DexHand> hand;
~StopGuard() {
if (!hand) {
return;
}
try {
hand->stop();
} catch (...) {
}
}
};
} // namespace
TEST(RH56DFTPDexhandLatencyTest, ReadConfiguredRegionAndMeasureLatency) {
cmvr::config::DexHandConfig dexhand_config;
ASSERT_TRUE(cmvr::ConfigHelper::getDexHandsConfig(dexhand_config));
const cmvr::config::RH56DFTPDexHandConfig* hand_config = nullptr;
for (const auto& config : dexhand_config.rh56dftp_dexhands()) {
if (config.enable() && !config.ip().empty()) {
hand_config = &config;
break;
}
if (hand_config == nullptr && !config.ip().empty()) {
hand_config = &config;
}
}
ASSERT_NE(hand_config, nullptr);
const auto finger = DexHand::FingerType::INDEX;
const auto region = DexHand::TactileRegion::FINGER;
const int iterations = 20000;
const int warmup_ms = 200;
const int read_interval_ms = 10;
auto hand = std::make_shared<RH56DexHand>(*hand_config);
ASSERT_NO_THROW(hand->init());
ASSERT_NO_THROW(hand->start());
StopGuard stop_guard{hand};
EXPECT_EQ(hand->state(), DexHand::Status::STREAMING);
ASSERT_NO_THROW(hand->setTactilePollingRegion(finger, region));
if (warmup_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(warmup_ms));
}
double last_pressure_sum = 0.0;
double last_pressure_peak = 0.0;
int point_count = 0;
std::string sensor_name;
for (int i = 0; i < iterations; ++i) {
const auto region_data = hand->getSensorData(finger, region);
ASSERT_TRUE(region_data.valid());
EXPECT_EQ(region_data.finger, finger);
EXPECT_EQ(region_data.region, region);
if (point_count == 0) {
point_count = region_data.view.pointCount();
sensor_name = region_data.name == nullptr ? "" : region_data.name;
}
last_pressure_sum = 0.0;
last_pressure_peak = 0.0;
for (int index = 0; index < region_data.view.pointCount(); ++index) {
const double pressure = static_cast<double>(region_data.view.data[index]);
last_pressure_sum += pressure;
if (pressure > last_pressure_peak) {
last_pressure_peak = pressure;
}
}
std::cout << std::fixed << std::setprecision(3)
<< "[RH56DFTPDexhandLatencyTest] iter=" << (i + 1)
<< "/" << iterations
<< " sensor=" << sensor_name
<< " pressure_sum=" << last_pressure_sum
<< " pressure_peak=" << last_pressure_peak
<< std::endl;
if (read_interval_ms > 0) {
std::this_thread::sleep_for(std::chrono::milliseconds(read_interval_ms));
}
}
std::cout << std::fixed << std::setprecision(3)
<< "[RH56DFTPDexhandLatencyTest] id=" << hand_config->id()
<< " ip=" << hand_config->ip()
<< " port=" << hand_config->port()
<< " finger=INDEX"
<< " region=" << tactileRegionToString(region)
<< " sensor=" << sensor_name
<< " iterations=" << iterations
<< " points=" << point_count
<< " last_pressure_sum=" << last_pressure_sum
<< " last_pressure_peak=" << last_pressure_peak
<< " read_interval_ms=" << read_interval_ms
<< "\n";
EXPECT_GT(point_count, 0);
}

20
cmvr-es/devices/robot/abstract_robot.h
View File

@ -12,6 +12,7 @@
#include "planner/cartesian_space_planner/include/cartesian_twist_limiter.h"
#include "cmvr/msgs/geometry.pb.h"
#include "cmvr/msgs/motor.pb.h"
#include <Eigen/Dense>
namespace cmvr::device{
@ -103,18 +104,23 @@ namespace cmvr::device{
virtual void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveL(math::Pose3d &pose, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
virtual void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel=0.5, double acc=0.1) { throw std::runtime_error("Not implemented"); }
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 moveL(const std::vector<double> &pose,
double speed = 0.25,
double acceleration = 1.2,
double jerk = 5.0,
const std::vector<double> &qd_max = std::vector<double>(7, 2.5),
bool asynchronous = false) {
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 Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase() {
return Eigen::Matrix<double, 6, 1>::Zero();
}
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"); }
@ -173,8 +179,10 @@ namespace cmvr::device{
{
toolFrame_ = toolFrame;
}
virtual std::string getToolFrame() const { return toolFrame_; }
virtual msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) = 0;
virtual msgs::Pose3d fk(bool is_tcp = true) = 0;
virtual std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) = 0;
protected:
int dof_{};

28
cmvr-es/devices/robot/humanoid_robot/include/humanoid_robot.h
View File

@ -86,10 +86,12 @@ namespace cmvr::device{
void moveJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d pose,double vel, double acc) override;
void moveJ_IK(const std::string &base_link, const std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override ;
void moveDeltaJ(const std::string &base_link, const std::string &ee_link, msgs::Pose3d delta_pose, double vel, double acc) override;
void moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel, double acc) override;
void moveL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d target_pose, double vel, double acc) override;
void moveDeltaL(const std::string &base_link, const std::string &ee_link,msgs::Pose3d delta_pose, double vel, double acc) override;
bool moveL(const std::vector<double> &pose,
double speed = 0.25,
double acceleration = 1.2,
double jerk = 5.0,
const std::vector<double> &qd_max = std::vector<double>(7, 2.5),
bool asynchronous = false) override;
void speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) override;
@ -99,7 +101,7 @@ namespace cmvr::device{
double time = 0.0,
cmvr::CartesianFrame frame = cmvr::CartesianFrame::Base) override;
void stopSpeedL() override;
Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase();
Eigen::Matrix<double, 6, 1> getSpeedLCommandTwistBase() override;
@ -124,6 +126,14 @@ namespace cmvr::device{
void ensureSpeedLWorkerStarted_();
void stopSpeedLWorker_();
void speedLWorkerLoop_();
bool executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input,
double v_tcp_max,
double a_tcp_max,
double j_tcp_max,
const std::vector<double>& qd_max,
double dt_real,
bool is_tcp,
cmvr::CartesianFrame input_frame);
@ -148,16 +158,10 @@ namespace cmvr::device{
static Eigen::Matrix3d eulerZYXToRotationMatrix(double rx, double ry, double rz);
static Eigen::Vector3d rotationMatrixToEulerZYX(const Eigen::Matrix3d &R);
msgs::Pose3d fk(const std::string &base_link, const std::string &ee_link) override;
msgs::Pose3d fk(bool is_tcp) override;
std::vector<double> ik(const std::string &base_link, const std::string &ee_link,msgs::Pose3d pose) override;
double calculateMoveTime(double distance, double vel, double acc);
void generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
double total_time, size_t num_points,
std::vector<double>& time_points,
std::vector<double>& distance_ratios);
private:
std::string id_;
int upd_freq_;

796
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot.cpp
View File

@ -899,181 +899,6 @@ void HumanoidRobot<DOF>::moveJ_IK(const std::string &base_link, const std::vecto
}
}
template<int DOF>
void HumanoidRobot<DOF>::moveL(std::string &base_link, std::vector<cmvr::ctrl::PoseTarget> &targets, double vel,
double acc) {
try {
stopSpeedLWorker_();
// 获取当前关节状态
Eigen::Vector<double, DOF> q_init;
auto q_map = getJointQ();
q_init << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
LOG(INFO) << "q_init: " << q_init;
m_state_->SetQ(q_init);
m_robot_->ComputeForwardKinematics(m_state_);
// 获取基座链接索引
auto base_idx = m_robot_->GetLinkIdx(base_link);
// 获取当前末端位姿 - 使用前向运动学计算
std::vector<Eigen::Matrix4d> current_poses;
for (const auto &target: targets) {
auto ee_idx = m_robot_->GetLinkIdx(target.link_name);
// 使用正向运动学计算当前位姿
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
current_poses.push_back(T);
// 打印当前末端执行器的 XYZ 和欧拉角
if (&target == &targets.front()) {
Eigen::Vector3d position = T.block<3, 1>(0, 3);
Eigen::Matrix3d rotation = T.block<3, 3>(0, 0);
Eigen::Vector3d euler = rotationMatrixToEulerZYX(rotation);
LOG(INFO) << "Starting point (Initial position): "
<< "X: " << position[0] << ", Y: " << position[1] << ", Z: " << position[2];
LOG(INFO) << "Starting orientation (Euler angles): "
<< "RX: " << euler[0] << ", RY: " << euler[1] << ", RZ: " << euler[2];
}
}
// 计算最大距离和插值点数
double max_distance = 0.0;
for (size_t i = 0; i < targets.size(); i++) {
Eigen::Vector3d current_pos = current_poses[i].block<3, 1>(0, 3);
Eigen::Vector3d target_pos = targets[i].T_target.block<3, 1>(0, 3);
double distance = (target_pos - current_pos).norm();
max_distance = std::max(max_distance, distance);
}
// 基于速度和距离计算插值点数
double move_time = max_distance / vel;
int num_points = static_cast<int>(move_time * 100); // 100Hz控制频率
// 存储所有插值点的关节角度
std::vector<Eigen::Vector<double, DOF> > joint_trajectory;
joint_trajectory.reserve(num_points + 1);
// 记录上一次成功的关节角度
Eigen::Vector<double, DOF> last_success_q = q_init;
// 预先计算所有插值点的逆运动学
for (int i = 0; i <= num_points; i++) {
if (flash_cmd_.load()) {
flash_cmd_.store(false);
rsm_.store(ROBOT_READY);
return;
}
double t = static_cast<double>(i) / num_points;
// 创建插值后的目标（只做位置插值，旋转保持不变）
std::vector<cmvr::ctrl::PoseTarget> interpolated_targets = targets;
for (size_t j = 0; j < targets.size(); j++) {
// 位置线性插值
Eigen::Vector3d current_pos = current_poses[j].block<3, 1>(0, 3);
Eigen::Vector3d target_pos = targets[j].T_target.block<3, 1>(0, 3);
Eigen::Vector3d interp_pos = current_pos + t * (target_pos - current_pos);
// 保持旋转不变
Eigen::Matrix3d current_rot_matrix = current_poses[j].block<3, 3>(0, 0);
interpolated_targets[j].T_target.setIdentity();
interpolated_targets[j].T_target.block<3, 3>(0, 0) = current_rot_matrix;
interpolated_targets[j].T_target.block<3, 1>(0, 3) = interp_pos;
}
// 求解逆运动学
Eigen::Vector<double, DOF> q_cmd;
bool ok = m_cctrl_->compute(m_state_, base_link, interpolated_targets, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd, 10000, 1e-6);
if (!ok) {
LOG(WARNING) << "IK failed at point " << i << ", using last successful configuration";
q_cmd = last_success_q;
} else {
last_success_q = q_cmd;
}
joint_trajectory.push_back(q_cmd);
// 获取当前末端执行器的位置 (通过正向运动学)
m_state_->SetQ(q_cmd);
m_robot_->ComputeForwardKinematics(m_state_);
// 获取当前末端执行器的位姿 (变换矩阵 T)
auto ee_idx = m_robot_->GetLinkIdx(targets[0].link_name);
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
// 从变换矩阵中提取 XYZ 坐标
Eigen::Vector3d end_effector_pos = T.block<3, 1>(0, 3);
// 打印 IK 解算出的 XYZ 位置
if (i % 10 == 0) {
// 每10个点打印一次，避免日志过多
LOG(INFO) << "IK solution at point " << i << " : "
<< "X: " << end_effector_pos[0] << ", Y: " << end_effector_pos[1] << ", Z: " << end_effector_pos
[2];
}
}
// 执行轨迹
for (int i = 0; i <= num_points; i++) {
if (flash_cmd_.load()) {
flash_cmd_.store(false);
break;
}
// 获取当前时间点的关节角度
Eigen::Vector<double, DOF> q_cmd = joint_trajectory[i];
// 发送关节命令
std::vector<JointPoint> joint_points{
{"R_SHOULDER_P", q_cmd[7]}, {"R_SHOULDER_R", q_cmd[8]},
{"R_SHOULDER_Y", q_cmd[9]}, {"R_ELBOW_R", q_cmd[10]},
{"R_WRIST_P", q_cmd[11]}, {"R_WRIST_Y", q_cmd[12]},
{"R_WRIST_R", q_cmd[13]}
};
// 设置每个关节的速度和位置
for (size_t j = 0; j < joint_points.size(); j++) {
auto &joint_point = joint_points[j];
auto motor = motor_manager_->getMotor(joint_point.joint_name);
if (motor != nullptr) {
motor->setQ(joint_point.rad);
}
}
// 等待一段时间，控制频率
std::this_thread::sleep_for(std::chrono::milliseconds(10));
}
// 等待最终位置到达 - 检查所有关节
bool completion = true;
do {
completion = true;
for (const auto &name: joint_names_) {
auto motor = motor_manager_->getMotor(name);
if (motor != nullptr && !motor->reachedTargetQ()) {
completion = false;
break;
}
}
if (flash_cmd_.load()) {
flash_cmd_.store(false);
return;
}
std::this_thread::sleep_for(std::chrono::milliseconds(2));
} while (!completion);
} catch (std::exception &e) {
throw std::runtime_error(e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::speedJ(std::string &joint_name, RobotJointIndexDirection dir, double vel, double acc) {
@ -1121,8 +946,7 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double dt) {
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motor->setQd(j.vel);
motor->setQ(j.rad);
motor->setTarget(j.rad,j.vel);
}
}
rsm_.store(ROBOT_READY);
@ -1137,8 +961,7 @@ void HumanoidRobot<DOF>::servoJ(std::vector<JointPoint> &joints, double vel, dou
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motor->setQd(vel);
motor->setQ(j.rad);
motor->setTarget(j.rad,vel);
}
}
}
@ -1383,36 +1206,95 @@ std::vector<double> HumanoidRobot<
template<int DOF>
cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const std::string &ee_link) {
cmvr::msgs::Pose3d pose;
static const std::vector<std::string> kRightArmJointNames = {
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
};
try {
// 获取当前关节角度
Eigen::Vector<double, DOF> q;
auto q_map = getJointQ(); // 类似 moveJ 中获取关节角度
q << q_map["L_SHOULDER_P"], q_map["L_SHOULDER_R"], q_map["L_SHOULDER_Y"], q_map["L_ELBOW_R"],
q_map["L_WRIST_P"], q_map["L_WRIST_Y"], q_map["L_WRIST_R"],
q_map["R_SHOULDER_P"], q_map["R_SHOULDER_R"], q_map["R_SHOULDER_Y"], q_map["R_ELBOW_R"],
q_map["R_WRIST_P"], q_map["R_WRIST_Y"], q_map["R_WRIST_R"];
if (!ik_solver_) {
throw std::runtime_error("ik_solver_ is not initialized");
}
LOG(INFO) << "q_current: " << q;
// 更新状态并计算前向运动学
m_state_->SetQ(q);
m_robot_->ComputeForwardKinematics(m_state_);
const auto q_map = getJointQ();
std::vector<double> q_current;
q_current.reserve(kRightArmJointNames.size());
for (const auto& joint_name : kRightArmJointNames) {
const auto it = q_map.find(joint_name);
if (it == q_map.end()) {
throw std::runtime_error("missing joint state for " + joint_name);
}
q_current.push_back(it->second);
}
// 获取基座和末端索引
auto base_idx = m_robot_->GetLinkIdx(base_link);
auto ee_idx = m_robot_->GetLinkIdx(ee_link);
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
if (!ik_solver_->fk(base_link, ee_link, q_current, T)) {
throw std::runtime_error("PinocchioDlsIKSolver::fk failed");
}
// 获取变换矩阵
Eigen::Matrix4d T = m_robot_->GetTransformation(m_state_, base_idx, ee_idx);
// 填充 Pose3d
pose.mutable_position()->set_x(T(0, 3));
pose.mutable_position()->set_y(T(1, 3));
pose.mutable_position()->set_z(T(2, 3));
// 将旋转矩阵转换为欧拉角
Eigen::Matrix3d R = T.block<3, 3>(0, 0);
Eigen::Vector3d euler = rotationMatrixToEulerZYX(R); // 你需要实现或已有此工具函数
const Eigen::Quaterniond q(R);
pose.mutable_quaternion()->set_w(q.w());
pose.mutable_quaternion()->set_x(q.x());
pose.mutable_quaternion()->set_y(q.y());
pose.mutable_quaternion()->set_z(q.z());
Eigen::Vector3d euler = rotationMatrixToEulerZYX(R);
pose.mutable_euler()->set_rx(euler(0));
pose.mutable_euler()->set_ry(euler(1));
pose.mutable_euler()->set_rz(euler(2));
} catch (const std::exception &e) {
throw std::runtime_error(std::string("FK计算失败: ") + e.what());
}
return pose;
}
template<int DOF>
cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(bool is_tcp) {
cmvr::msgs::Pose3d pose;
static const std::vector<std::string> kRightArmJointNames = {
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
};
try {
if (!ik_solver_) {
throw std::runtime_error("ik_solver_ is not initialized");
}
const auto q_map = getJointQ();
std::vector<double> q_current;
q_current.reserve(kRightArmJointNames.size());
for (const auto& joint_name : kRightArmJointNames) {
const auto it = q_map.find(joint_name);
if (it == q_map.end()) {
throw std::runtime_error("missing joint state for " + joint_name);
}
q_current.push_back(it->second);
}
Eigen::Matrix4d T = Eigen::Matrix4d::Identity();
if (!ik_solver_->fk(q_current, T, is_tcp)) {
throw std::runtime_error("PinocchioDlsIKSolver::fk failed");
}
pose.mutable_position()->set_x(T(0, 3));
pose.mutable_position()->set_y(T(1, 3));
pose.mutable_position()->set_z(T(2, 3));
const Eigen::Matrix3d R = T.block<3, 3>(0, 0);
const Eigen::Quaterniond q(R);
pose.mutable_quaternion()->set_w(q.w());
pose.mutable_quaternion()->set_x(q.x());
pose.mutable_quaternion()->set_y(q.y());
pose.mutable_quaternion()->set_z(q.z());
const Eigen::Vector3d euler = rotationMatrixToEulerZYX(R);
pose.mutable_euler()->set_rx(euler(0));
pose.mutable_euler()->set_ry(euler(1));
pose.mutable_euler()->set_rz(euler(2));
@ -1424,397 +1306,161 @@ cmvr::msgs::Pose3d HumanoidRobot<DOF>::fk(const std::string &base_link, const st
}
void printTrajectoryInfo(
const std::vector<Eigen::Matrix4d> &trajectory,
const std::vector<double> &times,
const std::vector<double> &velocities,
double total_distance) {
std::cout << "\n===================================== 轨迹详细信息 =====================================" << std::endl;
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
std::cout << "总运动时间: " << std::fixed << std::setprecision(3) << times.back() << "s" << std::endl;
std::cout << "轨迹点总数: " << trajectory.size() << " 个" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
std::cout << std::setw(4) << "序号" << " | "
<< std::setw(8) << "时间(s)" << " | "
<< std::setw(10) << "x(m)" << " | "
<< std::setw(10) << "y(m)" << " | "
<< std::setw(10) << "z(m)" << " | "
<< std::setw(12) << "速度(m/s)" << " | "
<< std::setw(16) << "到起点距离(m)" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
Eigen::Vector3d start_pos(trajectory[0](0, 3), trajectory[0](1, 3), trajectory[0](2, 3));
for (size_t idx = 0; idx < trajectory.size(); ++idx) {
const auto &T = trajectory[idx];
Eigen::Vector3d pos(T(0, 3), T(1, 3), T(2, 3));
double dist_from_start = (pos - start_pos).norm();
std::cout << std::setw(4) << idx << " | "
<< std::fixed << std::setprecision(3) << std::setw(8) << times[idx] << " | "
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.x() << " | "
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.y() << " | "
<< std::fixed << std::setprecision(6) << std::setw(10) << pos.z() << " | "
<< std::fixed << std::setprecision(6) << std::setw(12) << velocities[idx] << " | "
<< std::fixed << std::setprecision(6) << std::setw(16) << dist_from_start << std::endl;
}
std::cout << "=========================================================================================\n" <<
std::endl;
}
template<int DOF>
void HumanoidRobot<DOF>::moveDeltaL(const std::string &base_link, const std::string &ee_link,
msgs::Pose3d delta_pose, double vel, double acc) {
try {
Eigen::Vector<double, DOF> q_current_for_ik;
auto q_map_current = getJointQ();
q_current_for_ik << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"],
q_map_current["L_ELBOW_R"],
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"],
q_map_current["R_ELBOW_R"],
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
LOG(INFO) << "q_current_for_ik: " << q_current_for_ik;
// 1. 计算末端当前位姿（通过FK）
msgs::Pose3d current_pose = fk(base_link, ee_link);
LOG(INFO) << current_pose.mutable_position()->x() << " " << current_pose.mutable_position()->y() << " "
<< current_pose.mutable_position()->z() << " " << current_pose.mutable_euler()->rx() << " "
<< current_pose.mutable_euler()->ry() << " " << current_pose.mutable_euler()->rz();
// 2. 计算目标位姿 = 当前位姿 + 相对偏移（位置/姿态分别叠加）
msgs::Pose3d target_pose;
target_pose.mutable_position()->set_x(current_pose.position().x() + delta_pose.position().x());
target_pose.mutable_position()->set_y(current_pose.position().y() + delta_pose.position().y());
target_pose.mutable_position()->set_z(current_pose.position().z() + delta_pose.position().z());
target_pose.mutable_euler()->set_rx(current_pose.euler().rx() + delta_pose.euler().rx());
target_pose.mutable_euler()->set_ry(current_pose.euler().ry() + delta_pose.euler().ry());
target_pose.mutable_euler()->set_rz(current_pose.euler().rz() + delta_pose.euler().rz());
// 3. 调用moveL执行直线运动到目标位姿
moveL(base_link, ee_link, target_pose, vel, acc);
} catch (const std::exception &e) {
LOG(ERROR) << "moveDeltaL failed: " << e.what();
throw std::runtime_error(std::string("moveDeltaL error: ") + e.what());
}
}
template<int DOF>
void HumanoidRobot<DOF>::moveL(const std::string &base_link, const std::string &ee_link,
msgs::Pose3d target_pose, double vel, double acc) {
bool HumanoidRobot<DOF>::moveL(const std::vector<double> &pose,
double speed,
double acceleration,
double jerk,
const std::vector<double> &qd_max,
bool asynchronous) {
stopSpeedLWorker_();
if (vel <= 0 || acc <= 0) {
throw std::runtime_error("moveL: vel and acc must be positive");
if (asynchronous) {
LOG(ERROR) << "moveL: asynchronous=true is not supported yet";
return false;
}
if (!ik_solver_) {
LOG(ERROR) << "moveL: ik_solver_ is not initialized";
return false;
}
if (pose.size() != 6) {
LOG(ERROR) << "moveL: pose size must be 6 [x, y, z, rx, ry, rz], got " << pose.size();
return false;
}
if (speed <= 0.0 || acceleration <= 0.0) {
LOG(ERROR) << "moveL: speed and acceleration must be positive";
return false;
}
if (jerk <= 0.0) {
LOG(ERROR) << "moveL: jerk must be positive";
return false;
}
Eigen::Matrix4d target_pose_base = Eigen::Matrix4d::Identity();
target_pose_base.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(pose[3], pose[4], pose[5]);
target_pose_base(0, 3) = pose[0];
target_pose_base(1, 3) = pose[1];
target_pose_base(2, 3) = pose[2];
const double dt_real = 0.001;
return executeMoveLScurve_(target_pose_base,
speed,
acceleration,
jerk,
qd_max,
dt_real,
true,
cmvr::CartesianFrame::Tool);
}
template<int DOF>
bool HumanoidRobot<DOF>::executeMoveLScurve_(const Eigen::Matrix4d& target_pose_input,
double v_tcp_max,
double a_tcp_max,
double j_tcp_max,
const std::vector<double>& qd_max,
double dt_real,
bool is_tcp,
cmvr::CartesianFrame input_frame) {
static const std::vector<std::string> kRightArmJointNames = {
"R_SHOULDER_P", "R_SHOULDER_R", "R_SHOULDER_Y",
"R_ELBOW_R", "R_WRIST_P", "R_WRIST_Y", "R_WRIST_R"
};
if (!ik_solver_) {
LOG(ERROR) << "executeMoveLScurve_: ik_solver_ is not initialized";
return false;
}
if (v_tcp_max <= 0.0 || a_tcp_max <= 0.0 || j_tcp_max <= 0.0 || dt_real <= 0.0) {
LOG(ERROR) << "executeMoveLScurve_: invalid parameters, v=" << v_tcp_max
<< ", a=" << a_tcp_max
<< ", j=" << j_tcp_max
<< ", dt=" << dt_real;
return false;
}
std::lock_guard<std::mutex> lock(right_arm_mutex_);
if (is_right_arm_busy_.exchange(true)) {
LOG(ERROR) << "executeMoveLScurve_: right arm is busy";
return false;
}
struct BusyGuard {
std::atomic<bool>& busy_flag;
~BusyGuard() { busy_flag.store(false); }
} busy_guard{is_right_arm_busy_};
try {
const double CONTROL_PERIOD = 1.0 / 50.0; // 控制周期保持不变
msgs::Pose3d current_pose = fk(base_link, ee_link);
// 1. 初始化当前和目标位姿矩阵
Eigen::Matrix4d T_current = Eigen::Matrix4d::Identity();
T_current.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
current_pose.euler().rx(), current_pose.euler().ry(), current_pose.euler().rz()
);
T_current(0, 3) = current_pose.position().x();
T_current(1, 3) = current_pose.position().y();
T_current(2, 3) = current_pose.position().z();
Eigen::Matrix4d T_target = Eigen::Matrix4d::Identity();
T_target.block<3, 3>(0, 0) = eulerZYXToRotationMatrix(
target_pose.euler().rx(), target_pose.euler().ry(), target_pose.euler().rz()
);
T_target(0, 3) = target_pose.position().x();
T_target(1, 3) = target_pose.position().y();
T_target(2, 3) = target_pose.position().z();
// 保存起始姿态，确保整个运动过程中姿态保持不变
Eigen::Matrix3d start_orientation = T_current.block<3, 3>(0, 0);
// 2. 获取当前关节配置并验证目标可达性
Eigen::Vector<double, DOF> q_current;
auto q_map_current = getJointQ();
q_current << q_map_current["L_SHOULDER_P"], q_map_current["L_SHOULDER_R"], q_map_current["L_SHOULDER_Y"],
q_map_current["L_ELBOW_R"],
q_map_current["L_WRIST_P"], q_map_current["L_WRIST_Y"], q_map_current["L_WRIST_R"],
q_map_current["R_SHOULDER_P"], q_map_current["R_SHOULDER_R"], q_map_current["R_SHOULDER_Y"],
q_map_current["R_ELBOW_R"],
q_map_current["R_WRIST_P"], q_map_current["R_WRIST_Y"], q_map_current["R_WRIST_R"];
LOG(INFO) << "Current joint configuration: " << q_current;
m_state_->SetQ(q_current);
m_robot_->ComputeForwardKinematics(m_state_);
// 验证目标点可达性 - 使用起始姿态，确保姿态不变
cmvr::ctrl::PoseTarget target_ik_check;
target_ik_check.T_target = T_target;
target_ik_check.T_target.block<3, 3>(0, 0) = start_orientation; // 使用起始姿态
target_ik_check.w_posrot = 0.5;
target_ik_check.weight = 1.0;
target_ik_check.link_name = ee_link;
Eigen::Vector<double, DOF> q_cmd_check;
bool ik_solvable = m_cctrl_->compute(m_state_, base_link, {target_ik_check}, 0.002,
ctrl::CartesianController<DOF>::Mode::Position,
q_cmd_check, 10000, 1e-6);
if (!ik_solvable) {
throw std::runtime_error("moveL: Target pose is unreachable with constant orientation");
std::vector<double> q_start;
q_start.reserve(kRightArmJointNames.size());
const auto q_map = getJointQ();
for (const auto& joint_name : kRightArmJointNames) {
const auto it = q_map.find(joint_name);
if (it == q_map.end()) {
LOG(ERROR) << "executeMoveLScurve_: missing joint state for " << joint_name;
return false;
}
q_start.push_back(it->second);
}
// 3. 计算位置差值（保持姿态不变）
Eigen::Vector3d delta_pos = T_target.block<3, 1>(0, 3) - T_current.block<3, 1>(0, 3);
double total_distance = delta_pos.norm();
if (total_distance < 1e-6) {
LOG(INFO) << "moveL: Target is already reached";
return;
if (!qd_max.empty() && qd_max.size() != kRightArmJointNames.size()) {
LOG(ERROR) << "executeMoveLScurve_: qd_max size mismatch, expect "
<< kRightArmJointNames.size() << ", got " << qd_max.size();
return false;
}
// 4. 基于S曲线速度规划的时间规划
// 计算总时间和插值点数
double move_time = calculateMoveTime(total_distance, vel, acc);
size_t num_points = std::max(2ul, static_cast<size_t>(ceil(move_time / CONTROL_PERIOD)));
// 生成时间轴和距离比例
std::vector<double> time_points;
std::vector<double> distance_ratios;
generateSTrapezoidalProfile(total_distance, vel, acc, move_time, num_points,
time_points, distance_ratios);
LOG(INFO) << "moveL: Planning trajectory - points=" << num_points
<< ", total distance=" << total_distance << "m, move time=" << move_time << "s";
// 5. 生成轨迹点（位置线性插值，姿态保持不变）
std::vector<Eigen::Matrix4d> cartesian_trajectory;
for (size_t i = 0; i <= num_points; ++i) {
double s = distance_ratios[i]; // 使用S曲线规划的距离比例
Eigen::Matrix4d T_interp = Eigen::Matrix4d::Identity();
T_interp.block<3, 3>(0, 0) = start_orientation; // 保持起始姿态不变
// 仅位置按比例插值
T_interp(0, 3) = T_current(0, 3) + s * delta_pos.x();
T_interp(1, 3) = T_current(1, 3) + s * delta_pos.y();
T_interp(2, 3) = T_current(2, 3) + s * delta_pos.z();
cartesian_trajectory.push_back(T_interp);
std::vector<std::vector<double>> q_traj;
std::vector<std::vector<double>> qdot_traj;
std::vector<double> t_traj;
if (!ik_solver_->moveL_SCurveLocal(target_pose_input,
q_start,
q_traj,
t_traj,
dt_real,
v_tcp_max,
a_tcp_max,
j_tcp_max,
qd_max,
is_tcp,
input_frame,
&qdot_traj)) {
LOG(ERROR) << "executeMoveLScurve_: moveL_SCurveLocal failed";
return false;
}
if (q_traj.empty() || t_traj.size() != q_traj.size()) {
LOG(ERROR) << "executeMoveLScurve_: invalid trajectory output";
return false;
}
if (qdot_traj.size() != q_traj.size()) {
LOG(WARNING) << "executeMoveLScurve_: qdot_traj size mismatch, use zero fallback";
qdot_traj.assign(q_traj.size(), std::vector<double>(kRightArmJointNames.size(), 0.0));
}
if (q_traj.size() == 1) {
return true;
}
// 6. 预先计算所有轨迹点的关节位置
std::vector<Eigen::Vector<double, DOF> > joint_positions;
joint_positions.push_back(q_current); // 起始位置
// 预先计算所有关节位置
for (size_t i = 1; i < cartesian_trajectory.size(); ++i) {
const auto &T_interp = cartesian_trajectory[i];
// 构造当前目标
cmvr::ctrl::PoseTarget current_target;
current_target.T_target = T_interp;
current_target.link_name = ee_link;
current_target.w_posrot = 0.5;
current_target.weight = 1.0;
// 使用前一点的位置作为初始值求解IK
Eigen::Vector<double, DOF> q_next;
bool ok = m_cctrl_->compute(m_state_, base_link, {current_target}, CONTROL_PERIOD,
ctrl::CartesianController<DOF>::Mode::Position,
q_next, 10000, 1e-6);
if (!ok) {
throw std::runtime_error("Pre-computation IK failed");
// LOG(WARNING) << "Pre-computation IK failed at point " << i << ", using previous point";
// q_next = joint_positions.back();
auto next_deadline = std::chrono::steady_clock::now();
for (size_t i = 1; i < q_traj.size(); ++i) {
const double dt_segment = std::max(1e-4, t_traj[i] - t_traj[i - 1]);
std::vector<JointPoint> joint_cmd;
joint_cmd.reserve(kRightArmJointNames.size());
for (size_t j = 0; j < kRightArmJointNames.size(); ++j) {
const double qd_abs = (j < qdot_traj[i].size()) ? std::abs(qdot_traj[i][j]) : 0.0;
joint_cmd.emplace_back(kRightArmJointNames[j], q_traj[i][j], qd_abs);
}
joint_positions.push_back(q_next);
servoJ(joint_cmd, dt_segment);
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment));
std::this_thread::sleep_until(next_deadline);
}
// 7. 计算每个点的关节速度
std::vector<Eigen::Vector<double, DOF> > joint_velocities;
joint_velocities.push_back(Eigen::Vector<double, DOF>::Zero()); // 起始速度为零
for (size_t i = 1; i < joint_positions.size(); ++i) {
double dt = time_points[i] - time_points[i - 1];
Eigen::Vector<double, DOF> vel = (joint_positions[i] - joint_positions[i - 1]) / dt;
joint_velocities.push_back(vel);
}
// 8. 打印轨迹信息
std::cout << "\n===================================== 轨迹规划信息 =====================================" <<
std::endl;
std::cout << "轨迹点总数: " << cartesian_trajectory.size() << " 个" << std::endl;
std::cout << "总路径长度: " << std::fixed << std::setprecision(6) << total_distance << "m" << std::endl;
std::cout << "最大速度: " << std::fixed << std::setprecision(6) << vel << "m/s" << std::endl;
std::cout << "加速度: " << std::fixed << std::setprecision(6) << acc << "m/s²" << std::endl;
std::cout << "总时间: " << std::fixed << std::setprecision(6) << move_time << "s" << std::endl;
std::cout << "起点位置: (x=" << T_current(0, 3) << ", y=" << T_current(1, 3) << ", z=" << T_current(2, 3) << ")" <<
std::endl;
std::cout << "终点位置: (x=" << T_target(0, 3) << ", y=" << T_target(1, 3) << ", z=" << T_target(2, 3) << ")" <<
std::endl;
std::cout << "保持姿态不变" << std::endl;
std::cout << "-----------------------------------------------------------------------------------------" <<
std::endl;
// 9. 执行轨迹
auto loop_start_time = std::chrono::high_resolution_clock::now();
for (size_t i = 0; i < cartesian_trajectory.size(); ++i) {
// 获取当前点的关节位置和速度
Eigen::Vector<double, DOF> q_cmd = joint_positions[i];
Eigen::Vector<double, DOF> q_vel = joint_velocities[i];
// 更新状态
m_state_->SetQ(q_cmd);
m_robot_->ComputeForwardKinematics(m_state_);
// 发送关节命令 - 为每个电机单独设置位置和速度
std::vector<JointPoint> joint_command;
for (size_t j = 0; j < DOF; ++j) {
JointPoint jp;
jp.joint_name = joint_names_[j];
jp.rad = q_cmd[j];
jp.vel = std::abs(q_vel[j]); // 使用计算出的关节速度
joint_command.push_back(jp);
}
// 计算当前点应该执行的时间
double expected_time = time_points[i];
// servoJ(joint_command, vel, expected_time);
for (const auto &j: joint_command) {
auto motor = motor_manager_->getMotor(j.joint_name);
if (motor != nullptr) {
if (motor->getMode() != msgs::RUN_MODE_CYCLIC_SYNC_POSITION) {
motor->setMode(msgs::RUN_MODE_CYCLIC_SYNC_POSITION);
}
motor->setQd(j.vel);
motor->setQ(j.rad);
}
}
// 检查中断
if (flash_cmd_.load()) {
flash_cmd_.store(false);
LOG(INFO) << "moveL: Interrupted by external command";
return;
}
// 控制时间节奏 - 使用精确的时间规划
auto expected_time_point = loop_start_time + std::chrono::nanoseconds(
static_cast<long long>(expected_time * 1e9)
);
auto now = std::chrono::high_resolution_clock::now();
if (now < expected_time_point) {
std::this_thread::sleep_until(expected_time_point);
} else {
LOG(WARNING) << "moveL: Behind schedule at point " << i
<< " by " << std::chrono::duration_cast<std::chrono::milliseconds>(now - expected_time_point).
count() << "ms";
}
}
// 最终状态更新
m_state_->SetQ(joint_positions.back());
m_robot_->ComputeForwardKinematics(m_state_);
rsm_.store(ROBOT_READY);
LOG(INFO) << "moveL: Trajectory completed successfully";
} catch (const std::exception &e) {
LOG(ERROR) << "moveL failed: " << e.what();
rsm_.store(ROBOT_ERROR);
throw std::runtime_error(std::string("moveL error: ") + e.what());
}
}
// 辅助函数：计算运动时间
template<int DOF>
double HumanoidRobot<DOF>::calculateMoveTime(double distance, double vel, double acc) {
// 计算加速和减速所需的时间和距离
double acc_time = vel / acc;
double acc_distance = 0.5 * acc * acc_time * acc_time;
// 如果加速距离超过总距离的一半，需要调整最大速度
if (2 * acc_distance > distance) {
// 三角形速度曲线：加速然后直接减速
double max_reachable_vel = std::sqrt(acc * distance);
return 2 * max_reachable_vel / acc;
} else {
// 梯形速度曲线：加速-匀速-减速
double constant_distance = distance - 2 * acc_distance;
double constant_time = constant_distance / vel;
return 2 * acc_time + constant_time;
}
}
// 辅助函数：生成S曲线轨迹规划
template<int DOF>
void HumanoidRobot<DOF>::generateSTrapezoidalProfile(double total_distance, double max_vel, double max_acc,
double total_time, size_t num_points,
std::vector<double> &time_points,
std::vector<double> &distance_ratios) {
time_points.clear();
distance_ratios.clear();
// 计算加速和减速阶段的时间
double acc_time = max_vel / max_acc;
double acc_distance = 0.5 * max_acc * acc_time * acc_time;
// 确定实际的速度曲线形状
if (2 * acc_distance > total_distance) {
// 三角形速度曲线
double actual_max_vel = std::sqrt(max_acc * total_distance);
acc_time = actual_max_vel / max_acc;
acc_distance = 0.5 * max_acc * acc_time * acc_time;
double dec_time = acc_time;
// 生成时间点和距离比例
for (size_t i = 0; i <= num_points; ++i) {
double t = static_cast<double>(i) / num_points * total_time;
time_points.push_back(t);
if (t <= acc_time) {
// 加速阶段
double s = 0.5 * max_acc * t * t;
distance_ratios.push_back(s / total_distance);
} else {
// 减速阶段
double dec_start_time = total_time - dec_time;
double dec_elapsed = t - dec_start_time;
double s = acc_distance + actual_max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed;
distance_ratios.push_back(s / total_distance);
}
}
} else {
// 梯形速度曲线
double constant_time = (total_distance - 2 * acc_distance) / max_vel;
double dec_time = acc_time;
// 生成时间点和距离比例
for (size_t i = 0; i <= num_points; ++i) {
double t = static_cast<double>(i) / num_points * total_time;
time_points.push_back(t);
if (t <= acc_time) {
// 加速阶段
double s = 0.5 * max_acc * t * t;
distance_ratios.push_back(s / total_distance);
} else if (t <= acc_time + constant_time) {
// 匀速阶段
double s = acc_distance + max_vel * (t - acc_time);
distance_ratios.push_back(s / total_distance);
} else {
// 减速阶段
double dec_start_time = acc_time + constant_time;
double dec_elapsed = t - dec_start_time;
double s = acc_distance + max_vel * constant_time +
max_vel * dec_elapsed - 0.5 * max_acc * dec_elapsed * dec_elapsed;
distance_ratios.push_back(s / total_distance);
}
}
LOG(INFO) << "executeMoveLScurve_: completed, samples=" << q_traj.size()
<< ", duration_s=" << t_traj.back();
return true;
} catch (const std::exception& e) {
LOG(ERROR) << "executeMoveLScurve_ failed: " << e.what();
return false;
}
}

254
cmvr-es/devices/robot/humanoid_robot/src/humanoid_robot_test.cpp
View File

@ -81,6 +81,23 @@ std::unordered_map<std::string, double> makeRightArmQMap() {
return q_map;
}
bool readRightArmQ(const std::shared_ptr<AbstractRobot>& robot, std::vector<double>& q_now) {
if (!robot) {
return false;
}
auto q_map_now = makeRightArmQMap();
robot->getJointQ(q_map_now);
q_now.assign(kRightArmJointNames.size(), 0.0);
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;
}
} // namespace
@ -210,6 +227,243 @@ TEST(HumanoidRobotTest, speedLSmokeTest) {
// show(fig);
}
TEST(HumanoidRobotTest, moveLSCurveLocalRealRobot) {
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_abs = dmgr.getDevice<AbstractRobot>("hc01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
std::vector<JointPoint> 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.1},
};
robot->moveJ(init_cmd, 1.0, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
struct ReturnHomeGuard {
std::shared_ptr<HumanoidRobot<14>> robot;
std::vector<JointPoint> home;
~ReturnHomeGuard() {
if (!robot) {
return;
}
try {
robot->moveJ(home, 1.0, 2.0);
} catch (...) {
}
}
} guard{robot, init_cmd};
std::vector<double> q_start;
ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state";
cmvr::PinocchioDlsIKSolver solver(
"/home/lgv/cmvr/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";
Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity();
const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool;
target_pose_input(1, 3) = -0.08;
const double dt_gen = 0.001;
const double v_tcp = 0.08;
const double a_tcp = 5;
const double j_tcp = 5;
const bool is_tcp = true;
std::vector<double> qd_max(7, 3.0);
std::vector<std::vector<double>> q_traj;
std::vector<std::vector<double>> qdot_traj;
std::vector<double> t_traj;
ASSERT_TRUE(solver.moveL_SCurveLocal(target_pose_input,
q_start,
q_traj,
t_traj,
dt_gen,
v_tcp,
a_tcp,
j_tcp,
qd_max,
is_tcp,
input_frame,
&qdot_traj))
<< "moveL_SCurveLocal failed";
ASSERT_EQ(q_traj.size(), t_traj.size()) << "q_traj and t_traj size mismatch";
ASSERT_EQ(q_traj.size(), qdot_traj.size()) << "q_traj and qdot_traj size mismatch";
ASSERT_FALSE(q_traj.empty()) << "Generated trajectory is empty";
LOG(INFO) << "moveLSCurveLocalRealRobot generated samples=" << q_traj.size()
<< ", duration_s=" << (t_traj.empty() ? 0.0 : t_traj.back())
<< ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base")
<< ", delta_xyz=[" << target_pose_input(0, 3) << ", "
<< target_pose_input(1, 3) << ", "
<< target_pose_input(2, 3) << "]";
auto next_deadline = std::chrono::steady_clock::now();
for (size_t i = 1; i < q_traj.size(); ++i) {
const double dt_segment = std::max(0.0, t_traj[i] - t_traj[i - 1]);
std::vector<double> qd_abs = qdot_traj[i];
for (double& v : qd_abs) {
v = std::abs(v);
}
auto joint_cmd = buildRightArmJointCmdWithVel(q_traj[i], qd_abs, 0.05);
ASSERT_NO_THROW(robot->servoJ(joint_cmd, dt_segment));
next_deadline += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(dt_segment));
std::this_thread::sleep_until(next_deadline);
}
std::this_thread::sleep_for(std::chrono::milliseconds(500));
std::vector<double> q_end;
ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end";
Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state";
Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity();
ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state";
Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity();
ASSERT_TRUE(solver.fk(q_traj.back(), T_plan_end, is_tcp)) << "Failed to compute FK for planned end joint state";
const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0);
const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0);
const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0);
const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3);
const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3);
const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan;
const double rot_err_start_to_actual_rad =
Eigen::AngleAxisd(R_start.transpose() * R_end).angle();
const double rot_err_plan_to_actual_rad =
Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle();
LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", "
<< delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]";
LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", "
<< delta_tcp.y() << ", " << delta_tcp.z() << "]";
LOG(INFO) << "moveLSCurveLocalRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", "
<< delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]";
LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_start_to_actual_deg="
<< rot_err_start_to_actual_rad * 180.0 / M_PI;
LOG(INFO) << "moveLSCurveLocalRealRobot rot_err_plan_to_actual_deg="
<< rot_err_plan_to_actual_rad * 180.0 / M_PI;
}
TEST(HumanoidRobotTest, moveLRealRobot) {
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_abs = dmgr.getDevice<AbstractRobot>("hc01");
auto robot = std::dynamic_pointer_cast<HumanoidRobot<14>>(robot_abs);
ASSERT_NE(robot, nullptr) << "hc01 is not HumanoidRobot<14>";
std::vector<JointPoint> 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.1},
};
robot->moveJ(init_cmd, 1.0, 2.0);
std::this_thread::sleep_for(std::chrono::milliseconds(1000));
std::vector<double> q_start;
ASSERT_TRUE(readRightArmQ(robot_abs, q_start)) << "Failed to read right arm joint state";
cmvr::PinocchioDlsIKSolver solver(
"/home/lgv/cmvr/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";
Eigen::Matrix4d target_pose_input = Eigen::Matrix4d::Identity();
const cmvr::CartesianFrame input_frame = cmvr::CartesianFrame::Tool;
target_pose_input(1, 3) = -0.08;
const double v_tcp = 0.08;
const double a_tcp = 5;
const double j_tcp = 5;
const bool is_tcp = true;
const std::vector<double> pose_cmd = {
target_pose_input(0, 3),
target_pose_input(1, 3),
target_pose_input(2, 3),
0.0,
0.0,
0.0
};
LOG(INFO) << "moveLRealRobot command"
<< ", frame=" << (input_frame == cmvr::CartesianFrame::Tool ? "Tool" : "Base")
<< ", delta_xyz=[" << target_pose_input(0, 3) << ", "
<< target_pose_input(1, 3) << ", "
<< target_pose_input(2, 3) << "]";
ASSERT_TRUE(robot->moveL(pose_cmd, v_tcp, a_tcp, j_tcp)) << "robot->moveL failed";
std::this_thread::sleep_for(std::chrono::milliseconds(500));
std::vector<double> q_end;
ASSERT_TRUE(readRightArmQ(robot_abs, q_end)) << "Failed to read right arm joint state at end";
Eigen::Matrix4d T_end = Eigen::Matrix4d::Identity();
ASSERT_TRUE(solver.fk(q_end, T_end, is_tcp)) << "Failed to compute FK for end joint state";
Eigen::Matrix4d T_start = Eigen::Matrix4d::Identity();
ASSERT_TRUE(solver.fk(q_start, T_start, is_tcp)) << "Failed to compute FK for start joint state";
Eigen::Matrix4d T_plan_end = Eigen::Matrix4d::Identity();
T_plan_end = T_start;
if (input_frame == cmvr::CartesianFrame::Tool) {
T_plan_end.block<3,1>(0,3) = (T_start * target_pose_input).block<3,1>(0,3);
} else {
T_plan_end.block<3,1>(0,3) = target_pose_input.block<3,1>(0,3);
}
const Eigen::Matrix3d R_start = T_start.block<3,3>(0, 0);
const Eigen::Matrix3d R_plan_end = T_plan_end.block<3,3>(0, 0);
const Eigen::Matrix3d R_end = T_end.block<3,3>(0, 0);
const Eigen::Vector3d delta_tcp_plan = T_plan_end.block<3,1>(0,3) - T_start.block<3,1>(0,3);
const Eigen::Vector3d delta_tcp = T_end.block<3,1>(0,3) - T_start.block<3,1>(0,3);
const Eigen::Vector3d delta_tcp_error = delta_tcp - delta_tcp_plan;
const double rot_err_start_to_actual_rad =
Eigen::AngleAxisd(R_start.transpose() * R_end).angle();
const double rot_err_plan_to_actual_rad =
Eigen::AngleAxisd(R_plan_end.transpose() * R_end).angle();
LOG(INFO) << "moveLRealRobot tcp_delta_plan=[" << delta_tcp_plan.x() << ", "
<< delta_tcp_plan.y() << ", " << delta_tcp_plan.z() << "]";
LOG(INFO) << "moveLRealRobot tcp_delta_actual=[" << delta_tcp.x() << ", "
<< delta_tcp.y() << ", " << delta_tcp.z() << "]";
LOG(INFO) << "moveLRealRobot tcp_delta_error=[" << delta_tcp_error.x() << ", "
<< delta_tcp_error.y() << ", " << delta_tcp_error.z() << "]";
LOG(INFO) << "moveLRealRobot rot_err_start_to_actual_deg="
<< rot_err_start_to_actual_rad * 180.0 / M_PI;
LOG(INFO) << "moveLRealRobot rot_err_plan_to_actual_deg="
<< rot_err_plan_to_actual_rad * 180.0 / M_PI;
}
TEST(HumanoidRobotTest,IBVSWithRealRobot) {
const XmlNode config("/home/lgv/cmvr/cmvr-es/cmvr-es/common/config/cabin_robot.xml");

10
cmvr-es/ik_solver/include/pinocchio_dls_ik_solver.h
View File

@ -37,7 +37,7 @@ public:
double angular_acceleration_max{3.0};
double angular_jerk_max{15.0};
std::vector<double> joint_velocity_max; ///< size=chain_v_dof_，为空则仅用 URDF limit
std::vector<double> joint_velocity_max = std::vector<double>(7, 2.5); ///< size=chain_v_dof_，为空则仅用 URDF limit
std::vector<double> joint_acceleration_max; ///< size=chain_v_dof_，为空则不做 joint accel 限制
bool enable_joint_soft_limit_velocity{true};
double joint_soft_limit_margin{0.05};
@ -72,7 +72,7 @@ public:
const Eigen::Matrix4d& target_pose,
std::vector<double>& joints_angle);
bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base,
bool moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input,
const std::vector<double>& q_start,
std::vector<std::vector<double>>& q_traj,
std::vector<double>& t_traj,
@ -80,8 +80,10 @@ public:
double v_tcp_max,
double a_tcp_max,
double j_tcp_max,
const std::vector<double>& qd_max,
bool is_tcp = true);
const std::vector<double>& qd_max, // empty => use URDF joint velocity limits only
bool is_tcp = true,
CartesianFrame input_frame = CartesianFrame::Base,
std::vector<std::vector<double>>* qdot_traj = nullptr);
bool ik(const std::string& base_link,
const std::string& ee_link,

116
cmvr-es/ik_solver/src/pinocchio_dls_ik_solver.cpp
View File

@ -441,7 +441,7 @@ bool PinocchioDlsIKSolver::ik(const std::string& base_link,
// ============================================================================
// MoveL with tau time-scaling
// ============================================================================
bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_base,
bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_input,
const std::vector<double>& q_start,
std::vector<std::vector<double>>& q_traj,
std::vector<double>& t_traj,
@ -450,11 +450,13 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
double a_tcp_max,
double j_tcp_max,
const std::vector<double>& qd_max,
bool is_tcp)
bool is_tcp,
CartesianFrame input_frame,
std::vector<std::vector<double>>* qdot_traj)
{
if (!initialized_) return false;
if ((int)q_start.size() != chain_dof_) return false;
if ((int)qd_max.size() != chain_dof_) return false;
if (!qd_max.empty() && (int)qd_max.size() != chain_dof_) return false;
dt_real = std::max(dt_real, 1e-4);
@ -467,13 +469,28 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
const Eigen::Vector3d p0 = T0_base.block<3,1>(0,3);
const Eigen::Matrix3d R_fixed = T0_base.block<3,3>(0,0);
// moveL_SCurveLocal 当前只对目标平移做直线规划，姿态仍保持起始末端姿态。
Eigen::Matrix4d target_pose_base = target_pose_input;
if (input_frame == CartesianFrame::Tool) {
target_pose_base = T0_base * target_pose_input;
}
const Eigen::Vector3d pg = target_pose_base.block<3,1>(0,3);
const Eigen::Vector3d dp = pg - p0;
const double L = dp.norm();
if (L < 1e-9) { q_traj = {q_start}; t_traj = {0.0}; return true; }
if (L < 1e-9) {
q_traj = {q_start};
t_traj = {0.0};
if (qdot_traj) {
qdot_traj->assign(1, std::vector<double>(static_cast<size_t>(chain_dof_), 0.0));
}
return true;
}
const Eigen::Vector3d dir_base = dp / L;
std::cerr << "dp(base)= " << dp.transpose() << "\n";
std::cerr << "dp(base)= " << dp.transpose()
<< " frame=" << (input_frame == CartesianFrame::Tool ? "tool" : "base")
<< "\n";
SCurve curve(v_tcp_max, a_tcp_max, j_tcp_max);
const SCurveProfile prof = curve.calculateProfile(0.0, L);
@ -488,7 +505,22 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
const double eps_dq = 1e-9;
Eigen::VectorXd qd_lim(chain_dof_);
for (int i = 0; i < chain_dof_; ++i) qd_lim[i] = std::max(1e-6, std::abs(qd_max[i]));
if (qd_max.empty()) {
for (int i = 0; i < chain_dof_; ++i) {
double limit_i = std::numeric_limits<double>::infinity();
if (joint_vel_limits_.size() == chain_dof_) {
const double urdf_limit = std::abs(joint_vel_limits_[i]);
if (std::isfinite(urdf_limit) && urdf_limit > 0.0) {
limit_i = urdf_limit;
}
}
qd_lim[i] = limit_i;
}
} else {
for (int i = 0; i < chain_dof_; ++i) {
qd_lim[i] = std::max(1e-6, std::abs(qd_max[i]));
}
}
Eigen::VectorXd q_ref = Eigen::Map<const Eigen::VectorXd>(q_start.data(), chain_dof_);
Eigen::VectorXd q_chain = Eigen::Map<const Eigen::VectorXd>(q_start.data(), chain_dof_);
@ -502,11 +534,11 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
t_traj.reserve((size_t)(prof.total_time / dt_real) + 10);
q_traj.push_back(q_start);
t_traj.push_back(0.0);
const double log_period = 0.05;
double next_log_t = 0.0;
const double sigma_min_th = 1e-4;
const double cond_th = 1e4;
if (qdot_traj) {
qdot_traj->clear();
qdot_traj->reserve((size_t)(prof.total_time / dt_real) + 10);
qdot_traj->push_back(std::vector<double>(static_cast<size_t>(chain_dof_), 0.0));
}
double gamma_prev = 1.0;
const double gamma_tau = 0.03;
@ -622,55 +654,35 @@ bool PinocchioDlsIKSolver::moveL_SCurveLocal(const Eigen::Matrix4d& target_pose_
return false;
}
q_chain += (gamma_tot * dq_raw) * step_real;
q_chain = q_chain.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_);
const Eigen::VectorXd qdot_cmd = gamma_tot * dq_raw;
const Eigen::VectorXd q_before_clamp = q_chain + qdot_cmd * step_real;
const Eigen::VectorXd q_after_clamp =
q_before_clamp.cwiseMax(joint_pos_lower_limits_).cwiseMin(joint_pos_upper_limits_);
for (int i = 0; i < chain_dof_; ++i) {
if (std::abs(q_after_clamp[i] - q_before_clamp[i]) > 1e-12) {
std::cerr << "[moveL_SCurveLocal] clamp triggered: step=" << q_traj.size()
<< ", joint_index=" << i
<< ", before=" << q_before_clamp[i]
<< ", after=" << q_after_clamp[i]
<< std::endl;
}
}
q_chain = q_after_clamp;
tau = std::min(prof.total_time, tau + gamma_tot * step_real);
t += step_real;
if (t >= next_log_t) {
next_log_t += log_period;
bool near_limit = false;
int near_cnt = 0;
for (int i = 0; i < chain_dof_; ++i) {
const bool nl = (joint_pos_upper_limits_[i] - q_chain[i] < 1e-8) ||
(q_chain[i] - joint_pos_lower_limits_[i] < 1e-8);
if (nl) { near_limit = true; near_cnt++; }
}
std::cerr
<< "[MoveL ABC] t=" << t
<< " tau=" << tau << "/" << prof.total_time
<< " s=" << s << "/" << L
<< " sd=" << sd
<< " gamma_speed=" << gamma_speed
<< " gamma_lim=" << gamma_lim
<< " gamma_tot=" << gamma_tot
<< " near_limit=" << (near_limit?1:0)
<< " near_cnt=" << near_cnt
<< " |e_perp|=" << e_perp.norm()
<< " sigma_min=" << sigma_min
<< " cond=" << cond
<< " w_end=" << w_end
<< "\n";
if (sigma_min < sigma_min_th || cond > cond_th) {
std::cerr << " [C] near singular! sigma_min=" << sigma_min
<< " cond=" << cond << "\n";
}
if (gamma_lim < 0.999) {
std::cerr << " [A2] limit scaling gamma_lim=" << gamma_lim << "\n";
}
if (gamma_speed < 0.999) {
std::cerr << " [B] speed scaling gamma_speed=" << gamma_speed << "\n";
}
}
std::vector<double> q_out(chain_dof_);
Eigen::Map<Eigen::VectorXd>(q_out.data(), chain_dof_) = q_chain;
q_traj.push_back(std::move(q_out));
t_traj.push_back(t);
if (qdot_traj) {
std::vector<double> qdot_out(static_cast<size_t>(chain_dof_));
Eigen::Map<Eigen::VectorXd>(qdot_out.data(), chain_dof_) = qdot_cmd;
qdot_traj->push_back(std::move(qdot_out));
}
}
return !q_traj.empty();

2
cmvr-es/service/grpc/src/grpc_camera_service.cpp
View File

@ -89,7 +89,7 @@ grpc::Status gRPCCameraServiceImpl::GetRGBImage(grpc::ServerContext* context,
{
try {
string dev_id = request->header().device_id();
// LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
LOG(INFO) << "[gRPCCameraServiceImpl] (GetRGBImage): id=" << dev_id;
cv::Mat image;
const auto dev = dmgr_.getDevice<AbstractCamera>(dev_id);
Rs2Intrinsics intrinsics = {0};

356
cmvr-es/service/grpc/src/grpc_dexhand_service.cpp
View File

@ -3,6 +3,16 @@
//
#include "../include/grpc_dexhand_service.h"
#include <cmath>
#include <chrono>
#include <memory>
#include <stdexcept>
#include <thread>
#include <vector>
#include "devices/dexhand/rh56dftp_dexhand/include/rh56dftp_dexhand.h"
using namespace std;
using namespace cmvr::service;
using namespace cmvr::device;
@ -12,6 +22,147 @@ using namespace cmvr::device;
#define DEXHAND_MAX_FORCE 3000
#define DEXHAND_MAX_SPEED 1000
namespace {
constexpr int kDexHandDofCount = 6;
cmvr::api::SensorData::FingerType toProtoFingerType(const AbstractDexHand::FingerType finger) {
switch (finger) {
case AbstractDexHand::FingerType::PINKY:
return cmvr::api::SensorData::PINKY;
case AbstractDexHand::FingerType::RING:
return cmvr::api::SensorData::RING;
case AbstractDexHand::FingerType::MIDDLE:
return cmvr::api::SensorData::MIDDLE_FINGER;
case AbstractDexHand::FingerType::INDEX:
return cmvr::api::SensorData::INDEX;
case AbstractDexHand::FingerType::THUMB:
return cmvr::api::SensorData::THUMB;
case AbstractDexHand::FingerType::PALM:
return cmvr::api::SensorData::PALM;
}
return cmvr::api::SensorData::PINKY;
}
cmvr::api::SensorData::PartType toProtoPartType(const AbstractDexHand::TactileRegion region) {
switch (region) {
case AbstractDexHand::TactileRegion::TIP:
return cmvr::api::SensorData::TIP;
case AbstractDexHand::TactileRegion::FINGER:
return cmvr::api::SensorData::FINGER;
case AbstractDexHand::TactileRegion::PAD:
return cmvr::api::SensorData::PAD;
case AbstractDexHand::TactileRegion::THUMB_MIDDLE:
return cmvr::api::SensorData::THUMB_MIDDLE;
case AbstractDexHand::TactileRegion::PALM_PAD:
return cmvr::api::SensorData::PALM_PAD;
}
return cmvr::api::SensorData::TIP;
}
void fillSensorData(const AbstractDexHand::TactileRegionData& tactile_data,
cmvr::api::SensorData* sensor_data) {
sensor_data->set_rows(tactile_data.view.rows);
sensor_data->set_cols(tactile_data.view.cols);
sensor_data->set_finger_type(toProtoFingerType(tactile_data.finger));
sensor_data->set_part_type(toProtoPartType(tactile_data.region));
sensor_data->set_sensor_name(tactile_data.name == nullptr ? "" : tactile_data.name);
for (int row = 0; row < tactile_data.view.rows; ++row) {
auto* row_data = sensor_data->add_data();
const auto* values = tactile_data.view.rowData(row);
for (int col = 0; col < tactile_data.view.cols; ++col) {
row_data->add_values(static_cast<int32_t>(values[col]));
}
}
}
template <typename ResponseT>
void appendSensorData(const std::vector<AbstractDexHand::TactileRegionData>& tactile_regions,
ResponseT* response) {
for (const auto& tactile_region : tactile_regions) {
if (!tactile_region.valid()) {
continue;
}
fillSensorData(tactile_region, response->add_sensor());
}
}
template <typename FreedomCollection>
void applyFreedomValues(const FreedomCollection& freedoms,
const int scale,
std::vector<int>& targets) {
for (const auto& freedom : freedoms) {
if (freedom.id() < 0 || freedom.id() >= static_cast<int>(targets.size())) {
throw std::invalid_argument("Invalid dexhand DOF id: " + std::to_string(freedom.id()));
}
if (!std::isfinite(freedom.value())) {
throw std::invalid_argument("Invalid dexhand command value: not finite.");
}
targets[static_cast<size_t>(freedom.id())] = static_cast<int>(freedom.value() * scale);
}
}
std::vector<int> readCurrentAngles(const std::shared_ptr<AbstractDexHand>& dev) {
DexHandState state{};
dev->getState(state);
std::vector<int> current_angles(static_cast<size_t>(kDexHandDofCount), 0);
for (int i = 0; i < kDexHandDofCount; ++i) {
current_angles[static_cast<size_t>(i)] = state.hands[i].angle;
}
return current_angles;
}
bool respondUnsupportedForRh56(const std::shared_ptr<AbstractDexHand>& dev,
const char* rpc_name,
const char* hint,
cmvr::api::CommandHeader_Feedback* header) {
if (std::dynamic_pointer_cast<RH56DFTPDexhand>(dev) == nullptr) {
return false;
}
header->set_success(false);
header->set_error_message(std::string(rpc_name) + " is not supported by RH56DFTPDexhand. " + hint);
setCurrentTimestamp(header->mutable_timestamp());
return true;
}
std::vector<AbstractDexHand::TactileRegionKey> buildRh56AllTactileRegions() {
using FingerType = AbstractDexHand::FingerType;
using TactileRegion = AbstractDexHand::TactileRegion;
return {
{FingerType::PINKY, TactileRegion::TIP},
{FingerType::PINKY, TactileRegion::FINGER},
{FingerType::PINKY, TactileRegion::PAD},
{FingerType::RING, TactileRegion::TIP},
{FingerType::RING, TactileRegion::FINGER},
{FingerType::RING, TactileRegion::PAD},
{FingerType::MIDDLE, TactileRegion::TIP},
{FingerType::MIDDLE, TactileRegion::FINGER},
{FingerType::MIDDLE, TactileRegion::PAD},
{FingerType::INDEX, TactileRegion::TIP},
{FingerType::INDEX, TactileRegion::FINGER},
{FingerType::INDEX, TactileRegion::PAD},
{FingerType::THUMB, TactileRegion::TIP},
{FingerType::THUMB, TactileRegion::FINGER},
{FingerType::THUMB, TactileRegion::THUMB_MIDDLE},
{FingerType::THUMB, TactileRegion::PAD},
{FingerType::PALM, TactileRegion::PALM_PAD}
};
}
void maybeConfigureRh56FullTactilePolling(const std::shared_ptr<AbstractDexHand>& dev) {
auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev);
if (!rh56) {
return;
}
rh56->setTactilePollingRegions(buildRh56AllTactileRegions());
}
} // namespace
gRPCDexHandServiceImpl::gRPCDexHandServiceImpl(): dmgr_(DeviceManager::getInstance()) {}
grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
@ -20,10 +171,10 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (GetStatus): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
DexHandState state;
DexHandState state{};
dev->getState(state);
response->mutable_state()->set_is_initialized(state.is_initialized);
for (int i = 0; i < 6; i++) {
for (int i = 0; i < kDexHandDofCount; i++) {
auto hand = response->mutable_state()->add_hands();
hand->set_dof_id(i);
hand->set_angle(state.hands[i].angle);
@ -33,7 +184,7 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
hand->set_speed(state.hands[i].speed);
hand->set_temperature(state.hands[i].temperature);
hand->set_error(state.hands[i].error);
for (auto& errormessage :state.hands[i].error_message) {
for (auto& errormessage : state.hands[i].error_message) {
hand->add_error_message(errormessage);
}
}
@ -48,21 +199,23 @@ grpc::Status gRPCDexHandServiceImpl::GetStatus(grpc::ServerContext* context,
return grpc::Status::OK;
}
}
grpc::Status gRPCDexHandServiceImpl::SetDexHandPos(grpc::ServerContext* context
, const cmvr::api::SetDexHandPositionsCommand_Request* request
, cmvr::api::SetDexHandPositionsCommand_Feedback* response){
, cmvr::api::SetDexHandPositionsCommand_Feedback* response) {
try {
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPos): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
auto freedoms = request->values();
//默认-1不修改状态
std::vector<int> finger_joint_targets(6,-1);
for (auto& freedom : freedoms) {
//遍历拿到需要配置的自由度,传入的是百分比0-1
finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_POSITION;
if (respondUnsupportedForRh56(dev,
"SetDexHandPos",
"Use SetDexHandAngle for RH56 joint commands.",
response->mutable_header())) {
return grpc::Status::OK;
}
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
applyFreedomValues(request->values(), DEXHAND_MAX_POSITION, finger_joint_targets);
dev->setPositions(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@ -83,21 +236,17 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandAngle(grpc::ServerContext* contex
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandAngle): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
auto freedoms = request->values();
//默认-1不修改状态
std::vector<int> finger_joint_targets(6,-1);
for (auto& freedom : freedoms) {
//遍历拿到需要配置的自由度,传入的是百分比0-1
finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_ANGLE;
}
// std::cout << "steAngle: ";
// for (auto& angle : finger_joint_targets)
// {
// std::cout << " " << angle;
// }
// std::cout << std::endl;
if (const auto rh56 = std::dynamic_pointer_cast<RH56DFTPDexhand>(dev)) {
std::vector<int> finger_joint_targets = readCurrentAngles(dev);
applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets);
rh56->setAngles(finger_joint_targets);
} else {
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
applyFreedomValues(request->values(), DEXHAND_MAX_ANGLE, finger_joint_targets);
dev->setAngles(finger_joint_targets);
}
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
@ -117,14 +266,15 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandForce(grpc::ServerContext* contex
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandForce): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
auto freedoms = request->values();
//默认-1不修改状态
std::vector<int> finger_joint_targets(6,-1);
for (auto& freedom : freedoms) {
//遍历拿到需要配置的自由度,传入的是百分比0-1
finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_FORCE;
if (respondUnsupportedForRh56(dev,
"SetDexHandForce",
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
response->mutable_header())) {
return grpc::Status::OK;
}
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
applyFreedomValues(request->values(), DEXHAND_MAX_FORCE, finger_joint_targets);
dev->setForce(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@ -145,14 +295,15 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandSpeed(grpc::ServerContext* contex
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandSpeed): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
auto freedoms = request->values();
//默认-1不修改状态
std::vector<int> finger_joint_targets(6,-1);
for (auto& freedom : freedoms) {
//遍历拿到需要配置的自由度,传入的是百分比0-1
finger_joint_targets[freedom.id()] = freedom.value() * DEXHAND_MAX_SPEED;
if (respondUnsupportedForRh56(dev,
"SetDexHandSpeed",
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
response->mutable_header())) {
return grpc::Status::OK;
}
std::vector<int> finger_joint_targets(static_cast<size_t>(kDexHandDofCount), -1);
applyFreedomValues(request->values(), DEXHAND_MAX_SPEED, finger_joint_targets);
dev->setVelocities(finger_joint_targets);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
@ -173,6 +324,13 @@ grpc::Status gRPCDexHandServiceImpl::SetDexHandPresetAct(grpc::ServerContext* co
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (SetDexHandPresetAct): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
if (respondUnsupportedForRh56(dev,
"SetDexHandPresetAct",
"RH56DFTPDexhand currently exposes angle and tactile APIs only.",
response->mutable_header())) {
return grpc::Status::OK;
}
auto presetActId = request->presetactid();
dev->setPresetAct(presetActId);
response->mutable_header()->set_success(true);
@ -195,60 +353,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorData(grpc::ServerContext* context
string dev_id = request->header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorData): id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
//从串口获取传感器数据并返回
auto sensors = dev->getSensorData();
// 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData
auto fillSensorData = [](const auto& tactileData,
cmvr::api::SensorData::FingerType fingerType,
cmvr::api::SensorData::PartType partType,
cmvr::api::SensorData* sensorData) {
// 设置行列数
sensorData->set_rows(tactileData.rows);
sensorData->set_cols(tactileData.cols);
// 设置传感器类型
sensorData->set_finger_type(fingerType);
sensorData->set_part_type(partType);
sensorData->set_sensor_name(tactileData.name); // 使用原结构体中的name字段
// 填充数据
for (const auto& row : tactileData.data) {
cmvr::api::SensorData_RowData* rowData = sensorData->add_data();
for (TactilePoint value : row) {
rowData->add_values(static_cast<int32_t>(value));
}
}
};
// 1. 填充小拇指数据
fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response->add_sensor());
fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response->add_sensor());
fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response->add_sensor());
// 2. 填充无名指数据
fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response->add_sensor());
fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response->add_sensor());
fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response->add_sensor());
// 3. 填充中指数据（修改为MIDDLE_FINGER）
fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response->add_sensor());
fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response->add_sensor());
fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response->add_sensor());
// 4. 填充食指数据
fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response->add_sensor());
fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response->add_sensor());
fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response->add_sensor());
// 5. 填充大拇指数据（修改为THUMB_MIDDLE）
fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response->add_sensor());
fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response->add_sensor());
fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response->add_sensor());
fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response->add_sensor());
// 6. 填充掌心数据（使用PALM_PAD）
fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response->add_sensor());
maybeConfigureRh56FullTactilePolling(dev);
appendSensorData(dev->getSensorData(), response);
response->mutable_header()->set_success(true);
setCurrentTimestamp(response->mutable_header()->mutable_timestamp());
return grpc::Status::OK;
@ -266,65 +372,21 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
{
try {
api::GetSensorDataStreamCommand_Request request;
stream->Read(&request);
if (!stream->Read(&request)) {
return grpc::Status::OK;
}
string dev_id = request.header().device_id();
LOG(INFO) << "[gRPCDexHandServiceImpl] (GetSensorDataStream): start,id=" << dev_id;
const auto dev = dmgr_.getDevice<AbstractDexHand>(dev_id);
while (true)
maybeConfigureRh56FullTactilePolling(dev);
while (!context->IsCancelled())
{
api::GetSensorDataStreamCommand_Feedback response;
//从串口获取传感器数据并返回
auto sensors = dev->getSensorData();
// 辅助函数：将FingerTactileData或PalmTactileData转换为Proto的SensorData
auto fillSensorData = [](const auto& tactileData,
cmvr::api::SensorData::FingerType fingerType,
cmvr::api::SensorData::PartType partType,
cmvr::api::SensorData* sensorData) {
// 设置行列数
sensorData->set_rows(tactileData.rows);
sensorData->set_cols(tactileData.cols);
// 设置传感器类型
sensorData->set_finger_type(fingerType);
sensorData->set_part_type(partType);
sensorData->set_sensor_name(tactileData.name); // 使用原结构体中的name字段
// 填充数据
for (const auto& row : tactileData.data) {
cmvr::api::SensorData_RowData* rowData = sensorData->add_data();
for (TactilePoint value : row) {
rowData->add_values(static_cast<int32_t>(value));
}
}
};
// 1. 填充小拇指数据
fillSensorData(sensors.pinky.tip, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::TIP, response.add_sensor());
fillSensorData(sensors.pinky.finger, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::FINGER, response.add_sensor());
fillSensorData(sensors.pinky.pad, cmvr::api::SensorData::PINKY, cmvr::api::SensorData::PAD, response.add_sensor());
// 2. 填充无名指数据
fillSensorData(sensors.ring.tip, cmvr::api::SensorData::RING, cmvr::api::SensorData::TIP, response.add_sensor());
fillSensorData(sensors.ring.finger, cmvr::api::SensorData::RING, cmvr::api::SensorData::FINGER, response.add_sensor());
fillSensorData(sensors.ring.pad, cmvr::api::SensorData::RING, cmvr::api::SensorData::PAD, response.add_sensor());
// 3. 填充中指数据（修改为MIDDLE_FINGER）
fillSensorData(sensors.middle.tip, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::TIP, response.add_sensor());
fillSensorData(sensors.middle.finger, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::FINGER, response.add_sensor());
fillSensorData(sensors.middle.pad, cmvr::api::SensorData::MIDDLE_FINGER, cmvr::api::SensorData::PAD, response.add_sensor());
// 4. 填充食指数据
fillSensorData(sensors.index.tip, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::TIP, response.add_sensor());
fillSensorData(sensors.index.finger, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::FINGER, response.add_sensor());
fillSensorData(sensors.index.pad, cmvr::api::SensorData::INDEX, cmvr::api::SensorData::PAD, response.add_sensor());
// 5. 填充大拇指数据（修改为THUMB_MIDDLE）
fillSensorData(sensors.thumb.tip, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::TIP, response.add_sensor());
fillSensorData(sensors.thumb.finger, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::FINGER, response.add_sensor());
fillSensorData(sensors.thumb.middle, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::THUMB_MIDDLE, response.add_sensor());
fillSensorData(sensors.thumb.pad, cmvr::api::SensorData::THUMB, cmvr::api::SensorData::PAD, response.add_sensor());
// 6. 填充掌心数据（使用PALM_PAD）
fillSensorData(sensors.palm, cmvr::api::SensorData::PALM, cmvr::api::SensorData::PALM_PAD, response.add_sensor());
appendSensorData(dev->getSensorData(), &response);
response.mutable_header()->set_success(true);
setCurrentTimestamp(response.mutable_header()->mutable_timestamp());
if (!stream->Write(response)) {
LOG(INFO) << "[gRPCDexHandServiceImpl] (stream->Write) failed,id=" << dev_id;
@ -336,8 +398,8 @@ grpc::Status gRPCDexHandServiceImpl::GetSensorDataStream(grpc::ServerContext* co
return grpc::Status::OK;
}
catch (const std::exception& e) {
LOG(ERROR) << "[gRPCDexHandServiceImpl] (GetSensorDataStream) exception: " << e.what();
return grpc::Status::OK;
}
}

13
cmvr-es/service/grpc/src/grpc_hlc_service.cpp
View File

@ -60,12 +60,13 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
std::lock_guard<std::mutex> lock(touch_mutex_);
try {
const double forward_l = request->max_force();
LOG(INFO) << "[gRPCHlcServiceImpl] touch: request.device_id="
<< request->header().device_id()
<< " (ignored, using TouchScreenApp config)"
<< ", u=" << request->u()
<< ", v=" << request->v()
<< ", max_force ignored";
<< ", forward_l(max_force)=" << forward_l;
std::string init_error;
if (!ensureTouchAppInitialized(init_error)) {
@ -73,6 +74,16 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
return grpc::Status(grpc::StatusCode::INTERNAL, init_error);
}
if (forward_l != -1.0) {
if (forward_l < 0.0) {
throw std::runtime_error("invalid forward_l(max_force): must be -1 or >= 0");
}
if (!touch_app_.setTouchSpeedlForwardL(forward_l)) {
throw std::runtime_error(
buildTouchFailureMessage(touch_app_, "TouchScreenApp set speedl forward L failed"));
}
}
if (!touch_app_.startFromPixel(request->u(), request->v())) {
throw std::runtime_error(
buildTouchFailureMessage(touch_app_, "TouchScreenApp startFromPixel failed"));

2
cmvr-es/service/grpc/src/grpc_humanoid_robot_service.cpp
View File

@ -102,7 +102,7 @@ grpc::Status gRPCHumanoidRobotServiceImpl::moveL(grpc::ServerContext* context, c
pose.mutable_euler()->set_ry(request->targetpose().ry());
pose.mutable_euler()->set_rz(request->targetpose().rz());
robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc());
// robot->moveL("PELVIS_S",ee_link,pose,request->vel(),request->acc());
response->mutable_header()->set_success(true);
response->mutable_header()->set_error_message("");
}catch (const std::exception& e) {

6
compile_commands.json
View File

@ -503,6 +503,12 @@
"file": "/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand.cpp",
"output": "cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand.dir/src/rh56dftp_dexhand.cpp.o"
},
{
"directory": "/home/lgv/cmvr/cmvr-es/cmake-build-debug",
"command": "/usr/bin/g++-11 -DPROTOBUF_USE_DLLS -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/coal/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/console_bridge/v1.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/eigenpy/v3.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/Eigen3/v3.4.0 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/example-robot-data/v4.3.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/fcl/v0.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x264/v165/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x265/v215/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg-jetson/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg/v4.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gflags/v2.2.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/glog/v0.5.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gtest/v1.17.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jrl-cmakemodules/v1.1.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jsoncpp/v1.9.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/libccd/v2.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/lodepng/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mujoco/v3.3.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp-eigen/v0.10.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/PCAN-Basic/v4.10.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/peak-linux-driver/v8.20.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pinocchio/v3.6.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pybind11/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/realsense/v2.56.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/tinyxml2/v11.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/toppra/v0.6.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom/v5.0.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom_headers/v2.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include/opencv4 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/modbus/3.1.11/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/visp/3.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mainif/0.0.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/matplotplusplus/1.2.0/include -I/home/lgv/cmvr/cmvr-es/cmake-build-debug/_protobuf -I/home/lgv/cmvr/cmvr-es/include -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices -I/home/lgv/cmvr/cmvr-es/cmvr-es/utils -I/home/lgv/cmvr/cmvr-es/cmvr-es -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include/osqp -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand -I/home/lgv/cmvr/cmvr-es/cmvr-es/common -I/home/lgv/cmvr/cmvr-es/protos -isystem /home/lgv/cmvr/cmvr-es/dependency/x86/third_party/grpc/v1.76.0/include -g -std=gnu++17 -fPIE -fdiagnostics-color=always -o cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand_test.dir/src/rh56dftp_dexhand_test.cpp.o -c /home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp",
"file": "/home/lgv/cmvr/cmvr-es/cmvr-es/devices/dexhand/rh56dftp_dexhand/src/rh56dftp_dexhand_test.cpp",
"output": "cmvr-es/devices/dexhand/rh56dftp_dexhand/CMakeFiles/rh56dftp_dexhand_test.dir/src/rh56dftp_dexhand_test.cpp.o"
},
{
"directory": "/home/lgv/cmvr/cmvr-es/cmake-build-debug",
"command": "/usr/bin/g++-11 -Dhead_esp32_EXPORTS -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/coal/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/console_bridge/v1.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/eigenpy/v3.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/Eigen3/v3.4.0 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/example-robot-data/v4.3.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/fcl/v0.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x264/v165/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/x265/v215/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg-jetson/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/ffmpeg/v4.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gflags/v2.2.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/glog/v0.5.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/grpc/v1.76.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/gtest/v1.17.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jrl-cmakemodules/v1.1.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/jsoncpp/v1.9.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/libccd/v2.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/lodepng/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mujoco/v3.3.7/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp-eigen/v0.10.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/PCAN-Basic/v4.10.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/peak-linux-driver/v8.20.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pinocchio/v3.6.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/pybind11/v3.0.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/realsense/v2.56.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/tinyxml2/v11.0.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/toppra/v0.6.2/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom/v5.0.3/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/urdfdom_headers/v2.0.1/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/opencv/4.13.0/include/opencv4 -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/modbus/3.1.11/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/visp/3.7.0/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/mainif/0.0.5/include -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/matplotplusplus/1.2.0/include -I/home/lgv/cmvr/cmvr-es/cmake-build-debug/_protobuf -I/home/lgv/cmvr/cmvr-es/include -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices -I/home/lgv/cmvr/cmvr-es/cmvr-es/utils -I/home/lgv/cmvr/cmvr-es/cmvr-es -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party -I/home/lgv/cmvr/cmvr-es/dependency/x86/third_party/osqp/v0.6.3/include/osqp -I/home/lgv/cmvr/cmvr-es/cmvr-es/devices/biohead/biohead_esp32 -g -std=gnu++17 -fPIC -fdiagnostics-color=always -o cmvr-es/devices/biohead/biohead_esp32/CMakeFiles/head_esp32.dir/src/biohead_esp32.cpp.o -c /home/lgv/cmvr/cmvr-es/cmvr-es/devices/biohead/biohead_esp32/src/biohead_esp32.cpp",

10
protos/cmvr/config/touch_screen_app_config/touch_screen_app_config.proto
View File

@ -107,8 +107,14 @@ message TouchScreenAppConfig {
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 touch_speedl_acceleration = 30;
optional double touch_speedl_forward_l = 31;
optional bool touch_use_speedl = 48;
optional double touch_forward_l = 49;
optional double touch_movel_speed = 50;
optional double touch_movel_acceleration = 51;
optional double touch_movel_jerk = 52;
repeated double touch_movel_qd_max = 53;
optional double dwell_time_s = 32;
TouchScreenTwist6 retract_twist_base = 33;