Search code examples
drake

Drifting observed when controlling a robot arm with implicit PD control + gravity compensation

I'm trying to setup a simulation where an iiwa arm is controlled by a combination of

  1. drake's latest implicit PD controller,
  2. InverseDynamics-based gravity compensation.

When I fix the desired position of the arm to q0 and run simulation, I expect the arm to stay at q0, but I observed the arm to drift to a different equilibrium, as shown in the figure below. enter image description here

The amount of drifting increases as the feedback gain decreases. Moreover, drifting disappears when gravity is set to 0.

The plot can be recreated by the code snippet below:

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()
Solution

  • I just wish all SOs came with a beautifully crafted script like this one. Thank you for this small self contained reproduction!

    This revealed a bug in our implementation. The PR fix is here