DifferentialInverseKinematicsIntegrator causes MultibodyPlant to snap to the same uninitialized point at start up every time

Question

Drake version: Dockerhub latest (1.14.0) Python 3.8, OS: Ubuntu 20.04

As mentioned in the title, using DifferentialInverseKinematicsIntegrator (DIKI) with an Inverse Dynamics controller causes MultibodyPlant's end-effector to always snap momentarily to some off point before returning to the properly initialized pose at the beginning of a sim. We do check our start_pose with an ik solver to make sure it is achievable. We set positions for the diki using diki.SetPositions(diki_context, start_pose) and for the plant using plant.SetPositions(plant_context, model, joint_positions). So that should be all that's needed to set an iiwa robot's initial pose (or start_pose) right?

In this example we are using a slider to command the position of the end-effector (final iiwa_link: 7) that feeds the DIKI. The graph below is visualizing the XYZ position of the end-effector (final iiwa_link: 7) every 0.1 delta_t. As seen in the image here there are three red dots far left field showing where the DIKI snaps to at the beginning of any simulation. The other dots are the expected motion along the x and y axes which I commanded and are executed correctly.

Here is a jist of the code (won't run as is):

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
resource = FindResourceOrThrow(
        "drake/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf")
model = Parser(plant, scene_graph).AddModelFromFile(resource)

# Define frames for Cartesian motion planning
ref_frame = 'iiwa_link_0'
tip_frame = 'iiwa_link_7'
# Finalize the multi-body plant
plant.Finalize()

# Start the meshcat visualizer
meshcat = StartMeshcat()
MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

# Create an arbitrary start pose for the robot
start_tcp_rt = RigidTransform(RollPitchYaw(np.array([np.pi, 0.0, 0.0])), np.array([0.3, 0.3, 0.3]))
start_pose = solve_ik(plant, model, ref_frame, tip_frame, [start_tcp_rt],
                        seed=np.ones(plant.num_actuated_dofs()) * 0.1, max_attempts=1)[0]
controller = InvDyn(plant, model)
DIKI(plant, model, start_pose, tip_frame=plant.GetFrameByName(tip_frame), euler_integr_time_step=0.1)
# Finalize the controller
context = diagram.CreateDefaultContext()
diki.SetPositions(diki_context, start_pose)
plant.SetPositions(plant_context, model, start_pose)

# Set up the simulation
simulator = Simulator(diagram, context)
simulator.set_target_realtime_rate(1.0)
simulator.Initialize()

# Add a button to start the simulation
start_sim_button = 'Start Simulation'
meshcat.AddButton(start_sim_button)
# Wait until the start button is pressed before beginning the simulation
print(f'Press the \'{start_sim_button}\' to start the simulation')
while meshcat.GetButtonClicks(start_sim_button) < 1:
    time.sleep(1.0)

# Replace the start button with a stop button
meshcat.DeleteButton(start_sim_button)
stop_sim_button = 'Stop Simulation'
meshcat.AddButton(stop_sim_button)
while meshcat.GetButtonClicks(stop_sim_button) < 1:
    # Advance the simulation
    t = simulator.get_context().get_time()
    simulator.AdvanceTo(t + 0.1)

meshcat.DeleteAddedControls()

if __name__ == '__main__':
    main()

I cannot tell why the DIKI snaps to the same point at the beginning of every sim, no matter if I change the start_pose. It just whips and comes back.

Printing DIKI context before and after simulator.Initialize() shows the same:

Time: 0                                                                                                                                                               
States: 
  2 discrete state groups with 
     7 states  
         1.764131330322971   1.313860284302006  -1.458064433679016  -2.053485204838976  -1.750486209896251  -1.354796418277838 -0.2552547953113156
     1 states                                                                                                                                                         
       0     

Updated Edits to original post ---

Here is an isolated script of the code that causes this effect (for some reason now isolating it

plant.SetPositions(plant_context, model, start_pose)

and

diki.finalize(context, start_pose)

no longer set the start_pose of the robot properly:

import numpy as np
import time
from pydrake.all import (AddMultibodyPlantSceneGraph, DiagramBuilder, FindResourceOrThrow, Parser, Simulator,
                         SceneGraph, GeometryFrame, MeshcatVisualizer, RigidTransform, RollPitchYaw, Solve, RotationMatrix, InverseKinematics, DifferentialInverseKinematicsIntegrator,
                         DifferentialInverseKinematicsParameters, DiscreteDerivative, FramePoseVector, InverseDynamicsController, ConstantVectorSource, ConstantValueSource, Value, AbstractValue, Multiplexer, PassThrough, BasicVector, LeafSystem)
from pydrake.geometry import MeshcatVisualizer

# Utilties
from src.mast.visualization import StartMeshcat, Meshcat, MeshcatPoseSliders, AddTriad

class AnimatedFrame(LeafSystem):
    def __init__(self, scene_graph: SceneGraph, name: str, triad_length: float = 0.2,
                 triad_radius: float = 0.005, triad_opacity: float = 0.5):
        """ Parent class for animated frames that aren't bodies
        :param scene_graph: Scene Graph
        :param name: desired frame name
        """
        super().__init__()
        # Create geometry/frame
        self.sid = scene_graph.RegisterSource()
        frame = GeometryFrame(name)
        # Register geometry with scene graph
        self.m_frame_id = scene_graph.RegisterFrame(self.sid, frame)
        # Add a visualization for the set point
        AddTriad(self.sid, self.m_frame_id, scene_graph, length=triad_length,
                 radius=triad_radius, opacity=triad_opacity, name=frame.name())

class SetPointVisualizer(AnimatedFrame):
    def __init__(self, scene_graph: SceneGraph, name: str):
        super().__init__(scene_graph, name)
        self.input_port = self.DeclareAbstractInputPort('u', Value[RigidTransform]())
        self.output_port = self.DeclareAbstractOutputPort(name='y', alloc=Value[FramePoseVector], calc=self.calc)

    def calc(self, context, output):
        fpv = FramePoseVector()
        fpv.set_value(id=self.m_frame_id, value=self.input_port.Eval(context))
        output.SetFrom(AbstractValue.Make(fpv))

class StateExtractor(LeafSystem):
    def __init__(self, n_dof: int, n_p: int):
        super().__init__()
        self.n_dof = n_dof
        self.n_p = n_p
        if self.n_p > self.n_dof:
            input_size = 13 + 2 * n_dof
        else:
            input_size = 2 * n_dof

        self.input_port = self.DeclareVectorInputPort("CURRENT_SYSTEM_STATE", BasicVector(input_size))
        self.output_port = self.DeclareVectorOutputPort("CURRENT_SYSTEM_STATE", BasicVector(2 * n_dof), self.calc_output)

    def calc_output(self, context, output):
        total_state = self.input_port.Eval(context)
        if self.n_p > self.n_dof:
            pose = total_state[7:(self.n_dof+7)]
            vel = total_state[(13+self.n_dof):]
            output.SetFromVector(np.hstack([pose, vel]))
        else:
            output.SetFromVector(np.hstack([total_state[:self.n_dof*2]]))

class CartesianSliderTrajectorySource():
    """
    Controls a robot by using Meshcat sliders to define set points for the end effector
    """
    def __init__(self, meshcat: Meshcat,
                 initial_pose: RigidTransform,
                 rotation_limits = (-np.pi, np.pi),
                 translation_limits = (-3, 3)):
        builder = DiagramBuilder()

        # Generate Pose Sliders & Add them to the system
        min_limits = (rotation_limits[0],) * 3 + (translation_limits[0],) * 3
        max_limits = (rotation_limits[1],) * 3 + (translation_limits[1],) * 3
        values = MeshcatPoseSliders.Value(*np.hstack([initial_pose.rotation().ToRollPitchYaw().vector(), initial_pose.translation()]))
        mps = MeshcatPoseSliders(meshcat, min_range=min_limits, max_range=max_limits, value=values)
        sliders = builder.AddNamedSystem("sliders", mps)

        # Export the pose from the slider as an output port
        builder.ExportOutput(sliders.get_output_port(), "DESIRED_EE_POSE")

        # Create a constant vector system that outputs zeros as the desired Cartesian velocity
        cvs = builder.AddNamedSystem("const_vector_zeros", ConstantVectorSource(np.zeros((6,))))
        # Export the zero velocity vector as the desired velocity output port
        builder.ExportOutput(cvs.get_output_port(), "DESIRED_EE_VELOCITY")

        # Generate the finalized diagram
        self.diagram = builder.Build()

    def get_diagram(self):
        return self.diagram

class InvDyn():
    def __init__(self,
                 plant,
                 model,
                 kp: float = 100.0,
                 ki: float = 1.0,
                 kd: float = 2.0 * np.sqrt(100.0 / 2.0),
                 ref_accel: bool = False):
        builder = DiagramBuilder()
        self.ref_accel = ref_accel
        self.n_dof = plant.num_actuated_dofs(model)

        kp_v = np.full((self.n_dof,), kp)
        ki_v = np.full((self.n_dof,), ki)
        kd_v = np.full((self.n_dof,), kd)

        # overloaded function, in welded states you can just input the plant instead of P_x,P_y
        jsc = builder.AddNamedSystem('Inverse Dynamics Controller', InverseDynamicsController(
            plant, kp_v, ki_v, kd_v, self.ref_accel))

        # self.input_port_desired_state = builder.AddSystem(PassThrough(2*n_dof))
        builder.ExportInput(
            jsc.get_input_port_desired_state(), "DESIRED_SYSTEM_STATE")

        # self.input_port_estimated_state = builder.AddSystem(PassThrough(2*n_dof))
        builder.ExportInput(
            jsc.get_input_port_estimated_state(), "CURRENT_SYSTEM_STATE")

        # output_actuation_command = builder.AddSystem(PassThrough(2 * n_dof))
        builder.ExportOutput(
            jsc.get_output_port(), "ACTUATION_COMMAND")
        self.diagram = builder.Build()

    def get_diagram(self):
        return self.diagram

class DIKI():
    def __init__(self,
                 plant,
                 model,
                 euler_integr_time_step: float = None,
                 tip_frame = None):
        n_dof = plant.num_actuated_dofs(model)
        self.builder = DiagramBuilder()

        # Create a DifferentialInverseKinematicsIntegrator
        self.parameters = DifferentialInverseKinematicsParameters(n_dof, n_dof)

        # Set the position, velocity, and acceleration limits
        self.parameters.set_joint_acceleration_limits(
            (plant.GetAccelerationLowerLimits()[-n_dof:], plant.GetAccelerationUpperLimits()[-n_dof:]))
        self.parameters.set_joint_velocity_limits(
            (plant.GetVelocityLowerLimits()[-n_dof:], plant.GetVelocityUpperLimits()[-n_dof:]))
        self.parameters.set_joint_position_limits(
            (plant.GetPositionLowerLimits()[-n_dof:], plant.GetPositionUpperLimits()[-n_dof:]))

        self.use_robot_state = False
        # Make all the required leaf systems
        self.diki = self.builder.AddNamedSystem('DIKI Controller', DifferentialInverseKinematicsIntegrator(
            plant, tip_frame, euler_integr_time_step, self.parameters))
        self.builder.ExportInput(self.diki.GetInputPort('X_WE_desired'), "X_WE_DESIRED")
        self.builder.ExportInput(self.diki.GetInputPort('robot_state'), "ROBOT_STATE")

        self.diki_derivative = self.builder.AddNamedSystem('discrete_derivative', DiscreteDerivative(n_dof, euler_integr_time_step))

        self.desired_system_state = self.builder.AddNamedSystem('multiplexer', Multiplexer([n_dof, n_dof]))
        self.builder.ExportOutput(self.desired_system_state.get_output_port(), "DESIRED_SYSTEM_STATE")

        const_bool = self.builder.AddNamedSystem("boolean_source", ConstantValueSource(AbstractValue.Make(self.use_robot_state)))
        # Turn on the DIKI's use_robot_state boolean port
        self.builder.Connect(const_bool.get_output_port(0), self.diki.GetInputPort('use_robot_state'))

        # Connect the DIKI Output to the Torque Controller's Input for Desired State
        self.builder.Connect(self.diki.GetOutputPort('joint_positions'), self.diki_derivative.get_input_port())
        self.builder.Connect(self.diki_derivative.get_output_port(), self.desired_system_state.get_input_port(1))
        self.builder.Connect(self.diki.GetOutputPort('joint_positions'), self.desired_system_state.get_input_port(0))

        #desired_ee_velocity = self.builder.AddNamedSystem("desired_ee_vel_passthrough", PassThrough([0] * 6))
        #self.builder.ExportInput(desired_ee_velocity.get_input_port(), trajectory_system.GetOutputPort("DESIRED_EE_VELOCITY"))

        self.diagram = self.builder.Build()

    def get_diagram(self):
        return self.diagram

    def finalize(self, context, start_pose):
        diki_context = self.diki.GetMyMutableContextFromRoot(context)
        self.diki.SetPositions(diki_context, start_pose)

def main():
    ####### INITIALIZE THE PLANT ###############################################################
    builder = DiagramBuilder()
    plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.001)
    resource = FindResourceOrThrow(
            "drake/manipulation/models/iiwa_description/sdf/iiwa14_polytope_collision.sdf")
    model = Parser(plant, scene_graph).AddModelFromFile(resource)

    # Define frames for Cartesian motion planning
    ref_frame_name = 'iiwa_link_0'
    tip_frame_name = 'iiwa_link_7'
    # Disable gravity
    plant.gravity_field().set_gravity_vector([0., 0., 0.])

    plant.WeldFrames(plant.world_frame(), plant.get_body(
    plant.GetBodyIndices(model)[0]).body_frame())

    # Finalize the multi-body plant
    plant.Finalize()
    plant_context = plant.CreateDefaultContext()
    n_dof = plant.num_actuated_dofs(model)

    # Start the meshcat visualizer
    meshcat = StartMeshcat()
    MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)

    ####### GENERATE A START POSE ##############################################################
    seed = np.ones(plant.num_actuated_dofs()) * 0.1
    # Extract the frame objects
    ref_frame = plant.GetFrameByName(ref_frame_name)
    tip_frame = plant.GetFrameByName(tip_frame_name)

    # Populate the upper and lower position bounds
    pos_error_tol = 1.0e-6
    rot_error_tol = np.radians(1.0e-3)
    ub = np.full(3, pos_error_tol)
    lb = np.full(3, -pos_error_tol)

    # Create an arbitrary start pose for the robot
    start_tcp_rt = RigidTransform(RollPitchYaw(np.array([np.pi, 0.0, 0.0])), np.array([0.5, 0.5, 0.5]))
    poses = [start_tcp_rt]
    ik_poses = np.full((len(poses), n_dof), np.nan)
    for idx, pose in enumerate(poses):
        ik = InverseKinematics(plant, plant_context)
        ik.AddPositionConstraint(
            ref_frame, pose.translation(), tip_frame, p_AQ_upper=ub, p_AQ_lower=lb)
        ik.AddOrientationConstraint(ref_frame, pose.rotation(), tip_frame, RotationMatrix.Identity(), theta_bound=rot_error_tol)
        result = Solve(ik.prog(), initial_guess=seed)
        attempts = 1
        while not result.is_success() and attempts < 5:
            seed[-n_dof:] = np.random.random(n_dof)
            result = Solve(ik.prog(), initial_guess=seed)
            attempts += 1

        if result.is_success():
            ik_poses[idx, :] = result.GetSolution(ik.q())[-n_dof:]
            start_pose = ik_poses[0]
        else:
            raise RuntimeError("Could not find a valid IK solution for the start pose! Please try another start pose.")
    ####### CREATE SUBSYSTEMS ##############################################################
    state_extractor = builder.AddNamedSystem("state_extractor", StateExtractor(
            plant.num_actuated_dofs(model), plant.num_positions(model)))

    traj_source = CartesianSliderTrajectorySource(meshcat, initial_pose=start_tcp_rt)
    trajectory_system = builder.AddNamedSystem("TrajectorySystem", traj_source.get_diagram())

    joint_space_controller = InvDyn(plant, model)
    jsc_system = builder.AddNamedSystem("JointSpaceController", joint_space_controller.get_diagram())

    euler_integr_time_step = 0.1
    diki = DIKI(plant, model, euler_integr_time_step, tip_frame)
    diki_system = builder.AddNamedSystem("DIKI", diki.get_diagram())

    ###### CONNECTs ########################################################################
    builder.Connect(plant.get_state_output_port(model), state_extractor.GetInputPort("CURRENT_SYSTEM_STATE"))
    builder.Connect(state_extractor.GetOutputPort("CURRENT_SYSTEM_STATE"), diki_system.GetInputPort('ROBOT_STATE'))
    builder.Connect(trajectory_system.GetOutputPort("DESIRED_EE_POSE"), diki_system.GetInputPort('X_WE_DESIRED'))
    builder.Connect(diki_system.GetOutputPort("DESIRED_SYSTEM_STATE"), jsc_system.GetInputPort("DESIRED_SYSTEM_STATE"))
    builder.Connect(state_extractor.GetOutputPort("CURRENT_SYSTEM_STATE"), jsc_system.GetInputPort("CURRENT_SYSTEM_STATE"))
    builder.Connect(jsc_system.GetOutputPort("ACTUATION_COMMAND"), plant.get_actuation_input_port(model))

    # Visualize the Cartesian set point
    set_point = builder.AddNamedSystem("set_point_visualizer",
                                    SetPointVisualizer(scene_graph=scene_graph, name="set_point"))
    builder.Connect(set_point.get_output_port(), scene_graph.get_source_pose_port(set_point.sid))
    builder.Connect(trajectory_system.GetOutputPort("DESIRED_EE_POSE"), set_point.get_input_port())

    # Finalize
    # Construct the diagram
    diagram = builder.Build()
    context = diagram.CreateDefaultContext()
    plant.SetPositions(plant_context, model, start_pose)
    diki.finalize(context, start_pose)

    ###### SIM SETUP ############################################################################
    # Set up the simulation
    simulator = Simulator(diagram, context)
    simulator.set_target_realtime_rate(1.0)
    simulator.Initialize()

    # Add a button to start the simulation
    start_sim_button = 'Start Simulation'
    meshcat.AddButton(start_sim_button)
    # Wait until the start button is pressed before beginning the simulation
    print(f'Press the \'{start_sim_button}\' to start the simulation')
    while meshcat.GetButtonClicks(start_sim_button) < 1:
        time.sleep(1.0)

    # Replace the start button with a stop button
    meshcat.DeleteButton(start_sim_button)
    stop_sim_button = 'Stop Simulation'
    meshcat.AddButton(stop_sim_button)

    # Run the simulation
    while meshcat.GetButtonClicks(stop_sim_button) < 1:
        # Advance the simulation
        t = simulator.get_context().get_time()
        simulator.AdvanceTo(t + 0.1)

    meshcat.DeleteAddedControls()

if __name__ == '__main__':
    main()

Answer 1

I was able to run your example. The DifferentialInverseKinematicsIntegrator is working the way that you expect. But you're generating the initial kick because of the way that you are using the DiscreteDerivative, without initializing its state. (The DiscreteDerivative is estimating a huge derivative in the first step.)

Passing the additional suppress_initial_transient=True argument when you construct the DiscreteDerivative resolves the problem:

        self.diki_derivative = self.builder.AddNamedSystem('discrete_derivative', DiscreteDerivative(n_dof, euler_integr_time_step, suppress_initial_transient=True))