如何在ROS-Gazebo中基于ros_control调用Pydrake控制器实现力矩控制

目前没有公开的直接对接ros_control的现成示例，可参考以下步骤独立实现：

步骤1：对齐Drake与Gazebo的机器人模型 #

首先保证Drake加载的URDF/SDF文件和Gazebo仿真所用文件完全一致，避免运动学、动力学参数不匹配导致控制失效。

from pydrake.multibody.plant import MultibodyPlant
from pydrake.multibody.parsing import Parser

# 初始化无固定时间步的MultibodyPlant，支持外部按需调用计算
plant = MultibodyPlant(time_step=0.0)
Parser(plant).AddModelsFromFile("你的机器人URDF/SDF文件绝对路径")
plant.Finalize()

# 确认关节数量、顺序与ros_control输出的关节状态顺序完全匹配
n_q = plant.num_positions()
n_v = plant.num_velocities()
n_act = plant.num_actuated_dofs()

步骤2：实现独立调用的Drake控制器逻辑 #

无需依赖Drake原生Simulator，直接将控制器作为计算模块调用，输入状态、输出力矩即可。

示例1：PID控制器 #

from pydrake.controllers.linear import PidController
import numpy as np

# 按关节顺序配置PID参数，向量长度与关节数量一致
kp = np.array([150.0]*n_act)
ki = np.array([0.5]*n_act)
kd = np.array([15.0]*n_act)
pid = PidController(
    state_size = n_q + n_v,
    output_size = n_act,
    kp = kp,
    ki = ki,
    kd = kd
)

# 运行时调用逻辑，输入为当前状态x = [q, v]、目标状态x_des = [q_des, v_des]
# q为关节位置向量，v为关节速度向量，顺序和plant定义一致
torque_cmd = pid.CalcOutput(x_current, x_desired)

示例2：LQR控制器 #

from pydrake.systems.controllers import LinearQuadraticRegulator

# 配置平衡点（以零位为例，可根据实际场景修改）
q0 = np.zeros(n_q)
v0 = np.zeros(n_v)
x0 = np.hstack([q0, v0])
u0 = np.zeros(n_act)

# 线性化机器人模型
context = plant.CreateDefaultContext()
plant.SetPositionsAndVelocities(context, x0)
linearized_plant = plant.Linearize(
    context,
    input_port_index=plant.get_actuation_input_port().get_index(),
    output_port_index=plant.get_state_output_port().get_index()
)

# 配置LQR权重
Q = np.diag([10.0]*n_q + [1.0]*n_v) # 状态权重：位置权重高于速度
R = np.diag([0.001]*n_act) # 输入力矩权重
K, _ = LinearQuadraticRegulator(linearized_plant, Q, R)

# 运行时调用逻辑，可额外增加重力补偿项提升控制精度
gravity_comp = plant.CalcGravityGeneralizedForces(context)
torque_cmd = u0 - K @ (x_current - x0) + gravity_comp

步骤3：对接ROS接口 #

用rospy实现控制节点，逻辑如下：

• 订阅/joint_states或自定义状态估计话题，将接收到的关节位置、速度按和plant一致的顺序拼接为当前状态向量x_current
• 接收上层运动规划输出的目标关节状态，拼接为x_desired
• 调用上述Drake控制器计算力矩指令
• 将力矩按关节顺序发布到ros_control力矩控制器的订阅话题（通常为/joint_group_effort_controller/command）

注意事项 #

• 必须严格对齐Drake、Gazebo、ros_control三者的关节顺序，否则力矩指令会映射到错误关节
• LQR为线性控制器，仅在平衡点附近控制效果最优，大范围运动场景可采用分段LQR或非线性MPC方案
• 控制节点的更新频率需和ros_control的控制频率匹配，避免指令延迟
• 可对输出力矩增加限幅逻辑，避免仿真中出现过大力矩导致模型异常

内容的提问来源于stack exchange，提问作者CMcGreal