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

feat : add ibvs acc limit

Browse Source
This commit is contained in:
lgv 2026-04-03 09:21:57 +08:00
parent 7148e1baf2
commit 2807fd158f
8 changed files with 147 additions and 56 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -116,10 +116,11 @@ 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};
// 相机 twist 六维加速度限幅 `[ax, ay, az, alphax, alphay, alphaz]`；
// 分量小于等于 0 表示该维度不启用加速度限幅。
std::array<double, 6> ibvs_amax6{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
// 相机 twist 一阶低通滤波系数；取值在 (0, 1) 时启用低通，默认 1.0 表示不过滤。
double ibvs_twist_filter_alpha{1.0};
// 是否启用关节限位回避。
bool enable_joint_limit_avoidance{true};
// 关节限位回避增益。

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

@ -1171,7 +1171,7 @@ bool TouchScreenApp::stepAligning(const double dt) {
}
std::vector<double> qdot_cmd;
if (!ibvs_.compute(q_now, ibvs_dt, qdot_cmd)) {
if (!ibvs_.computeQdot(q_now, ibvs_dt, qdot_cmd)) {
switch (ibvs_.lastComputeStatus()) {
case IbvsController::ComputeStatus::NO_NEW_FRAME:
case IbvsController::ComputeStatus::NO_TAG:
@ -1226,6 +1226,14 @@ bool TouchScreenApp::stepAligning(const double dt) {
}
if (align_stable_count_ >= options_.align_stable_frames) {
std::cout << "[TouchScreenApp][ALIGN_REACHED] tag_id=" << tag_id
<< ", err_xyz=[" << last_align_error_camera_.x() << ", "
<< last_align_error_camera_.y() << ", "
<< last_align_error_camera_.z() << "]"
<< ", err_rxyz=[" << rot_error_vec.x() << ", "
<< rot_error_vec.y() << ", "
<< rot_error_vec.z() << "]"
<< std::endl;
hardStopIbvsMotion();
phase_ = Phase::ALIGN_REACHED;
phase_start_time_ = Clock::now();

2
cmvr-es/common/config/cabin_robot.xml
View File

@ -58,7 +58,7 @@
<Motor id="21" jointName="R_WRIST_Y" limitQLb="-1.102" limitQUb="1.02" limitQd="3.0"/>
<Motor id="22" jointName="R_WRIST_R" limitQLb="-0.293" limitQUb="1.57079" limitQd="3.0"/>
</RightArmCan>
<HeadCan id = " " devId = " " channelId ="2" enable="true">
<HeadCan id = " " devId = " " channelId ="2" enable="false">
<Motor id="32" jointName="HEAD_Y" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="30" jointName="HEAD_P" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>
<Motor id="31" jointName="HEAD_R" limitQLb="3.14" limitQUb="3.14" limitQd="3.0"/>

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

@ -53,26 +53,26 @@ target_ry: 0.0
target_rz: 0.0
align_mode: TOUCH_SCREEN_ALIGN_MODE_RX_RY_AND_POSITION
ibvs_lambda: 0.3
ibvs_lambda: 0.4
ibvs_mu: 0.1
ibvs_qdot_max: 0.35
ibvs_qdot_max: 1.0
ibvs_vmax6 {
vx: 0.55
vy: 0.55
vz: 0.55
vx: 1.0
vy: 1.0
vz: 1.0
wx: 0.6
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
vx: 2.4
vy: 2.4
vz: 4.5
wx: 2.5
wy: 2.5
wz: 2.5
}
ibvs_twist_filter_alpha: 0.35
ibvs_twist_filter_alpha: 1.0
enable_joint_limit_avoidance: true
joint_limit_avoidance_gain: 0.2
joint_limit_avoidance_margin_ratio: 0.15
@ -98,15 +98,15 @@ control_joint_names: "R_WRIST_Y"
control_joint_names: "R_WRIST_R"
align_error_threshold6 {
x: 0.006
y: 0.006
z: 0.006
x: 0.005
y: 0.005
z: 0.01
rx: 0.1026646259971647
ry: 0.1026646259971647
rz: 0.1026646259971647
}
align_stable_frames: 2
align_timeout_s: 30.0
align_timeout_s: 20.0
pause_after_align_reached: false
touch_twist_base {

25
cmvr-es/controller/include/ibvs_controller.h
View File

@ -105,16 +105,15 @@ public:
std::vector<double>& q_cmd_out);
/**
* @brief 根据当前关节状态计算关节速度命令。
* @brief 根据当前关节状态和控制周期计算关节速度命令。
* @param joints_angle 当前关节角。
* @param dt 控制周期，单位秒。
* @param qdot_out 输出的关节速度命令。
* @return 计算成功返回 `true`。
*
* @note 不带 dt 的接口内部会使用一个保守默认周期来执行 twist 限加速度，
* 更推荐使用带 dt 的 compute 接口。
*/
bool compute(const std::vector<double>& joints_angle,
std::vector<double>& qdot_out);
bool computeQdot(const std::vector<double>& joints_angle,
double dt,
std::vector<double>& qdot_out);
/**
* @brief 设置 ViSP 视觉伺服增益。
@ -195,13 +194,15 @@ public:
/**
* @brief 设置相机 twist 六维加速度限幅。
* @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度。
* @param amax6 六维加速度限幅，前三维线加速度，后三维角加速度；
* 分量小于等于 0 时表示该维度不启用加速度限幅。
*/
void setAccelerationLimit6(const std::array<double, 6>& amax6);
/**
* @brief 设置相机 twist 一阶低通滤波系数。
* @param alpha 滤波系数，范围 [0, 1]；0 表示不启用，1 表示不过滤。
* @param alpha 滤波系数，范围 [0, 1]；取值在 (0, 1) 时启用一阶低通，
* 其余情况按不过滤处理。
*/
void setTwistFilterAlpha(double alpha);
@ -313,11 +314,11 @@ private:
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 六维加速度限幅；<=0 表示该维度默认不启用。
std::array<double, 6> amax6_{{0.0, 0.0, 0.0, 0.0, 0.0, 0.0}};
// 新增：相机 twist 一阶低通滤波系数
double twist_lpf_alpha_{0.35};
// 相机 twist 一阶低通滤波系数；默认 1.0 表示不过滤。
double twist_lpf_alpha_{1.0};
bool limit_avoidance_enabled_{false};
double limit_avoidance_gain_{0.2};

98
cmvr-es/controller/src/ibvs_controller.cpp
View File

@ -14,6 +14,65 @@
namespace cmvr {
namespace {
Eigen::Vector3d rotvecFromRotationMatrix(const Eigen::Matrix3d& R) {
const Eigen::AngleAxisd aa(R);
if (!std::isfinite(aa.angle()) || !aa.axis().allFinite() || std::abs(aa.angle()) <= 1e-12) {
return Eigen::Vector3d::Zero();
}
return aa.axis() * aa.angle();
}
Eigen::Matrix<double, 6, 1> computeAdaptiveAccelScale(const double tag_size_m,
const vpHomogeneousMatrix& cMo,
const double target_x,
const double target_y,
const double target_z,
const double target_rx,
const double target_ry,
const double target_rz) {
Eigen::Matrix<double, 6, 1> scale = Eigen::Matrix<double, 6, 1>::Ones();
Eigen::Vector3d pos_err;
pos_err << cMo[0][3] - target_x, cMo[1][3] - target_y, cMo[2][3] - target_z;
vpRotationMatrix R_des_visp;
R_des_visp.buildFrom(target_rx, target_ry, target_rz);
Eigen::Matrix3d R_des = Eigen::Matrix3d::Identity();
Eigen::Matrix3d R_cur = Eigen::Matrix3d::Identity();
for (int r = 0; r < 3; ++r) {
for (int c = 0; c < 3; ++c) {
R_des(r, c) = R_des_visp[r][c];
R_cur(r, c) = cMo[r][c];
}
}
const Eigen::Vector3d rot_err = rotvecFromRotationMatrix(R_des.transpose() * R_cur);
const double xy_ref = std::max(0.5 * tag_size_m, 0.01);
const double z_ref = std::max(tag_size_m, 0.03);
const double rot_ref = 0.20; // about 11.5 deg
constexpr double kMinScale = 0.15;
const auto axisScale = [&](const double err_abs, const double ref) {
if (!std::isfinite(err_abs) || !std::isfinite(ref) || ref <= 1e-9) {
return 1.0;
}
const double ratio = std::clamp(err_abs / ref, 0.0, 1.0);
return kMinScale + (1.0 - kMinScale) * ratio;
};
scale[0] = axisScale(std::abs(pos_err.x()), xy_ref);
scale[1] = axisScale(std::abs(pos_err.y()), xy_ref);
scale[2] = axisScale(std::abs(pos_err.z()), z_ref);
scale[3] = axisScale(std::abs(rot_err.x()), rot_ref);
scale[4] = axisScale(std::abs(rot_err.y()), rot_ref);
scale[5] = axisScale(std::abs(rot_err.z()), rot_ref);
return scale;
}
} // namespace
const char* IbvsController::statusToString(ComputeStatus status) {
switch (status) {
case ComputeStatus::OK: return "ok";
@ -139,10 +198,14 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
return true;
}
bool IbvsController::compute(const std::vector<double>& joints_angle,
std::vector<double>& qdot_out) {
constexpr double kDefaultDt = 0.02; // 50 Hz
return computeInternal(joints_angle, kDefaultDt, qdot_out);
bool IbvsController::computeQdot(const std::vector<double>& joints_angle,
double dt,
std::vector<double>& qdot_out) {
if (dt <= 0.0 || !std::isfinite(dt)) {
last_compute_status_ = ComputeStatus::INVALID_INPUT;
return false;
}
return computeInternal(joints_angle, dt, qdot_out);
}
bool IbvsController::getChainJointNames(std::vector<std::string>& joint_names) const {
@ -275,11 +338,32 @@ bool IbvsController::computeInternal(const std::vector<double>& joints_angle,
resetTwistCommandState();
}
// 2) 加速度限幅
// 2) 加速度限幅：误差大时放宽，误差小时收紧；减速/刹车保持满加速度
Eigen::Matrix<double, 6, 1> v_acc_limited = v_camera_cmd_prev_;
const Eigen::Matrix<double, 6, 1> accel_scale =
computeAdaptiveAccelScale(tag_size_m_,
cMo,
target_x_,
target_y_,
target_z_,
target_rx_,
target_ry_,
target_rz_);
for (int i = 0; i < 6; ++i) {
const double amax = std::max(0.0, amax6_[i]);
const double dv_max = amax * dt;
const double amax = amax6_[i];
if (!std::isfinite(amax) || amax <= 0.0) {
v_acc_limited[i] = v_raw[i];
continue;
}
double amax_eff = amax;
const bool same_direction = v_raw[i] * v_camera_cmd_prev_[i] >= 0.0;
const bool speeding_up = std::abs(v_raw[i]) > std::abs(v_camera_cmd_prev_[i]) + 1e-12;
if (same_direction && speeding_up) {
amax_eff *= accel_scale[i];
}
const double dv_max = amax_eff * 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;

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

@ -627,7 +627,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
}
std::vector<double> qdot_next;
const bool ok = ibvs_controller.compute(q_now, qdot_next);
const bool ok = ibvs_controller.computeQdot(q_now, static_cast<double>(cycle_ms) / 1000.0, qdot_next);
if (!ok) {
++compute_fail_steps;
++fail_steps;
@ -936,7 +936,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobotTrackedPoint) {
}
std::vector<double> qdot_next;
const bool ok = ibvs_controller.compute(q_now, qdot_next);
const bool ok = ibvs_controller.computeQdot(q_now, static_cast<double>(cycle_ms) / 1000.0, qdot_next);
if (!ok) {
++compute_fail_steps;
++fail_steps;

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

@ -93,8 +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;
constexpr double kControlDt = 0.01;
auto next_step_time = std::chrono::steady_clock::now();
while (touch_app_.isBusy()) {
if (context != nullptr && context->IsCancelled()) {
touch_app_.stop();
@ -103,16 +103,7 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
return grpc::Status(grpc::StatusCode::CANCELLED, error);
}
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)) {
if (!touch_app_.step(kControlDt)) {
throw std::runtime_error(
buildTouchFailureMessage(touch_app_, "touch flow failed"));
}
@ -136,7 +127,13 @@ grpc::Status gRPCHlcServiceImpl::touch(grpc::ServerContext *context, const cmvr:
last_logged_status = touch_app_.lastStatus();
}
std::this_thread::sleep_for(std::chrono::milliseconds(1));
next_step_time += std::chrono::duration_cast<std::chrono::steady_clock::duration>(
std::chrono::duration<double>(kControlDt));
const auto after_step = std::chrono::steady_clock::now();
if (next_step_time < after_step) {
next_step_time = after_step;
}
std::this_thread::sleep_until(next_step_time);
}
if (!align_reached) {