CRISP源码阅读——基于ROS2力矩反馈控制器与速度加速度零空间

`TorqueFeedbackController` 技术文档

本文基于 crisp_controllers::TorqueFeedbackController 源码，说明其力反馈控制律、冗余自由度零空间投影、ROS2 自定义控制器写法以及核心算法实现。

1. 概述

TorqueFeedbackController 是一个基于 ROS2 controller_interface::ControllerInterface 的力矩控制器。它：

通过 command_interface 向每个关节发送力矩命令 effort；
通过 state_interface 读取关节位置、速度、力矩；
使用 Pinocchio 进行正运动学、雅可比和惯性矩阵相关计算；
根据外部/目标关节力矩 tau_ext_ 生成力反馈项；
支持冗余自由度零空间控制，使机器人可以在不影响末端任务的前提下调整关节构型；
支持三种零空间投影：dynamic、kinematic、none；
将最终关节力矩通过 J^T 映射为笛卡尔 wrench 并发布。

2. 控制器接口与数据流

2.1 命令接口

cpp 复制代码

command_interface_configuration()

返回每个关节的：

text 复制代码

<joint_name>/effort

即控制器输出关节力矩。

2.2 状态接口

cpp 复制代码

state_interface_configuration()

text 复制代码

<joint_name>/position
<joint_name>/velocity
<joint_name>/effort

但 update() 中实际只读取了 position 和 velocity，effort 状态接口当前未使用。

2.3 数据流

text 复制代码

JointState 订阅
  └── msg->effort -> tau_ext_
        │
        ▼
update()
  ├── 读取 q, dq
  ├── Pinocchio 正运动学 / 雅可比
  ├── 阈值处理 tau_ext_thresholded
  ├── 计算零空间次级力矩 tau_secondary
  ├── 计算零空间投影 P
  ├── tau_nullspace = P * tau_secondary
  ├── tau_d = -k_fb * tau_ext_thresholded - kd * dq
  ├── tau_f = friction(dq)
  ├── tau_cmd = tau_d + tau_f + tau_nullspace
  └── 写入 command_interfaces_

3. 力反馈控制器的控制律

3.1 总控制律

控制器最终输出：

τcmd=τd+τf+τns\tau_{cmd} = \tau_d + \tau_f + \tau_{ns}τcmd=τd+τf+τns

其中：

τd\tau_dτd：力反馈项 + 关节阻尼项；
τf\tau_fτf：摩擦补偿项；
τns\tau_{ns}τns：零空间投影后的次级任务力矩。

3.2 力反馈与阻尼项

代码：

cpp 复制代码

auto tau_d = -params_.k_fb * tau_ext_thresholded - params_.kd * dq_;

即：

τd=−kfbτext,thr−kdq˙\tau_d = -k_{fb} \tau_{ext,thr} - k_d \dot qτd=−kfbτext,thr−kdq˙

其中：

kfbk_{fb}kfb：力反馈增益；
kdk_dkd：关节速度阻尼增益；
τext,thr\tau_{ext,thr}τext,thr：阈值处理后的外部/目标关节力矩。

若 τext\tau_{ext}τext 表示外部交互力矩，则该项为负反馈，用于抵抗外部力矩。

若 τext\tau_{ext}τext 表示期望交互力矩，则符号含义取决于上游定义。

3.3 外部力矩阈值

代码：

cpp 复制代码

double tau_ext_magnitude = tau_ext_.norm();
if (tau_ext_magnitude > params_.torque_threshold) {
  tau_ext_thresholded = tau_ext_;
}

即：

τext,thr={τext,∥τext∥>τthr0,∥τext∥≤τthr\tau_{ext,thr} = \begin{cases} \tau_{ext}, & \|\tau_{ext}\| > \tau_{thr} \\ 0, & \|\tau_{ext}\| \le \tau_{thr} \end{cases}τext,thr={τext,0,∥τext∥>τthr∥τext∥≤τthr

这是一个死区/阈值处理，用于抑制小力矩噪声。

3.4 摩擦补偿

代码：

cpp 复制代码

auto tau_f = get_friction(dq_, friction_fp1_, friction_fp2_, friction_fp3_);

摩擦模型由 friction_fp1_、friction_fp2_、friction_fp3_ 参数决定，具体公式在 friction_model.hpp 中。通常为速度相关的摩擦补偿项。

3.5 零空间次级任务

代码：

cpp 复制代码

Eigen::VectorXd q_error = q_ - q_init_;

double nullspace_damping = params_.nullspace.damping > 0
  ? params_.nullspace.damping
  : 2.0 * sqrt(params_.nullspace.stiffness);

Eigen::VectorXd tau_secondary =
  -params_.nullspace.stiffness * nullspace_weights_.cwiseProduct(q_error) -
  nullspace_damping * nullspace_weights_.cwiseProduct(dq_);

即：

τsecondary=−KnsW(q−qinit)−DnsWq˙\tau_{secondary}=-K_{ns} W (q - q_{init})-D_{ns} W \dot qτsecondary=−KnsW(q−qinit)−DnsWq˙

其中：

KnsK_{ns}Kns：零空间刚度；
DnsD_{ns}Dns：零空间阻尼；
WWW：关节权重向量，代码中通过 cwiseProduct 逐元素相乘，等价于对角矩阵；
qinitq_{init}qinit：激活时记录的初始关节位置。

若 damping <= 0，则：

Dns=2KnsD_{ns} = 2\sqrt{K_{ns}}Dns=2Kns

这是临界阻尼形式。

3.6 零空间投影与限幅

cpp 复制代码

Eigen::VectorXd tau_nullspace = nullspace_projection_ * tau_secondary;

然后逐关节限幅：

cpp 复制代码

tau_nullspace[i] = std::max(
  -params_.nullspace.max_tau,
  std::min(params_.nullspace.max_tau, tau_nullspace[i]));

即：

τns,i=clamp(τns,i,−τmax,τmax)\tau_{ns,i} = \mathrm{clamp}(\tau_{ns,i}, -\tau_{max}, \tau_{max})τns,i=clamp(τns,i,−τmax,τmax)

注意：限幅后可能破坏严格的零空间性质。

4. 冗余自由度机器人零空间

零空间控制的目标是：在完成主任务的同时，利用冗余自由度完成次级任务，而不影响主任务。

本控制器中，主任务由力反馈/阻尼项隐式体现，零空间用于将关节拉回初始位置。

4.1 运动学零空间投影

代码：

cpp 复制代码

Eigen::MatrixXd J_pinv = pseudo_inverse(J_, params_.nullspace.regularization);
nullspace_projection_ = Id_nv - J_pinv * J_;

公式：

Pkin=I−J+JP_{kin} = I - J^+ JPkin=I−J+J

其中 J+J^+J+ 是雅可比伪逆，可带正则化。

性质：

JPkin=0J P_{kin} = 0JPkin=0

因此：

J(Pkinq˙0)=0J (P_{kin} \dot q_0) = 0J(Pkinq˙0)=0

即零空间速度不会产生任务空间速度。

4.2 动力学零空间投影

代码：

cpp 复制代码

pinocchio::computeMinverse(model_, data_, q_);
auto Mx_inv = J_ * data_.Minv * J_.transpose();
auto Mx = pseudo_inverse(Mx_inv);
auto J_bar = data_.Minv * J_.transpose() * Mx;
nullspace_projection_ = Id_nv - J_.transpose() * J_bar.transpose();

公式：

Pdyn=I−JT(JM−1JT)−1JM−1P_{dyn}=I-J^T\left(J M^{-1} J^T\right)^{-1}J M^{-1}Pdyn=I−JT(JM−1JT)−1JM−1

其中：

MMM：关节空间惯性矩阵；
M−1M^{-1}M−1：代码中的 data_.Minv；
JM−1JTJ M^{-1} J^TJM−1JT：任务空间惯性逆；
(JM−1JT)−1\left(J M^{-1} J^T\right)^{-1}(JM−1JT)−1：代码中的 Mx。

性质：

JM−1Pdyn=0J M^{-1} P_{dyn} = 0JM−1Pdyn=0

因此：

JM−1(Pdynτ0)=0J M^{-1} (P_{dyn} \tau_0) = 0JM−1(Pdynτ0)=0

即零空间力矩不会产生任务空间加速度。

4.3 不投影

代码：

cpp 复制代码

nullspace_projection_ = Id_nv;

即：

Pnone=IP_{none} = IPnone=I

此时零空间力矩会通过动力学耦合影响任务空间加速度，从而影响末端运动。

4.4 三种投影对比

类型	作用量	公式	零空间条件
kinematic	速度	I−J+JI - J^+ JI−J+J	JP=0J P = 0JP=0
dynamic	力矩	I−JT(JM−1JT)−1JM−1I - J^T(JM^{-1}J^T)^{-1}JM^{-1}I−JT(JM−1JT)−1JM−1	JM−1P=0J M^{-1} P = 0JM−1P=0
none	力矩	III	无

5. 以该控制器为例介绍 ROS2 自定义 Controller 写法

5.1 继承 ControllerInterface

自定义控制器需要继承：

cpp 复制代码

class TorqueFeedbackController : public controller_interface::ControllerInterface

并实现以下关键函数：

cpp 复制代码

controller_interface::InterfaceConfiguration command_interface_configuration() const;
controller_interface::InterfaceConfiguration state_interface_configuration() const;
controller_interface::return_type update(const rclcpp::Time&, const rclcpp::Duration&);
CallbackReturn on_init();
CallbackReturn on_configure(const rclcpp_lifecycle::State&);
CallbackReturn on_activate(const rclcpp_lifecycle::State&);
CallbackReturn on_deactivate(const rclcpp_lifecycle::State&);

5.2 插件导出

文件末尾：

cpp 复制代码

PLUGINLIB_EXPORT_CLASS(
  crisp_controllers::TorqueFeedbackController,
  controller_interface::ControllerInterface)

这样 controller_manager 可以通过 pluginlib 加载该控制器。

5.3 生命周期

ROS2 控制器通常遵循生命周期：

on_init()

初始化参数监听、向量、订阅等。
on_configure()

获取 robot_description，构建 Pinocchio 模型，检查关节，创建发布器和定时器。
on_activate()

读取当前关节状态，记录 q_init_，准备开始控制。
update()

周期调用，执行控制算法并写入命令接口。
on_deactivate()

停止控制时的清理工作。

5.4 接口配置

command_interface_configuration() 和 state_interface_configuration() 返回接口名称列表。

controller_manager 会根据这些名称分配接口。

本控制器：

命令接口：<joint>/effort
状态接口：<joint>/position、<joint>/velocity、<joint>/effort

5.5 参数与订阅/发布

参数通过 ParamListener 读取：

cpp 复制代码

params_listener_ = std::make_shared<torque_feedback_controller::ParamListener>(get_node());
params_listener_->refresh_dynamic_parameters();
params_ = params_listener_->get_params();

cpp 复制代码

joint_sub_ = get_node()->create_subscription<sensor_msgs::msg::JointState>(
  params_.joint_source_topic,
  rclcpp::QoS(1).best_effort().keep_last(1).durability_volatile(),
  std::bind(&TorqueFeedbackController::target_joint_callback_, this, std::placeholders::_1));

发布：

cpp 复制代码

wrench_pub_ = get_node()->create_publisher<geometry_msgs::msg::WrenchStamped>(
  "~/commanded_wrench", rclcpp::QoS(10));

定时器：

cpp 复制代码

wrench_timer_ = get_node()->create_wall_timer(
  std::chrono::milliseconds(5),
  std::bind(&TorqueFeedbackController::publish_wrench_callback_, this));

6. 代码算法部分详解

6.1 `on_init()`

主要工作：

创建参数监听器；
读取关节名 joint_names_；
初始化 q_、dq_、tau_commanded_、q_init_、tau_ext_；
初始化零空间权重；
读取摩擦参数；
初始化零空间投影为单位阵；
创建 joint_source_topic 订阅。

6.2 `on_configure()`

主要工作：

从 robot_state_publisher 获取 robot_description；
使用 Pinocchio 从 URDF 构建模型；
检查参数中的关节是否存在于模型；
将未使用关节锁定，构建缩减模型 model_；
检查关节类型，仅允许旋转关节；
获取末端执行器 frame id；
初始化雅可比矩阵 J_；
创建 wrench 发布器和定时器。

关键代码：

cpp 复制代码

pinocchio::urdf::buildModelFromXML(robot_description_, raw_model_);
model_ = pinocchio::buildReducedModel(raw_model_, list_of_joints_to_lock_by_id, q_locked);
data_ = pinocchio::Data(model_);
end_effector_frame_id_ = model_.getFrameId(params_.end_effector_frame);
J_ = Eigen::MatrixXd::Zero(6, model_.nv);

6.3 `on_activate()`

读取当前关节位置和速度，并记录初始位置：

cpp 复制代码

q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);
tau_ext_[i] = 0.0;
q_init_[i] = q_[i];

因此零空间次级任务的目标是激活时的关节构型。

6.4 `update()`

这是核心控制循环。

步骤 1：读取关节状态

cpp 复制代码

q_[i] = state_interfaces_[i].get_optional().value_or(q_[i]);
dq_[i] = state_interfaces_[num_joints_ + i].get_optional().value_or(dq_[i]);

步骤 2：正运动学与雅可比

cpp 复制代码

pinocchio::forwardKinematics(model_, data_, q_, dq_);
pinocchio::updateFramePlacements(model_, data_);
J_.setZero();
pinocchio::computeFrameJacobian(
  model_, data_, q_, end_effector_frame_id_,
  pinocchio::ReferenceFrame::LOCAL, J_);

得到末端 frame 在 LOCAL 参考系下的 6×nv 雅可比。

步骤 3：外部力矩阈值

cpp 复制代码

Eigen::VectorXd tau_ext_thresholded = Eigen::VectorXd::Zero(num_joints_);
double tau_ext_magnitude = tau_ext_.norm();
if (tau_ext_magnitude > params_.torque_threshold) {
  tau_ext_thresholded = tau_ext_;
}

步骤 4：计算零空间次级力矩

cpp 复制代码

Eigen::VectorXd q_error = q_ - q_init_;
double nullspace_damping = params_.nullspace.damping > 0
  ? params_.nullspace.damping
  : 2.0 * sqrt(params_.nullspace.stiffness);

Eigen::VectorXd tau_secondary =
  -params_.nullspace.stiffness * nullspace_weights_.cwiseProduct(q_error) -
  nullspace_damping * nullspace_weights_.cwiseProduct(dq_);

步骤 5：计算零空间投影

根据 projector_type：

dynamic：使用 computeMinverse 和动力学投影；
kinematic：使用伪逆和运动学投影；
none：单位阵。

步骤 6：投影与限幅

cpp 复制代码

Eigen::VectorXd tau_nullspace = nullspace_projection_ * tau_secondary;
for (int i = 0; i < num_joints_; i++) {
  tau_nullspace[i] = std::max(
    -params_.nullspace.max_tau,
    std::min(params_.nullspace.max_tau, tau_nullspace[i]));
}

步骤 7：计算总力矩

cpp 复制代码

auto tau_d = -params_.k_fb * tau_ext_thresholded - params_.kd * dq_;
auto tau_f = get_friction(dq_, friction_fp1_, friction_fp2_, friction_fp3_);
tau_commanded_ = tau_d + tau_f + tau_nullspace;

步骤 8：写入命令接口

cpp 复制代码

command_interfaces_[i].set_value(tau_commanded_[i]);

步骤 9：刷新动态参数与权重

cpp 复制代码

params_listener_->refresh_dynamic_parameters();
params_ = params_listener_->get_params();

for (size_t i = 0; i < joint_names_.size(); ++i) {
  nullspace_weights_[i] =
    params_.nullspace.weights.joints_map.at(joint_names_[i]).value;
}

注意：权重更新在 update() 末尾，因此本周期使用的权重可能来自上一周期。

6.5 `publish_wrench_callback_()`

每周期执行一次：

cpp 复制代码

Eigen::VectorXd wrench_commanded = J_.transpose() * tau_commanded_;

即：

Fcmd=JTτcmdF_{cmd} = J^T \tau_{cmd}Fcmd=JTτcmd

然后填入 geometry_msgs::msg::WrenchStamped：

前三个元素为力；
后三个元素为力矩；
frame_id = params_.end_effector_frame。

注意：这只是命令关节力矩的雅可比转置映射，不等价于真实末端接触力。

7. 关键公式汇总

7.1 总控制律

τcmd=τd+τf+τns\tau_{cmd} = \tau_d + \tau_f + \tau_{ns}τcmd=τd+τf+τns

τd=−kfbτext,thr−kdq˙\tau_d = -k_{fb}\tau_{ext,thr} - k_d \dot qτd=−kfbτext,thr−kdq˙

τns=Pτsecondary\tau_{ns} = P \tau_{secondary}τns=Pτsecondary

τsecondary=−KnsW(q−qinit)−DnsWq˙\tau_{secondary} = -K_{ns} W(q-q_{init}) - D_{ns} W \dot qτsecondary=−KnsW(q−qinit)−DnsWq˙

7.2 阈值

7.3 零空间投影

运动学：

Pkin=I−J+JP_{kin} = I - J^+ JPkin=I−J+J

动力学：

Pdyn=I−JT(JM−1JT)−1JM−1P_{dyn} = I - J^T(JM^{-1}J^T)^{-1}JM^{-1}Pdyn=I−JT(JM−1JT)−1JM−1

不投影：

Pnone=IP_{none} = IPnone=I

7.4 Wrench 发布

Fcmd=JTτcmdF_{cmd} = J^T \tau_{cmd}Fcmd=JTτcmd

8. 总结

TorqueFeedbackController 的控制律可概括为：

τcmd=−kfbτext,thr−kdq˙+τf+Pτsecondary\boxed{\tau_{cmd}= -k_{fb}\tau_{ext,thr} -k_d \dot q +\tau_f +P\tau_{secondary} }τcmd=−kfbτext,thr−kdq˙+τf+Pτsecondary

其中零空间投影 PPP 可选：

运动学：P=I−J+JP=I-J^+JP=I−J+J，保证 JP=0JP=0JP=0；
动力学：P=I−JT(JM−1JT)−1JM−1P=I-J^T(JM^{-1}J^T)^{-1}JM^{-1}P=I−JT(JM−1JT)−1JM−1，保证 JM−1P=0JM^{-1}P=0JM−1P=0；
不投影：P=IP=IP=I。

该控制器展示了 ROS2 自定义控制器的典型结构：继承 ControllerInterface、配置接口、管理生命周期、使用 Pinocchio 进行机器人学计算，并在 update() 中完成实时控制。

CRISP源码阅读——基于ROS2力矩反馈控制器与速度加速度零空间

TorqueFeedbackController 技术文档

1. 概述

2. 控制器接口与数据流

2.1 命令接口

2.2 状态接口

2.3 数据流

3. 力反馈控制器的控制律

3.1 总控制律

3.2 力反馈与阻尼项

3.3 外部力矩阈值

3.4 摩擦补偿

3.5 零空间次级任务

3.6 零空间投影与限幅

4. 冗余自由度机器人零空间

4.1 运动学零空间投影

4.2 动力学零空间投影

4.3 不投影

4.4 三种投影对比

5. 以该控制器为例介绍 ROS2 自定义 Controller 写法

5.1 继承 ControllerInterface

5.2 插件导出

5.3 生命周期

5.4 接口配置

5.5 参数与订阅/发布

6. 代码算法部分详解

6.1 on_init()

6.2 on_configure()

6.3 on_activate()

6.4 update()

步骤 1：读取关节状态

步骤 2：正运动学与雅可比

步骤 3：外部力矩阈值

步骤 4：计算零空间次级力矩

步骤 5：计算零空间投影

步骤 6：投影与限幅

步骤 7：计算总力矩

步骤 8：写入命令接口

步骤 9：刷新动态参数与权重

6.5 publish_wrench_callback_()

7. 关键公式汇总

7.1 总控制律

7.2 阈值

7.3 零空间投影

7.4 Wrench 发布

Fcmd=JTτcmdF_{cmd} = J^T \tau_{cmd}Fcmd=JTτcmd

8. 总结

`TorqueFeedbackController` 技术文档

6.1 `on_init()`

6.2 `on_configure()`

6.3 `on_activate()`

6.4 `update()`

6.5 `publish_wrench_callback_()`