[Iron] - Possible bug in cartesian compliance controller - fast rising force

[firesurfer]

Hi @stefanscherzinger as requested yesterday I also open a bug about the fast rising force issue we encountered in our setup.

Information

Ubuntu 22.04
Commit: 4b5daab
Used controller: Cartesian compliance controller

Controller yaml:

cartesian_compliance_controller_top_magnet_gripper:
  ros__parameters:
    end_effector_link: "ur_top/magnet_gripper/attach"
    robot_base_link: "ur_top/base_link"
    ft_sensor_ref_link: "ur_top/tool0"
    compliance_ref_link: "ur_top/magnet_gripper/attach"
    joints:
      - ur_top/shoulder_pan_joint
      - ur_top/shoulder_lift_joint
      - ur_top/elbow_joint
      - ur_top/wrist_1_joint
      - ur_top/wrist_2_joint
      - ur_top/wrist_3_joint

    # Choose: position or velocity.
    command_interfaces:
      - position
        #- velocity

    stiffness:  # w.r.t. compliance_ref_link
        trans_x: 8900.0
        trans_y: 900.0
        trans_z: 900.0
        rot_x: 40.0
        rot_y: 40.0
        rot_z: 40.0

    solver:
        error_scale: 0.68
        iterations: 2

    pd_gains:
        trans_x: {p: 0.015, d: 0.001}
        trans_y: {p: 0.019, d: 0.001}
        trans_z: {p: 0.021, d: 0.001}
        rot_x: {p: 0.3}
        rot_y: {p: 0.3}
        rot_z: {p: 0.3}

We are already in contact (while using the cartesian compliance controller) when we decide to stop. Command the current position and a force of 1N downwards (in the plot the y axis)
The actual stiffness values used during the motion where: [1.0, 100.0, 5000.0, 190.0, 190.0, 1.0] (same order as above). We set them at runtime.

You can clearly see the point at which the force starts rising super quick into the direction of the axis we command the force to. (Y axis, up down, X axis in/out of the object we are in contact with)

@stefanscherzinger If you want to debug this issue I can probably also provide additional information about our setup via mail.