如何在Drake中实现连续机器人的纯运动学控制（无动力学）

针对你遇到的机器人受重力摆动、无法仅用运动学控制的问题，有三种可行方案：

方案1：纯运动学可视化（完全跳过动力学仿真） #

不需要使用Simulator，直接手动控制关节位置并刷新可视化，完全忽略动力学效应：

from drake.geometry import MeshcatVisualizer
from drake.systems.framework import DiagramBuilder
import time

# 假设已完成plant和scene_graph的加载与初始化
builder = DiagramBuilder()
plant = builder.AddSystem(plant)
scene_graph = builder.AddSystem(scene_graph)
plant.RegisterAsSourceForSceneGraph(scene_graph)

# 设置Meshcat可视化
meshcat = Meshcat()
visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
context = diagram.CreateDefaultContext()
plant_context = plant.GetMyContextFromRoot(context)

# 遍历你的分段常曲率运动学轨迹
for q_desired in your_pcc_trajectory:
    # 设置当前期望关节位置
    plant.SetPositions(plant_context, q_desired)
    # 强制刷新可视化
    diagram.ForcedPublish(context)
    # 控制可视化帧率
    time.sleep(0.05)

这种方式完全绕开动力学计算，关节位置严格跟随你的运动学指令。

方案2：高增益位置控制器抵消动力学影响 #

如果需要保留Simulator框架（比如要集成其他传感器），可以用高增益逆动力学控制器让关节严格跟踪期望位置，抵消重力、惯性的影响：

from drake.multibody.inverse_kinematics import InverseDynamicsController
from drake.systems.framework import DiagramBuilder
from drake.systems.primitives import ConstantVectorSource

# 加载plant后初始化控制器
controller = InverseDynamicsController(
    plant,
    kp=[1000]*plant.num_positions(),  # 高位置增益确保跟踪精度
    ki=[0]*plant.num_positions(),
    kd=[100]*plant.num_positions(),
    has_reference_acceleration=False
)

# 构建系统图
builder = DiagramBuilder()
plant = builder.AddSystem(plant)
controller = builder.AddSystem(controller)
# 连接状态输出到控制器
builder.Connect(plant.get_state_output_port(), controller.get_input_port_estimated_state())
# 连接控制器输出到机器人驱动
builder.Connect(controller.get_output_port(), plant.get_actuation_input_port())

# 创建期望位置输入（这里用ConstantVectorSource示例，可替换为你的轨迹生成器）
desired_pos_source = builder.AddSystem(ConstantVectorSource(initial_q))
builder.Connect(
    desired_pos_source.get_output_port(),
    controller.get_input_port_desired_state().get_slice(0, plant.num_positions())
)

# 添加可视化并构建仿真
meshcat = Meshcat()
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
diagram = builder.Build()
simulator = Simulator(diagram)
context = simulator.get_mutable_context()

# 运行仿真时，可随时更新期望位置：
# desired_pos_source.SetOutputValue(desired_pos_source.GetMyContextFromRoot(context), new_q)
simulator.AdvanceTo(10.0)

高增益会让关节快速响应期望位置，几乎消除动力学带来的摆动。

方案3：禁用重力与惯性（快速验证） #

如果只是临时验证运动学效果，可以直接关闭重力，并将机器人惯性设为零：

# 关闭全局重力
plant.SetGravityField([0, 0, 0])
# 遍历所有连杆，设置惯性为零
for body_index in plant.GetBodyIndices():
    body = plant.get_body(body_index)
    body.SetSpatialInertiaInBodyFrame(SpatialInertia<double>())

这种方式简单粗暴，但能快速让机器人不受动力学影响。

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