用隐式PD控制+重力补偿控制机械臂时观察到的漂移

我正在尝试设置一个模拟,其中iiwa臂由以下各项的组合控制

1. drake最新的隐式PD控制器,
2. InverseDynamics -基于重力补偿。

当我将手臂的所需位置固定到 q0 并进行模拟,我希望手臂保持在 q0 ,但我观察到手臂漂移到了一个不同的平衡点,如下图所示。

漂移量随着反馈增益的减小而增大。此外,当重力设置为0时,漂移会消失。

可以通过以下代码片段重新创建绘图:

import numpy as np
import matplotlib.pyplot as plt

from pydrake.all import (
    DiagramBuilder,
    MultibodyPlant,
    AddMultibodyPlant,
    ProcessModelDirectives,
    MultibodyPlantConfig,
    Parser,
    LoadModelDirectivesFromString,
    InverseDynamics,
    JointActuatorIndex,
    PdControllerGains,
    VisualizationConfig,
    ApplyVisualizationConfig,
    StartMeshcat,
    Simulator,
    LogVectorOutput,
)


directives = """
directives:
    - add_model:
        name: iiwa
        file: package://drake/manipulation/models/iiwa_description/iiwa7/iiwa7_no_collision.sdf
        default_joint_positions:
            iiwa_joint_1: [ 0 ]
            iiwa_joint_2: [ 0 ]
            iiwa_joint_3: [ 0 ]
            iiwa_joint_4: [ -1.7 ]
            iiwa_joint_5: [ 0 ]
            iiwa_joint_6: [ 1.0 ]
            iiwa_joint_7: [ 0 ]
    - add_weld:
        parent: world
        child: iiwa::iiwa_link_0
"""

meshcat = StartMeshcat()

#%%
builder = DiagramBuilder()

plant_config = MultibodyPlantConfig()
plant_config.discrete_contact_solver = "sap"
plant_config.time_step = 1e-3
plant, scene_graph = AddMultibodyPlant(config=plant_config, builder=builder)

model_directives = LoadModelDirectivesFromString(
    directives
)
parser = Parser(plant)
added_models = ProcessModelDirectives(
    directives=model_directives,
    parser=parser,
)
iiwa_model = added_models[0].model_instance

# Implicit PD
Kp = np.ones(7) * 10
Kd = np.ones(7) * 5

actuator_indices = []
for i in range(plant.num_actuators()):
    actuator_index = JointActuatorIndex(i)
    if plant.get_joint_actuator(actuator_index).model_instance() == iiwa_model:
        actuator_indices.append(actuator_index)

for actuator_index, Kp, Kd in zip(actuator_indices, Kp, Kd):
    plant.get_joint_actuator(actuator_index).set_controller_gains(
        PdControllerGains(p=Kp, d=Kd)
    )

plant.Finalize()

# Gravity compensation
inverse_dynamics_controller = builder.AddSystem(
    InverseDynamics(
        plant,
        InverseDynamics.InverseDynamicsMode.kGravityCompensation,
    )
)
builder.Connect(
    plant.get_state_output_port(iiwa_model),
    inverse_dynamics_controller.get_input_port_estimated_state(),
)

builder.Connect(
    inverse_dynamics_controller.get_output_port_generalized_force(),
    plant.get_actuation_input_port(iiwa_model)
)

state_log_sink = LogVectorOutput(
    src=plant.get_state_output_port(iiwa_model),
    builder=builder
)

ApplyVisualizationConfig(VisualizationConfig(), builder, meshcat=meshcat)

diagram = builder.Build()


#%%
q0 = np.array([0, 0, 0, -1.7, 0, 1.0, 0])
v0 = np.zeros(7)
x0 = np.hstack([q0, v0])

simulator = Simulator(diagram)
context = simulator.get_mutable_context()
context_plant = plant.GetMyContextFromRoot(context)

plant.SetPositions(context_plant, q0)
plant.SetVelocities(context_plant, v0)
plant.get_desired_state_input_port(iiwa_model).FixValue(context_plant, x0)


meshcat.StartRecording()
simulator.AdvanceTo(5.0)
meshcat.StopRecording()
meshcat.PublishRecording()


#%%
state_log = state_log_sink.FindLog(context)

t = state_log.sample_times()
x_log = state_log.data().T

#%%
figure = plt.figure()
plt.xlabel("t [s]")
plt.ylabel("joint angles [rad]")
for i in range(7):
    plt.plot(t, x_log[:, i], label=f"q_{i}")

plt.legend(loc='upper right')
plt.show()