feat:add ibvs acc limit
This commit is contained in:
parent
d02d23a3f9
commit
7148e1baf2
@ -116,6 +116,10 @@ public:
|
||||
double ibvs_qdot_max{0.15};
|
||||
// 相机 twist 六维限幅 `[vx, vy, vz, wx, wy, wz]`。
|
||||
std::array<double, 6> ibvs_vmax6{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
|
||||
// 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`。
|
||||
std::array<double, 6> ibvs_amax6{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
|
||||
// 相机 twist 一阶低通滤波系数，范围 [0, 1]。
|
||||
double ibvs_twist_filter_alpha{0.35};
|
||||
// 是否启用关节限位回避。
|
||||
bool enable_joint_limit_avoidance{true};
|
||||
// 关节限位回避增益。
|
||||
@ -221,7 +225,7 @@ public:
|
||||
bool setTouchSpeedlForwardL(double forward_l);
|
||||
|
||||
bool startFromPixel(int u, int v);
|
||||
bool step();
|
||||
bool step(double dt);
|
||||
void stop();
|
||||
|
||||
Phase phase() const { return phase_; }
|
||||
@ -255,7 +259,7 @@ private:
|
||||
void setOptions(const Options& options);
|
||||
bool applyOptions();
|
||||
bool validateControlJointNames() const;
|
||||
bool stepAligning();
|
||||
bool stepAligning(double dt);
|
||||
bool stepTouching();
|
||||
bool stepDwelling();
|
||||
bool stepRetracting();
|
||||
@ -263,6 +267,7 @@ private:
|
||||
bool readControlledJointPositions(std::vector<double>& q_out) const;
|
||||
bool sendJointVelocity(const std::vector<double>& qdot) const;
|
||||
bool sendZeroJointVelocity() const;
|
||||
void hardStopIbvsMotion();
|
||||
bool holdCurrentControlledPosition() const;
|
||||
bool moveToInitPositionIfEnabled() const;
|
||||
bool readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const;
|
||||
|
||||
@ -606,11 +606,15 @@ bool TouchScreenApp::startFromPixel(int u, int v) {
|
||||
return true;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::step() {
|
||||
bool TouchScreenApp::step(const double dt) {
|
||||
if (!initialized_) {
|
||||
last_status_ = Status::NOT_INITIALIZED;
|
||||
return false;
|
||||
}
|
||||
if (!std::isfinite(dt) || dt <= 0.0) {
|
||||
last_status_ = Status::INVALID_CONFIG;
|
||||
return false;
|
||||
}
|
||||
|
||||
if (dexhand_) {
|
||||
const bool tactile_ok = updateTouchPressure();
|
||||
@ -629,7 +633,7 @@ bool TouchScreenApp::step() {
|
||||
last_status_ = Status::IDLE;
|
||||
return true;
|
||||
case Phase::ALIGNING:
|
||||
return stepAligning();
|
||||
return stepAligning(dt);
|
||||
case Phase::ALIGN_REACHED:
|
||||
last_status_ = Status::ALIGN_REACHED;
|
||||
if (options_.pause_after_align_reached) {
|
||||
@ -669,6 +673,7 @@ void TouchScreenApp::stop() {
|
||||
}
|
||||
|
||||
sendZeroJointVelocity();
|
||||
ibvs_.resetTwistCommandState();
|
||||
holdCurrentControlledPosition();
|
||||
|
||||
phase_ = Phase::IDLE;
|
||||
@ -814,6 +819,18 @@ bool TouchScreenApp::optionsFromConfig(const cmvr::config::TouchScreenAppConfig&
|
||||
options.ibvs_vmax6[static_cast<size_t>(i)] = ibvs_vmax[i];
|
||||
}
|
||||
}
|
||||
if (config.has_ibvs_amax6()) {
|
||||
Eigen::Matrix<double, 6, 1> ibvs_amax;
|
||||
ibvs_amax << options.ibvs_amax6[0], options.ibvs_amax6[1], options.ibvs_amax6[2],
|
||||
options.ibvs_amax6[3], options.ibvs_amax6[4], options.ibvs_amax6[5];
|
||||
applyTwist6FromConfig(config.ibvs_amax6(), ibvs_amax);
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
options.ibvs_amax6[static_cast<size_t>(i)] = ibvs_amax[i];
|
||||
}
|
||||
}
|
||||
if (config.has_ibvs_twist_filter_alpha()) {
|
||||
options.ibvs_twist_filter_alpha = std::clamp(config.ibvs_twist_filter_alpha(), 0.0, 1.0);
|
||||
}
|
||||
if (config.has_align_error_threshold6()) {
|
||||
Eigen::Matrix<double, 6, 1> align_err;
|
||||
align_err << options.align_error_threshold6[0], options.align_error_threshold6[1], options.align_error_threshold6[2],
|
||||
@ -1007,6 +1024,8 @@ bool TouchScreenApp::applyOptions() {
|
||||
ibvs_.setMu(options_.ibvs_mu);
|
||||
ibvs_.setQdotMax(options_.ibvs_qdot_max);
|
||||
ibvs_.setVelocityLimit6(options_.ibvs_vmax6);
|
||||
ibvs_.setAccelerationLimit6(options_.ibvs_amax6);
|
||||
ibvs_.setTwistFilterAlpha(options_.ibvs_twist_filter_alpha);
|
||||
ibvs_.setJointLimitAvoidance(options_.enable_joint_limit_avoidance,
|
||||
options_.joint_limit_avoidance_gain,
|
||||
options_.joint_limit_avoidance_margin_ratio,
|
||||
@ -1038,20 +1057,21 @@ bool TouchScreenApp::validateControlJointNames() const {
|
||||
return false;
|
||||
}
|
||||
|
||||
bool TouchScreenApp::stepAligning() {
|
||||
bool TouchScreenApp::stepAligning(const double dt) {
|
||||
const auto now = Clock::now();
|
||||
const double elapsed = std::chrono::duration<double>(now - phase_start_time_).count();
|
||||
if (elapsed > options_.align_timeout_s) {
|
||||
enterFailed(Status::ALIGN_TIMEOUT);
|
||||
return false;
|
||||
}
|
||||
const double ibvs_dt = std::clamp(dt, 0.005, 0.05);
|
||||
|
||||
perception::AprilTagPerception::Options perception_options;
|
||||
perception_options.depth_policy = options_.depth_policy;
|
||||
perception_options.detect_tags = true;
|
||||
perception_options.fetch_encoded = false;
|
||||
if (!perception_->update(perception_options)) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
last_status_ = Status::ALIGN_WAITING_PERCEPTION;
|
||||
return true;
|
||||
}
|
||||
@ -1069,7 +1089,7 @@ bool TouchScreenApp::stepAligning() {
|
||||
}
|
||||
|
||||
if (!tracking_ok) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
@ -1078,7 +1098,7 @@ bool TouchScreenApp::stepAligning() {
|
||||
const int tag_id = tracker_.activeTagId();
|
||||
last_active_tag_id_ = tag_id;
|
||||
if (tag_id < 0) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
@ -1086,7 +1106,7 @@ bool TouchScreenApp::stepAligning() {
|
||||
|
||||
Eigen::Vector3d p_t_target = Eigen::Vector3d::Zero();
|
||||
if (!tracker_.getAnchorInTag(tag_id, p_t_target)) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
@ -1094,7 +1114,7 @@ bool TouchScreenApp::stepAligning() {
|
||||
|
||||
const auto* current_tag = perception_->findTag(tag_id);
|
||||
if (!current_tag) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
@ -1151,13 +1171,13 @@ bool TouchScreenApp::stepAligning() {
|
||||
}
|
||||
|
||||
std::vector<double> qdot_cmd;
|
||||
if (!ibvs_.compute(q_now, qdot_cmd)) {
|
||||
if (!ibvs_.compute(q_now, ibvs_dt, qdot_cmd)) {
|
||||
switch (ibvs_.lastComputeStatus()) {
|
||||
case IbvsController::ComputeStatus::NO_NEW_FRAME:
|
||||
case IbvsController::ComputeStatus::NO_TAG:
|
||||
case IbvsController::ComputeStatus::TAG_MISMATCH:
|
||||
case IbvsController::ComputeStatus::NO_DEPTH:
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
align_stable_count_ = 0;
|
||||
last_status_ = Status::ALIGN_WAITING_TRACK;
|
||||
return true;
|
||||
@ -1206,7 +1226,7 @@ bool TouchScreenApp::stepAligning() {
|
||||
}
|
||||
|
||||
if (align_stable_count_ >= options_.align_stable_frames) {
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
phase_ = Phase::ALIGN_REACHED;
|
||||
phase_start_time_ = Clock::now();
|
||||
touch_command_started_ = false;
|
||||
@ -1390,6 +1410,11 @@ bool TouchScreenApp::sendZeroJointVelocity() const {
|
||||
return sendJointVelocity(zero);
|
||||
}
|
||||
|
||||
void TouchScreenApp::hardStopIbvsMotion() {
|
||||
sendZeroJointVelocity();
|
||||
ibvs_.resetTwistCommandState();
|
||||
}
|
||||
|
||||
bool TouchScreenApp::readCurrentTouchPointPositionBase(Eigen::Vector3d& p_out) const {
|
||||
if (!robot_) {
|
||||
return false;
|
||||
@ -1622,7 +1647,7 @@ void TouchScreenApp::enterFailed(const Status status) {
|
||||
}
|
||||
} catch (...) {
|
||||
}
|
||||
sendZeroJointVelocity();
|
||||
hardStopIbvsMotion();
|
||||
holdCurrentControlledPosition();
|
||||
phase_ = Phase::FAILED;
|
||||
touch_command_started_ = false;
|
||||
|
||||
@ -1,5 +1,6 @@
|
||||
#include "gtest/gtest.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <iostream>
|
||||
#include <thread>
|
||||
@ -36,8 +37,19 @@ void run_touch_once(int u, int v) {
|
||||
|
||||
bool align_reached = false;
|
||||
bool touch_triggered = false;
|
||||
auto last_step_time = std::chrono::steady_clock::now();
|
||||
bool first_step = true;
|
||||
while (app.isBusy()) {
|
||||
const bool step_ok = app.step();
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
double dt = 0.02;
|
||||
if (!first_step) {
|
||||
dt = std::chrono::duration<double>(now - last_step_time).count();
|
||||
dt = std::clamp(dt, 0.005, 0.05);
|
||||
}
|
||||
last_step_time = now;
|
||||
first_step = false;
|
||||
|
||||
const bool step_ok = app.step(dt);
|
||||
const auto& p_c_target = app.tracker().lastTargetInCamera();
|
||||
std::cout << "phase=" << cmvr::app::TouchScreenApp::phaseToString(app.phase())
|
||||
<< ", status=" << cmvr::app::TouchScreenApp::statusToString(app.lastStatus())
|
||||
|
||||
@ -64,6 +64,15 @@ ibvs_vmax6 {
|
||||
wy: 0.6
|
||||
wz: 0.6
|
||||
}
|
||||
ibvs_amax6 {
|
||||
vx: 0.4
|
||||
vy: 0.4
|
||||
vz: 0.5
|
||||
wx: 1.5
|
||||
wy: 1.5
|
||||
wz: 1.5
|
||||
}
|
||||
ibvs_twist_filter_alpha: 0.35
|
||||
enable_joint_limit_avoidance: true
|
||||
joint_limit_avoidance_gain: 0.2
|
||||
joint_limit_avoidance_margin_ratio: 0.15
|
||||
|
||||
@ -109,6 +109,9 @@ public:
|
||||
* @param joints_angle 当前关节角。
|
||||
* @param qdot_out 输出的关节速度命令。
|
||||
* @return 计算成功返回 `true`。
|
||||
*
|
||||
* @note 不带 dt 的接口内部会使用一个保守默认周期来执行 twist 限加速度，
|
||||
* 更推荐使用带 dt 的 compute 接口。
|
||||
*/
|
||||
bool compute(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out);
|
||||
@ -190,6 +193,25 @@ public:
|
||||
*/
|
||||
void setVelocityLimit6(const std::array<double, 6>& vmax6);
|
||||
|
||||
/**
|
||||
* @brief 设置相机 twist 六维加速度限幅。
|
||||
* @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。
|
||||
*/
|
||||
void setAccelerationLimit6(const std::array<double, 6>& amax6);
|
||||
|
||||
/**
|
||||
* @brief 设置相机 twist 一阶低通滤波系数。
|
||||
* @param alpha 滤波系数，范围 [0, 1]；0 表示不启用，1 表示不过滤。
|
||||
*/
|
||||
void setTwistFilterAlpha(double alpha);
|
||||
|
||||
/**
|
||||
* @brief 将内部相机 twist 平滑状态同步到静止状态。
|
||||
*
|
||||
* 适用于上层已经执行硬停（直接发 0 速）后，避免下一次恢复控制时继续沿用旧的 twist 历史。
|
||||
*/
|
||||
void resetTwistCommandState();
|
||||
|
||||
/**
|
||||
* @brief 设置关节限位回避参数。
|
||||
* @param enable 是否启用。
|
||||
@ -214,31 +236,14 @@ public:
|
||||
*/
|
||||
void setAlignCameraToUrdf(const Eigen::Matrix3d& R_camera_urdf);
|
||||
|
||||
// 最近一帧是否检测到了当前被跟踪的 tag。
|
||||
bool isTagDetected() const { return last_tag_detected_; }
|
||||
|
||||
// 最近一次 `compute()` 的状态。
|
||||
ComputeStatus lastComputeStatus() const { return last_compute_status_; }
|
||||
|
||||
// 计算状态转字符串。
|
||||
static const char* statusToString(ComputeStatus status);
|
||||
|
||||
// 最近一次 `compute()` 实际使用的深度来源。
|
||||
DepthUsage lastDepthUsage() const { return last_depth_usage_; }
|
||||
|
||||
// 深度来源转字符串。
|
||||
static const char* depthUsageToString(DepthUsage usage);
|
||||
|
||||
// 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
|
||||
const Eigen::Vector3d& lastTagPositionVisp() const { return last_tag_pos_visp_; }
|
||||
|
||||
// 当前配置要跟踪的 tag id。
|
||||
int trackedTagId() const { return tracked_tag_id_; }
|
||||
|
||||
// 最近一次成功控制时实际使用的 tag id。
|
||||
int lastUsedTagId() const { return last_used_tag_id_; }
|
||||
|
||||
// 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
|
||||
const Eigen::Matrix<double, 6, 1>& lastCameraTwistVisp() const { return last_v_camera_visp_; }
|
||||
|
||||
/**
|
||||
@ -252,10 +257,12 @@ private:
|
||||
/**
|
||||
* @brief 核心控制链路：由感知缓存和当前关节状态计算关节速度命令。
|
||||
* @param joints_angle 当前关节角。
|
||||
* @param dt 控制周期，单位秒。
|
||||
* @param qdot_out 输出的关节速度命令。
|
||||
* @return 计算成功返回 `true`。
|
||||
*/
|
||||
bool computeInternal(const std::vector<double>& joints_angle,
|
||||
double dt,
|
||||
std::vector<double>& qdot_out);
|
||||
|
||||
/**
|
||||
@ -281,128 +288,72 @@ private:
|
||||
void initTask();
|
||||
|
||||
private:
|
||||
// 控制器和 IK 求解器是否已成功初始化。
|
||||
bool initialized_{false};
|
||||
|
||||
// IK 链基座 frame 名称。
|
||||
std::string base_frame_name_;
|
||||
|
||||
// URDF 中相机 frame 名称，对应坐标系 `u`。
|
||||
std::string camera_frame_name_;
|
||||
|
||||
// 共享感知前端。
|
||||
std::shared_ptr<cmvr::perception::AprilTagPerception> perception_{nullptr};
|
||||
|
||||
// ViSP 视觉伺服增益。
|
||||
double lambda_{0.7};
|
||||
|
||||
// 控制模型使用的 tag 边长，单位米。
|
||||
double tag_size_m_{0.12};
|
||||
|
||||
// 控制模型使用的 tag 半边长，单位米。
|
||||
double tag_half_{0.06};
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 x 坐标，单位米。
|
||||
double target_x_{0.0};
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 y 坐标，单位米。
|
||||
double target_y_{0.0};
|
||||
|
||||
// 期望 tag 原点在 ViSP 相机坐标系 `c` 中的 z 坐标，单位米。
|
||||
double target_z_{0.33};
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 rx，单位弧度。
|
||||
double target_rx_{3.14159265358979323846};
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 ry，单位弧度。
|
||||
double target_ry_{0.0};
|
||||
|
||||
// 期望位姿 `cMo_des` 的旋转参数 rz，单位弧度。
|
||||
double target_rz_{0.0};
|
||||
|
||||
// DLS IK 阻尼系数。
|
||||
double mu_{0.02};
|
||||
|
||||
// 关节速度上限。
|
||||
double qdot_max_{0.6};
|
||||
|
||||
// 深度使用模式。
|
||||
DepthMode depth_mode_{DepthMode::MONOCULAR};
|
||||
|
||||
// 深度闭环比例增益。
|
||||
double depth_z_kp_{1.0};
|
||||
|
||||
// 相机 twist 六维限幅。
|
||||
std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
|
||||
|
||||
// 是否启用关节限位回避。
|
||||
// 新增：相机 twist 六维加速度限幅
|
||||
std::array<double, 6> amax6_{{0.4, 0.4, 0.5, 1.5, 1.5, 1.5}};
|
||||
|
||||
// 新增：相机 twist 一阶低通滤波系数
|
||||
double twist_lpf_alpha_{0.35};
|
||||
|
||||
bool limit_avoidance_enabled_{false};
|
||||
|
||||
// 关节限位回避增益。
|
||||
double limit_avoidance_gain_{0.2};
|
||||
|
||||
// 关节限位回避触发边界比例。
|
||||
double limit_avoidance_margin_ratio_{0.15};
|
||||
|
||||
// 单关节最大推回速度。
|
||||
double limit_avoidance_max_push_{0.25};
|
||||
|
||||
// 用于深度闭环的 tag 平面控制点，位于 tag 坐标系 `t` 的 xy 平面，单位米。
|
||||
Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};
|
||||
|
||||
// 从 `AbstractCamera` 相机坐标系 `cam` 到 ViSP 相机坐标系 `c` 的旋转矩阵。
|
||||
Eigen::Matrix3d R_cv_{Eigen::Matrix3d::Identity()};
|
||||
|
||||
// 从 `AbstractCamera` 相机坐标系 `cam` 到 URDF 相机坐标系 `u` 的旋转矩阵。
|
||||
Eigen::Matrix3d R_camera_urdf_{Eigen::Matrix3d::Identity()};
|
||||
|
||||
// ViSP 视觉伺服任务。
|
||||
std::unique_ptr<vpServo> task_{nullptr};
|
||||
|
||||
// tag 四个角点在 tag 坐标系 `t` 中的 3D 模型点。
|
||||
vpPoint obj_pts_[4];
|
||||
|
||||
// 当前观测特征。
|
||||
vpFeaturePoint s_cur_[4];
|
||||
|
||||
// 期望特征。
|
||||
vpFeaturePoint s_star_[4];
|
||||
|
||||
// 当前配置要跟踪的 tag id。
|
||||
int tracked_tag_id_{-1};
|
||||
|
||||
// 速度 IK 求解器。
|
||||
std::unique_ptr<PinocchioDlsIKSolver> dls_solver_{nullptr};
|
||||
|
||||
// 是否成功读取 URDF 中的关节位置限位。
|
||||
bool has_joint_position_limits_{false};
|
||||
|
||||
// 关节位置下限。
|
||||
Eigen::VectorXd q_lower_limits_;
|
||||
|
||||
// 关节位置上限。
|
||||
Eigen::VectorXd q_upper_limits_;
|
||||
|
||||
// 最近一次成功控制时实际使用的 tag id。
|
||||
int last_used_tag_id_{-1};
|
||||
|
||||
// 内部积分得到的关节位置命令缓存。
|
||||
std::vector<double> q_cmd_;
|
||||
|
||||
// 最近一帧是否检测到了当前被跟踪的 tag。
|
||||
bool last_tag_detected_{false};
|
||||
|
||||
// 最近一次 `compute()` 的状态。
|
||||
ComputeStatus last_compute_status_{ComputeStatus::NOT_READY};
|
||||
|
||||
// 最近一次 `compute()` 实际使用的深度来源。
|
||||
DepthUsage last_depth_usage_{DepthUsage::NONE};
|
||||
|
||||
// 最近一次使用到的 tag 原点在 ViSP 相机坐标系 `c` 中的位置。
|
||||
Eigen::Vector3d last_tag_pos_visp_{Eigen::Vector3d::Zero()};
|
||||
|
||||
// 最近一次输出的相机 twist，位于 ViSP 相机坐标系 `c`。
|
||||
Eigen::Matrix<double, 6, 1> last_v_camera_visp_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
|
||||
// 新增：上一拍实际发送给 IK 的相机 twist（ViSP 相机系）
|
||||
Eigen::Matrix<double, 6, 1> v_camera_cmd_prev_{Eigen::Matrix<double, 6, 1>::Zero()};
|
||||
bool has_v_camera_cmd_prev_{false};
|
||||
};
|
||||
|
||||
} // namespace cmvr
|
||||
|
||||
@ -42,6 +42,7 @@ const char* IbvsController::depthUsageToString(DepthUsage usage) {
|
||||
IbvsController::IbvsController() {
|
||||
R_cv_.setIdentity();
|
||||
R_camera_urdf_.setIdentity();
|
||||
resetTwistCommandState();
|
||||
|
||||
updateDepthControlPointInTag();
|
||||
initTask();
|
||||
@ -77,7 +78,6 @@ bool IbvsController::init(const std::string& urdf_path,
|
||||
|
||||
void IbvsController::setPerception(const std::shared_ptr<cmvr::perception::AprilTagPerception>& perception) {
|
||||
perception_ = perception;
|
||||
// 注入后立刻同步一次 tag size（用于控制模型）
|
||||
syncTagSizeFromPerception(true);
|
||||
}
|
||||
|
||||
@ -92,10 +92,7 @@ void IbvsController::syncTagSizeFromPerception(bool force) {
|
||||
tag_size_m_ = s;
|
||||
tag_half_ = 0.5 * tag_size_m_;
|
||||
|
||||
// 深度控制点约束依赖 tag_half_
|
||||
updateDepthControlPointInTag();
|
||||
|
||||
// 任务几何依赖 tag_half_
|
||||
initTask();
|
||||
}
|
||||
|
||||
@ -113,6 +110,7 @@ void IbvsController::reset(const std::vector<double>& q_init) {
|
||||
last_depth_usage_ = DepthUsage::NONE;
|
||||
last_tag_pos_visp_.setZero();
|
||||
last_v_camera_visp_.setZero();
|
||||
resetTwistCommandState();
|
||||
}
|
||||
|
||||
bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
@ -128,7 +126,7 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
}
|
||||
|
||||
std::vector<double> qdot;
|
||||
if (!computeInternal(joints_angle, qdot)) {
|
||||
if (!computeInternal(joints_angle, dt, qdot)) {
|
||||
return false;
|
||||
}
|
||||
|
||||
@ -143,7 +141,8 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
|
||||
bool IbvsController::compute(const std::vector<double>& joints_angle,
|
||||
std::vector<double>& qdot_out) {
|
||||
return computeInternal(joints_angle, qdot_out);
|
||||
constexpr double kDefaultDt = 0.02; // 50 Hz
|
||||
return computeInternal(joints_angle, kDefaultDt, qdot_out);
|
||||
}
|
||||
|
||||
bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
|
||||
@ -155,6 +154,7 @@ bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) c
|
||||
}
|
||||
|
||||
bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
double dt,
|
||||
std::vector<double>& qdot_out) {
|
||||
last_depth_usage_ = DepthUsage::NONE;
|
||||
last_tag_detected_ = false;
|
||||
@ -170,6 +170,10 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
last_compute_status_ = ComputeStatus::NOT_READY;
|
||||
return false;
|
||||
}
|
||||
if (dt <= 0.0 || !std::isfinite(dt)) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
}
|
||||
if (joints_angle.empty()) {
|
||||
last_compute_status_ = ComputeStatus::INVALID_INPUT;
|
||||
return false;
|
||||
@ -179,10 +183,8 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
return false;
|
||||
}
|
||||
|
||||
// 每帧确保 tag_size 同步（你可能运行时调 perception->setTagSize()）
|
||||
syncTagSizeFromPerception(false);
|
||||
|
||||
// perception 必须先 update()，这里以 color 是否为空作为“是否ready”的简单判据
|
||||
if (perception_->color().empty()) {
|
||||
last_compute_status_ = ComputeStatus::NO_NEW_FRAME;
|
||||
return false;
|
||||
@ -205,7 +207,6 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
const vpHomogeneousMatrix& cMo = tag->cMo;
|
||||
last_tag_pos_visp_ << cMo[0][3], cMo[1][3], cMo[2][3];
|
||||
|
||||
// intrinsics
|
||||
const auto& intr = perception_->intrinsics();
|
||||
const double fx = static_cast<double>(intr.fx);
|
||||
const double fy = static_cast<double>(intr.fy);
|
||||
@ -250,7 +251,7 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
s_cur_[i].buildFrom(x, y, Z);
|
||||
}
|
||||
|
||||
last_depth_usage_ = depth_ctrl_used ? DepthUsage::DEPTH_ONLY : DepthUsage::POSE_ONLY;
|
||||
last_depth_usage_ = depth_ctrl_used ? DepthUsage::MIXED : DepthUsage::POSE_ONLY;
|
||||
|
||||
vpColVector v_c = task_->computeControlLaw();
|
||||
|
||||
@ -259,9 +260,41 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
|
||||
v_c[2] = depth_z_kp_ * (z_depth_ctrl - target_z_);
|
||||
}
|
||||
|
||||
// 限幅+缓存（ViSP camera系）
|
||||
// ---------- 视觉速度整形：速度限幅 + 加速度限幅 + 一阶低通 ----------
|
||||
Eigen::Matrix<double, 6, 1> v_raw;
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
v_c[i] = SupportFunctions::clamp(v_c[i], -vmax6_[i], vmax6_[i]);
|
||||
v_raw[i] = std::isfinite(v_c[i]) ? v_c[i] : 0.0;
|
||||
}
|
||||
|
||||
// 1) 速度限幅
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
v_raw[i] = SupportFunctions::clamp(v_raw[i], -vmax6_[i], vmax6_[i]);
|
||||
}
|
||||
|
||||
if (!has_v_camera_cmd_prev_) {
|
||||
resetTwistCommandState();
|
||||
}
|
||||
|
||||
// 2) 加速度限幅
|
||||
Eigen::Matrix<double, 6, 1> v_acc_limited = v_camera_cmd_prev_;
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
const double amax = std::max(0.0, amax6_[i]);
|
||||
const double dv_max = amax * dt;
|
||||
const double dv_des = v_raw[i] - v_camera_cmd_prev_[i];
|
||||
const double dv = SupportFunctions::clamp(dv_des, -dv_max, dv_max);
|
||||
v_acc_limited[i] = v_camera_cmd_prev_[i] + dv;
|
||||
}
|
||||
|
||||
// 3) 一阶低通
|
||||
const double alpha = std::clamp(twist_lpf_alpha_, 0.0, 1.0);
|
||||
if (alpha <= 0.0 || alpha >= 1.0) {
|
||||
v_camera_cmd_prev_ = v_acc_limited;
|
||||
} else {
|
||||
v_camera_cmd_prev_ = alpha * v_acc_limited + (1.0 - alpha) * v_camera_cmd_prev_;
|
||||
}
|
||||
|
||||
for (int i = 0; i < 6; ++i) {
|
||||
v_c[i] = v_camera_cmd_prev_[i];
|
||||
last_v_camera_visp_[i] = v_c[i];
|
||||
}
|
||||
|
||||
@ -346,7 +379,7 @@ void IbvsController::setTarget(double x,
|
||||
initTask();
|
||||
}
|
||||
|
||||
bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
|
||||
bool IbvsController::setTargetFromPointInTag(const Eigen::Vector3d& p_t_target,
|
||||
const Eigen::Vector3d& p_c_target_des,
|
||||
double rx,
|
||||
double ry,
|
||||
@ -356,7 +389,7 @@ void IbvsController::setTarget(double x,
|
||||
}
|
||||
|
||||
vpRotationMatrix R_des_visp;
|
||||
R_des_visp.buildFrom(rx, ry, rz); // 注意：这是 rotvec（theta*u）
|
||||
R_des_visp.buildFrom(rx, ry, rz);
|
||||
|
||||
Eigen::Matrix3d R_des;
|
||||
for (int r = 0; r < 3; ++r) {
|
||||
@ -399,6 +432,20 @@ void IbvsController::setVelocityLimit6(const std::array<double, 6>& vmax6) {
|
||||
vmax6_ = vmax6;
|
||||
}
|
||||
|
||||
void IbvsController::setAccelerationLimit6(const std::array<double, 6>& amax6) {
|
||||
amax6_ = amax6;
|
||||
}
|
||||
|
||||
void IbvsController::setTwistFilterAlpha(double alpha) {
|
||||
twist_lpf_alpha_ = std::clamp(alpha, 0.0, 1.0);
|
||||
}
|
||||
|
||||
void IbvsController::resetTwistCommandState() {
|
||||
v_camera_cmd_prev_.setZero();
|
||||
last_v_camera_visp_.setZero();
|
||||
has_v_camera_cmd_prev_ = true;
|
||||
}
|
||||
|
||||
void IbvsController::setJointLimitAvoidance(bool enable,
|
||||
double gain,
|
||||
double margin_ratio,
|
||||
@ -440,7 +487,6 @@ void IbvsController::updateDepthControlPointInTag() {
|
||||
|
||||
depth_control_point_tag_ = A.fullPivLu().solve(b);
|
||||
|
||||
// 限制在 tag 边界内
|
||||
depth_control_point_tag_.x() = SupportFunctions::clamp(depth_control_point_tag_.x(), -tag_half_, tag_half_);
|
||||
depth_control_point_tag_.y() = SupportFunctions::clamp(depth_control_point_tag_.y(), -tag_half_, tag_half_);
|
||||
}
|
||||
|
||||
@ -5,6 +5,7 @@
|
||||
|
||||
#include "../include/grpc_hlc_service.h"
|
||||
|
||||
#include <algorithm>
|
||||
#include <chrono>
|
||||
#include <stdexcept>
|
||||
#include <thread>
|
||||
@ -92,6 +93,8 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
|
||||
bool align_reached = false;
|
||||
bool touch_triggered = false;
|
||||
auto last_logged_status = cmvr::app::TouchScreenApp::Status::IDLE;
|
||||
auto last_step_time = std::chrono::steady_clock::now();
|
||||
bool first_step = true;
|
||||
while (touch_app_.isBusy()) {
|
||||
if (context != nullptr && context->IsCancelled()) {
|
||||
touch_app_.stop();
|
||||
@ -100,7 +103,16 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
|
||||
return grpc::Status(grpc::StatusCode::CANCELLED, error);
|
||||
}
|
||||
|
||||
if (!touch_app_.step()) {
|
||||
const auto now = std::chrono::steady_clock::now();
|
||||
double dt = 0.02;
|
||||
if (!first_step) {
|
||||
dt = std::chrono::duration<double>(now - last_step_time).count();
|
||||
dt = std::clamp(dt, 0.005, 0.05);
|
||||
}
|
||||
last_step_time = now;
|
||||
first_step = false;
|
||||
|
||||
if (!touch_app_.step(dt)) {
|
||||
throw std::runtime_error(
|
||||
buildTouchFailureMessage(touch_app_, "touch flow failed"));
|
||||
}
|
||||
|
||||
@ -104,6 +104,8 @@ message TouchScreenAppConfig {
|
||||
optional double ibvs_mu = 13;
|
||||
optional double ibvs_qdot_max = 14;
|
||||
TouchScreenTwist6 ibvs_vmax6 = 15;
|
||||
TouchScreenTwist6 ibvs_amax6 = 57;
|
||||
optional double ibvs_twist_filter_alpha = 58;
|
||||
optional bool enable_joint_limit_avoidance = 16;
|
||||
optional double joint_limit_avoidance_gain = 17;
|
||||
optional double joint_limit_avoidance_margin_ratio = 18;
|
||||
|
||||
Loading…
Reference in New Issue
Block a user