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

feat: add null space aviod

Browse Source
This commit is contained in:
lgv 2026-02-28 16:29:16 +08:00
parent e37073f493
commit 720d851cd0
7 changed files with 121 additions and 9 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

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

@ -161,6 +161,18 @@ public:
* @param vmax6 线速度/角速度六维限幅。
*/
void setVelocityLimit6(const std::array<double, 6>& vmax6);
/**
* @brief 设置关节限位避障 null-space 项参数。
*
* @param enable 是否启用。
* @param gain 避障增益（rad/s 量纲）。
* @param margin_ratio 上下限触发边界占关节行程比例 (0, 0.5)。
* @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。
*/
void setJointLimitAvoidance(bool enable,
double gain = 0.2,
double margin_ratio = 0.05,
double max_push = 0.25);
/**
* @brief 设置 AbstractCamera 坐标系到 ViSP 坐标系旋转。
* @param R_cv 旋转矩阵。
@ -280,6 +292,11 @@ private:
double depth_z_kp_{1.0};
// 相机 twist 六维限幅。
std::array<double, 6> vmax6_{{0.15, 0.15, 0.20, 0.6, 0.6, 0.6}};
// 关节限位避障 null-space 参数。
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 平面中用于采样深度的控制点。
Eigen::Vector2d depth_control_point_tag_{Eigen::Vector2d::Zero()};

5
cmvr-es/controller/src/controller_test.cpp
View File

@ -531,7 +531,7 @@ protected:
// MuJoCo 渲染线程与控制线程不同步：允许复用最近一帧，避免长时间 no_new_frame。
mujoco_camera_->setConsumeNewFrameOnly(false);
q_home_ = {0.25, 1.00, M_PI / 2 - 0.2, M_PI / 2 - 0.2, -M_PI + 0.5, 0.0, 0.0};
q_home_ = {-0.2423, 1.2929,1.61, 1.58, -2.8792, 0.1150,-0.08};
ibvs_controller_ = std::make_unique<IbvsController>();
ibvs_controller_->setMu(mu_);
@ -540,7 +540,8 @@ protected:
ibvs_controller_->setDepthZGain(1.0);
ibvs_controller_->setVelocityLimit6(vmax6_);
ibvs_controller_->setTrackedTagId(tracked_tag_id_);
ibvs_controller_->setTarget(0.0,0.0,0.35);
ibvs_controller_->setTarget(0.0,0.0,0.4);
ibvs_controller_->setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
const Eigen::Matrix3d R_align = (Eigen::Matrix3d() <<
1, 0, 0,

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

@ -83,6 +83,10 @@ bool IbvsController::init(const std::shared_ptr<device::AbstractCamera>& camera,
initialized_ = dls_solver_->init();
if (initialized_) {
dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
limit_avoidance_gain_,
limit_avoidance_margin_ratio_,
limit_avoidance_max_push_);
has_joint_position_limits_ =
dls_solver_->getJointPositionLimits(q_lower_limits_, q_upper_limits_);
} else {
@ -340,11 +344,11 @@ bool IbvsController::compute(const std::vector<double>& joints_angle,
}
std::cout << "b_cmd: " ;
for (const auto& it : q_cmd_) {
std::cout << it << " ";
}
std::cout << std::endl;
// std::cout << "b_cmd: " ;
// for (const auto& it : q_cmd_) {
// std::cout << it << " ";
// }
// std::cout << std::endl;
clampJointCommandInPlace(q_cmd_);
q_cmd_out = q_cmd_;
@ -420,6 +424,22 @@ void IbvsController::setVelocityLimit6(const std::array<double, 6>& vmax6) {
vmax6_ = vmax6;
}
void IbvsController::setJointLimitAvoidance(bool enable,
double gain,
double margin_ratio,
double max_push) {
limit_avoidance_enabled_ = enable;
limit_avoidance_gain_ = std::max(0.0, gain);
limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
limit_avoidance_max_push_ = max_push;
if (dls_solver_) {
dls_solver_->setJointLimitAvoidance(limit_avoidance_enabled_,
limit_avoidance_gain_,
limit_avoidance_margin_ratio_,
limit_avoidance_max_push_);
}
}
void IbvsController::setAlignCameraToVisp(const Eigen::Matrix3d& R_cv) {
R_cv_ = R_cv;
}

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

@ -201,6 +201,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
ibvs_controller.setMu(0.1);
ibvs_controller.setLambda(0.6);
ibvs_controller.setQdotMax(0.6);
ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
ibvs_controller.setTagSize(0.12);
ibvs_controller.setTrackedTagId(0);
ibvs_controller.setTarget(0.0, 0.0, 0.40);
@ -221,6 +222,7 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {
// ibvs_controller.setVelocityLimit6({0.15, 0.15, 0.20, 0, 0, 0});
// ibvs_controller.setVelocityLimit6({0.03,0.03,0.03,0.005,0.005,0.005});
ibvs_controller.setQdotMax(0.15);
ibvs_controller.setJointLimitAvoidance(true, 0.2, 0.15, 0.25);
@ -321,4 +323,3 @@ TEST(HumanoidRobotTest,IBVSWithRealRobot) {

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

@ -78,6 +78,19 @@ public:
double damping = -1.0,
double qdot_abs_max = std::numeric_limits<double>::infinity());
/**
* @brief 配置关节限位避障 null-space 项。
*
* @param enable 是否启用。
* @param gain 避障增益（rad/s 量纲）。
* @param margin_ratio 在上下限附近触发的边界带占总行程比例 (0, 0.5)。
* @param max_push 每关节最大推回速度（rad/s），<=0 表示不额外限幅。
*/
void setJointLimitAvoidance(bool enable,
double gain = 0.2,
double margin_ratio = 0.15,
double max_push = 0.25);
/**
* @brief 读取本链关节位置限位（来自 URDF）。
* @param lower 输出下限，size=chain_dof_。
@ -122,6 +135,12 @@ private:
Eigen::VectorXd q_lower_chain_;
Eigen::VectorXd q_upper_chain_;
// null-space 关节限位避障参数
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};
// base pose cache (PELVIS_S fixed)
bool base_pose_cached_{false};
pinocchio::SE3 oM_base_cached_;

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

@ -289,6 +289,16 @@ bool PinocchioDlsIKSolver::fk(const std::string& base_link,
return true;
}
void PinocchioDlsIKSolver::setJointLimitAvoidance(bool enable,
double gain,
double margin_ratio,
double max_push) {
limit_avoidance_enabled_ = enable;
limit_avoidance_gain_ = std::max(0.0, gain);
limit_avoidance_margin_ratio_ = std::clamp(margin_ratio, 1e-3, 0.49);
limit_avoidance_max_push_ = max_push;
}
bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
const Eigen::Matrix<double,6,1>& ee_velocity,
std::vector<double>& joints_vel,
@ -359,6 +369,50 @@ bool PinocchioDlsIKSolver::velocityIk(const std::vector<double>& cur_angle,
A.diagonal().array() += (lambda * lambda);
Eigen::VectorXd qdot = J.transpose() * A.ldlt().solve(twist_base);
// 6.1) null-space 关节限位避障：在主任务零空间叠加“离限位推回”速度。
if (limit_avoidance_enabled_ &&
limit_avoidance_gain_ > 0.0 &&
chain_v_dof_ == chain_dof_ &&
q_lower_chain_.size() == chain_dof_ &&
q_upper_chain_.size() == chain_dof_) {
const Eigen::VectorXd q_chain = q_full.segment(chain_q_start_, chain_dof_);
Eigen::VectorXd qdot_avoid = Eigen::VectorXd::Zero(chain_v_dof_);
for (int i = 0; i < chain_dof_; ++i) {
const double lo = q_lower_chain_[i];
const double hi = q_upper_chain_[i];
if (!std::isfinite(lo) || !std::isfinite(hi) || hi <= lo) {
continue;
}
const double span = hi - lo;
const double margin = std::max(1e-4, limit_avoidance_margin_ratio_ * span);
double push = 0.0;
if (q_chain[i] < lo + margin) {
const double s = (lo + margin - q_chain[i]) / margin; // 0..1+
push += limit_avoidance_gain_ * s * s;
} else if (q_chain[i] > hi - margin) {
const double s = (q_chain[i] - (hi - margin)) / margin; // 0..1+
push -= limit_avoidance_gain_ * s * s;
}
if (limit_avoidance_max_push_ > 0.0) {
push = clampd(push, -limit_avoidance_max_push_, limit_avoidance_max_push_);
}
qdot_avoid[i] = push;
}
if (qdot_avoid.squaredNorm() > 1e-16) {
const Eigen::Matrix<double,6,6> A_inv =
A.ldlt().solve(Eigen::Matrix<double,6,6>::Identity());
const Eigen::MatrixXd J_pinv = J.transpose() * A_inv; // n x 6
const Eigen::MatrixXd N =
Eigen::MatrixXd::Identity(chain_v_dof_, chain_v_dof_) - J_pinv * J;
qdot += N * qdot_avoid;
}
}
// 7) 关节速度限幅：取调用者限幅与 URDF velocityLimit 的更严格值。
joints_vel.resize(chain_v_dof_);
for (int i = 0; i < chain_v_dof_; ++i) {

2
model/xiaoyan_description/dual_arm.xml
View File

@ -66,7 +66,7 @@
</asset>
<worldbody>
<body name="tag_board" pos="0.7 -0.2 1.1" euler="1.57 -1.57 0">
<body name="tag_board" pos="0.7 -0.2 1.05" euler="1.37 -1.57 0">
<!-- 15cm x 15cm 白板，同时贴 apriltag 纹理（纹理里自带白边） -->
<geom name="tag_board_geom"