1. 概述

CartesianAdmittanceController 是 crisp_controllers 中的笛卡尔导纳控制器。它基于 ROS2 control 和 Pinocchio，输出关节力矩指令（effort），实现导纳外环 + 阻抗内环的双层控制结构。

与纯阻抗控制器 CartesianController 不同，导纳控制器通过 F/T 传感器测量外部力/力矩，驱动一个虚拟质量-弹簧-阻尼系统，生成“让步后的内部参考位姿”，再由阻抗内环跟踪该位姿。其核心目标是让机器人在外力作用下表现出柔顺的导纳行为，适用于人机协作、拖动示教、软环境力控等场景。

---

2. 类接口与配置

2.1 命令接口

controller_interface::InterfaceConfiguration
CartesianAdmittanceController::command_interface_configuration() const {
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (const auto & joint_name : params_.joints) {
    config.names.push_back(joint_name + "/effort");
  }
  return config;
}

控制器请求每个关节的 effort 接口，最终写入关节力矩。

2.2 状态接口

controller_interface::InterfaceConfiguration
CartesianAdmittanceController::state_interface_configuration() const {
  config.type = controller_interface::interface_configuration_type::INDIVIDUAL;
  for (const auto & joint_name : params_.joints) {
    config.names.push_back(joint_name + "/position");
  }
  for (const auto & joint_name : params_.joints) {
    config.names.push_back(joint_name + "/velocity");
  }
  return config;
}

读取关节位置和速度。

---

3. 整体控制架构

控制器采用双层结构：

F_ext (F/T传感器)
    │
    ▼
┌─────────────────────────────────────────┐
│ 外环：导纳动力学                         │
│  M_adm ẍ = F_ext - D_adm ẋ + K_adm(x_d - x) │
│  积分得到内部虚拟位姿 inner_SE3_          │
└─────────────────────────────────────────┘
    │
    │ inner_SE3_
    ▼
┌─────────────────────────────────────────┐
│ 内环：阻抗控制                           │
│  e = inner_SE3_ - end_effector_pose     │
│  τ_task = J^T (K e - D J dq)            │
│  τ_d = τ_task + 补偿项                   │
└─────────────────────────────────────────┘
    │
    │ τ_d
    ▼
  电机

• 外环：输入外力，输出内部虚拟位姿 inner_SE3_；
• 内环：输入 inner_SE3_ 和机器人当前状态，输出关节力矩 τ_d。

---

4. 外环：导纳动力学

4.1 导纳模型

导纳外环模拟一个 6 维虚拟质量-弹簧-阻尼系统：

$$M_{\text{adm}}\ddot{x}=F^{ext}-D_{\text{adm}}\dot{x}+K_{\text{adm}}(x_d-x)$$
其中：

符号	代码变量	含义
$M_{\text{adm}}$	adm_mass_	导纳质量矩阵（6×6 对角）
$D_{\text{adm}}$	adm_damping_	导纳阻尼矩阵
$K_{\text{adm}}$	adm_stiffness_	导纳刚度矩阵
$F^{ext}$	ft_wrench_world	外部力/力矩，已变换到 LOCAL_WORLD_ALIGNED
$x_d$	desired_SE3	用户目标位姿（EMA 平滑后）
$x$	inner_SE3_	内部虚拟位姿
$\dot{x}$	inner_motion_	内部虚拟速度

4.2 导纳误差

Eigen::Vector3d adm_pos_error = desired_SE3.translation() - inner_SE3_.translation();
Eigen::Vector3d adm_rot_error =
    pinocchio::log3(desired_SE3.rotation() * inner_SE3_.rotation().transpose());
Eigen::Vector<double, 6> adm_error;
adm_error << adm_pos_error, adm_rot_error;

• 位置误差：世界系下的向量差；
• 姿态误差：${\log }_3(R_d{R}_{\text{inner}}^T)$，即世界系下的旋转误差（左扰动）。

4.3 F/T 传感器坐标变换

F/T 传感器测量的是传感器坐标系下的力/力矩。代码将其变换到 LOCAL_WORLD_ALIGNED：

pinocchio::SE3 ft_sensor_pose = data_.oMf[ft_sensor_frame_id];
pinocchio::Force ft_local(ft_wrench_.head(3), ft_wrench_.tail(3));
pinocchio::Force ft_world = pinocchio::Force::Zero();
pinocchio::changeReferenceFrame(
    ft_sensor_pose, ft_local, pinocchio::LOCAL, pinocchio::LOCAL_WORLD_ALIGNED, ft_world);
Eigen::Vector<double, 6> ft_wrench_world = ft_world.toVector();

ft_sensor_frame_id 由参数 params_.ft_sensor.frame 指定。若未设置，默认使用 end_effector_frame，并发出警告。

4.4 加速度与积分

Eigen::Matrix<double, 6, 6> K_adm =
    use_topic_adm_stiffness_ ? topic_adm_stiffness_ : adm_stiffness_;
Eigen::Vector<double, 6> adm_force =
    ft_wrench_world - adm_damping_ * inner_motion_ + K_adm * adm_error;
Eigen::Vector<double, 6> accel = adm_mass_inv_ * adm_force;

double dt = period.seconds();
inner_motion_ += accel * dt;

对应公式：

$$\ddot{x}=M_{\text{adm}}^{-1}\left[F^{ext}-D_{\text{adm}}\dot{x}+K_{\text{adm}}(x_d-x)\right]$$$$\dot{x}\leftarrow \dot{x}+\ddot{x}\Delta t$$
然后积分 inner_SE3_，支持两种模式：

耦合模式（coupled_se3_integration = true）：

pinocchio::SE3 delta = pinocchio::exp6(pinocchio::Motion(inner_motion_ * dt));
inner_SE3_ = delta * inner_SE3_;

公式：

$$T_{\text{inner}}\leftarrow {\exp }_6(\xi \Delta t)\cdot T_{\text{inner}}$$
平移和旋转耦合，产生螺旋运动。

解耦模式（默认）：

inner_SE3_.translation() += inner_motion_.head(3) * dt;
inner_SE3_.rotation() =
    (pinocchio::exp3(Eigen::Vector3d(inner_motion_.tail(3) * dt)) * inner_SE3_.rotation()).eval();

公式：

$$p_{\text{inner}}\leftarrow p_{\text{inner}}+\dot{p}\Delta t$$$$R_{\text{inner}}\leftarrow {\exp }_3(\omega \Delta t)\cdot R_{\text{inner}}$$
平移用欧拉积分，旋转用 SO(3) 指数映射，避免螺旋耦合。

4.5 初始化

if (!admittance_initialized_) {
  inner_SE3_ = end_effector_pose;
  inner_motion_.setZero();
  admittance_initialized_ = true;
}

首次循环时，内部虚拟位姿初始化为当前末端位姿，速度为 0，避免启动跳变。

在 on_activate() 中也会重置 admittance_initialized_ = false，确保激活后重新初始化。

---

5. 内环：阻抗控制

内环与 CartesianController 几乎相同，区别仅在于目标位姿是 inner_SE3_ 而非用户目标位姿。

5.1 阻抗误差

if (params_.use_local_jacobian) {
  error.head(3) = end_effector_pose.rotation().transpose() *
    (inner_SE3_.translation() - end_effector_pose.translation());
  error.tail(3) =
    pinocchio::log3(end_effector_pose.rotation().transpose() * inner_SE3_.rotation());
} else {
  error.head(3) = inner_SE3_.translation() - end_effector_pose.translation();
  error.tail(3) =
    pinocchio::log3(inner_SE3_.rotation() * end_effector_pose.rotation().transpose());
}

• 局部模式：右扰动，误差表达在末端系；
• 世界对齐模式：左扰动，误差表达在世界系。

误差裁剪：

if (params_.limit_error) {
  max_delta_ << params_.task.error_clip.x, ...;
  error = error.cwiseMax(-max_delta_).cwiseMin(max_delta_);
}

5.2 雅可比与零空间

auto reference_frame = params_.use_local_jacobian
  ? pinocchio::ReferenceFrame::LOCAL
  : pinocchio::ReferenceFrame::LOCAL_WORLD_ALIGNED;
pinocchio::computeFrameJacobian(model_, data_, q_pin, end_effector_frame_id, reference_frame, J);

零空间投影：

• dynamic：$N=I-J^T(J{M}^{-1}{J}^T{)}^{-1}J{M}^{-1}$
• kinematic：$N=I-J^{+}J$
• none：$N=I$

零空间力矩：

$${\tau }_{\text{secondary}}=K_n(q_{\text{ref}}-q)+D_n({\dot{q}}_{\text{ref}}-\dot{q})$$$${\tau }_{\text{nullspace}}=N{\tau }_{\text{secondary}}$$
并限幅。

5.3 任务力矩

if (params_.use_operational_space) {
  tau_task << J.transpose() * Mx * (stiffness * error - damping * (J * dq));
} else {
  tau_task << J.transpose() * (stiffness * error - damping * (J * dq));
}

• 普通阻抗：${\tau }_{\text{task}}=J^T(Ke-DJ\dot{q})$
• OSC：${\tau }_{\text{task}}=J^T{M}_x(Ke-DJ\dot{q})$

其中 $M_x=(J{M}^{-1}{J}^T{)}^{-1}$，通过 pinocchio::computeMinverse 和 pseudo_inverse 计算。

5.4 补偿项

• 关节限位斥力：get_joint_limit_torque(...)
• 摩擦：get_friction(dq, fp1, fp2, fp3)
• 科里奥利：$C(q,\dot{q})\dot{q}$
• 重力：$g(q)$
• 外部扳手：${\tau }_{\text{wrench}}=J^{T}W$

5.5 总力矩

$${\tau }_d={\tau }_{\text{task}}+{\tau }_{\text{nullspace}}+{\tau }_{\text{friction}}+{\tau }_{\text{coriolis}}+{\tau }_{\text{gravity}}+{\tau }_{\text{joint\_limits}}+{\tau }_{\text{wrench}}$$
代码：

tau_d << tau_task + tau_nullspace + tau_friction + tau_coriolis + tau_gravity + tau_joint_limits +
    tau_wrench;

---

6. 安全与滤波

6.1 NaN/Inf 检查

if (!end_effector_pose.translation().allFinite() ||
    !end_effector_pose.rotation().allFinite()) {
  // 保持上一周期力矩
  return controller_interface::return_type::OK;
}

6.2 扭矩变化率限制

if (params_.limit_torques) {
  tau_d = saturateTorqueRate(tau_d, tau_previous, params_.max_delta_tau);
}

6.3 输出 EMA 滤波

tau_d = exponential_moving_average(tau_d, tau_previous, params_.filter.output_torque);

6.4 多发布者检测

check_topic_publisher_count() 检测同一话题是否有多个发布者，若有则忽略命令，防止冲突控制源。

---

7. 参数配置

7.1 导纳参数

setAdmittanceParameters() 设置：

adm_mass_.diagonal() << params_.admittance.mass_x, ..., params_.admittance.mass_rz;
adm_mass_inv_ = adm_mass_.inverse();

adm_stiffness_.diagonal() << params_.admittance.stiffness_x, ..., params_.admittance.stiffness_rz;

adm_damping_.diagonal() << params_.admittance.damping_x, ..., params_.admittance.damping_rz;

支持通过话题动态修改导纳刚度，并限制在 variable_max_admittance_stiffness 范围内。

7.2 阻抗参数

与 CartesianController 相同，由 setStiffnessAndDamping() 设置，支持可变阻抗刚度。

7.3 F/T 传感器

if (params_.ft_sensor.frame.empty()) {
  ft_sensor_frame_id = end_effector_frame_id;
  RCLCPP_WARN(...);
} else {
  ft_sensor_frame_id = model_.getFrameId(params_.ft_sensor.frame);
}

---

8. 与 CartesianController 的对比

方面	CartesianController（阻抗）	CartesianAdmittanceController（导纳）
控制目标	用户目标位姿 $x_d$	内部虚拟位姿 $x_{\text{inner}}$
外力处理	不直接使用 F/T 传感器	用 F/T 传感器驱动导纳动力学
内环结构	无	二阶导纳系统
积分方式	无	半隐式欧拉，SE(3) 指数映射
阻抗外环	直接跟踪 $x_d$	跟踪 $x_{\text{inner}}$
是否需要 F/T	否	是
典型场景	纯柔顺控制，无传感器	力控制、接触任务、拖动示教

---

9. 适用场景

• 软环境：擦拭、抛光、柔性抓取、人机协作；
• 拖动示教：外力推动虚拟质量，机器人柔顺跟随；
• 精细力控：需要 F/T 传感器感知外力，实现主动柔顺；
• 接触任务：装配、表面跟踪。

不适用：硬环境高刚度打磨、刚性装配（更适合纯阻抗控制器）。

---

10. 总结

CartesianAdmittanceController 通过导纳外环 + 阻抗内环实现柔顺力控：

1. 外环：输入外力，输出内部虚拟位姿 inner_SE3_；
2. 内环：输入 inner_SE3_ 和机器人当前状态，输出关节力矩 τ_d；
3. F/T 传感器：测量外力，变换到 LOCAL_WORLD_ALIGNED；
4. 积分：支持耦合/解耦两种 SE(3) 积分模式；
5. 安全：NaN 检查、扭矩限幅、速率限制、EMA 滤波、多发布者检测。

该控制器适用于软环境、人机协作和需要外力感知的力控任务，是 crisp_controllers 中实现主动柔顺的重要组件。