Drake中结合碰撞几何使用LQR控制器报错解决及控制方案咨询

你遇到的类型不匹配报错，是因为直接将单独绑定了SceneGraph的MultibodyPlant传入LQR接口导致的：LQR做线性化时需要推导AutoDiffXd类型的导数，单独的Plant无法提供对应标量类型的geometry_query端口输入，所以触发类型校验错误。具体修复步骤如下：

1. 不要传入单独的Plant实例，改为传入包含MultibodyPlant和SceneGraph的顶层Diagram，同时传入对应Diagram的Context而非Plant内部的Context
2. 在LQR调用的参数末尾添加true，对应assume_non_continuous_states_are_fixed=true配置，让LQR忽略接触相关的离散状态，仅对连续多体状态做线性化

修改后的核心代码示例：

// atlas_run_dynamics 中已经完成了Diagram的构建，直接使用即可
auto diagram = builder.Build();
auto diagram_context = diagram->CreateDefaultContext();

// 从Diagram Context中获取Plant的Context，设置站立平衡点的状态（关节位姿、速度、浮动基位姿等）
MultibodyPlant<double>& plant = diagram->GetMutableSubsystemByName("plant", diagram_context.get());
Context<double>& plant_context = plant.GetMyMutableContextFromRoot(diagram_context.get());
/* 此处添加你的平衡点设置代码 */

int num_act = plant.num_actuators();
int num_states = plant.num_multibody_states();

Eigen::MatrixXd Q = 10 * Eigen::MatrixXd::Identity(num_states, num_states);
Eigen::MatrixXd R = Eigen::MatrixXd::Identity(num_act, num_act);

// 获取驱动端口在整个Diagram中的索引，而非Plant内部索引
const InputPort<double>& actuation_port = plant.get_actuation_input_port();
auto diagram_actuation_port = diagram->GetInputPort(actuation_port.get_index());

// 调用LQR接口，传入Diagram和对应参数
auto controller = systems::controllers::LinearQuadraticRegulator(
    *diagram, *diagram_context, Q, R,
    Eigen::Matrix<double, 0, 0>::Zero(),
    diagram_actuation_port.get_index(),
    true // 对应 assume_non_continuous_states_are_fixed = true
);

• 你对带接触系统控制逻辑的理解正确：直接在站立平衡点做LQR仅能处理小范围扰动，大扰动下接触状态变化会导致线性化模型失效，确实需要先通过轨迹优化得到包含接触状态、关节扭矩的标称轨迹，再用LQR做跟踪控制。
• 带浮动基、接触的直接配置法（direct collocation）实现可以直接参考Drake内置的humanoid_standup示例，该示例就是针对Atlas机器人实现站立轨迹优化的，已经内置了接触约束、质心动量约束的实现，你可以直接基于该示例修改。
• 添加pin关节确实会改变浮动基特性，不需要修改原有模型结构，在轨迹优化中直接添加接触点约束即可保留浮动基特性。
• 轨迹优化完成后，你可以直接将得到的关节位姿、速度标称轨迹喂给LQR做轨迹跟踪，接触力会在多体动力学仿真过程中自动计算，不需要手动施加。

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