matthias-mayr commented 1 year ago

Problem description I am setting up a UR5e setup in simulation and reality right now. With an older version of this controller package we found a simulation and real-robot configuration that we were happy with.

Now with the latest version of this package, I am getting very unstable behavior in simulation (haven't tried the real robot yet) and none of the gain & solver configurations that I saw here in the issues worked for me.

Software versions

Commit: 41a11e2efac7125c9795f1365ae158fa82b8f6c8
OS: Ubuntu 20.04
ROS version: ROS 1 Noetic
Robot: UR5e

To Reproduce Start a UR5e Gazebo setup with the following configuration:

cartesian_compliance_controller:
    type: "position_controllers/CartesianComplianceController"
    end_effector_link: "ur5e_tcp_link"      # All links below must come before this link
    robot_base_link: "ur5e_base_link"       # Which is the "world" frame of this controller
    ft_sensor_ref_link: "ur5e_wrist_3_link"
    compliance_ref_link: "ur5e_tcp_link"    # Specify based on which frame you want to be compliant
    target_frame_topic: "target_frame"      # Name of the topic where it receives commands
    joints:
     - ur5e_shoulder_pan_joint
     - ur5e_shoulder_lift_joint
     - ur5e_elbow_joint
     - ur5e_wrist_1_joint
     - ur5e_wrist_2_joint
     - ur5e_wrist_3_joint

    stiffness:  # w.r.t. compliance_ref_link
        trans_x: 200
        trans_y: 200
        trans_z: 200
        rot_x: 20
        rot_y: 20
        rot_z: 20
    solver:
        error_scale: 0.2
        iterations: 3
    pd_gains:
        trans_x:
          p: 0.05
          d: 0.015
        trans_y:
          p: 0.05
          d: 0.015
        trans_z:
          p: 0.05
          d: 0.015
        rot_x:
          p: 3
          d: 0.01
        rot_y:
          p: 3
          d: 0.01
        rot_z:
          p: 3
          d: 0.01

and the following gains:

gazebo_ros_control/pid_gains:
  # from https://github.com/ros-industrial/universal_robot/pull/525/commits/77cc72fa846d0ce69bd4122769a5f8cd951cc517
  ur5e_shoulder_pan_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
  ur5e_shoulder_lift_joint: {p: 6000,  d: 40, i: 1, i_clamp: 1}
  ur5e_elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
  ur5e_wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
  ur5e_wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
  ur5e_wrist_3_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}

This configuration was stable until 90f8509: define high-level goals for the next development and broke with 6fc2898: Merge pull request #44 from fzi-forschungszentrum-informatik/change-robot-synchronization-policy.

Now the robot overshoots a lot. Sometimes it converges after some minutes, but most often it does do continuous large motions of 10-50cm and all sorts of rotations.

Expected behavior Stable performance in simulation.

If someone has a configuration that does not make the robot "dance", I'd be very happy to get the configuration or some hints.

matthias-mayr commented 1 year ago

A part of the issue are the PID gains. I tried these ones from the current configuration in the universal_robots repo as well:

  gains: # Required because we're controlling an effort interface
    shoulder_pan_joint: {p: 4000,  d: 200, i: 1, i_clamp: 1}
    shoulder_lift_joint: {p: 10000,  d: 200, i: 1, i_clamp: 1}
    elbow_joint: {p: 2000,  d: 20, i: 1, i_clamp: 1}
    wrist_1_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
    wrist_2_joint: {p: 500,  d: 1, i: 1, i_clamp: 1}
    wrist_3_joint: {p: 10,  d: 0.1, i: 0, i_clamp: 1}

They are also very unstable. Even for non-compliant position control with the standard "UR" scaled position trajectory controller. Not sure why. My understand was that the Gazebo PID gains do the same job as the ones specified in the effort_joint_trajectory_controller above, just in a different place.

As an addition: I rate-limit the Gazebo FT-sensor plugin values to a change of 100 N and 20 Nm a second, because of the noise.

When not using PID gains, current master branch is also very unstable, but it gets better with strongly rate-limited FT sensor feedback. Without the rate-limiter, this is not very surprising as the signal is very noise. About +-10 N on the z-Axis for example.

stefanscherzinger commented 1 year ago

@matthias-mayr

Thanks for reporting this! I haven't worked with Gazebo (classic) lately but I also think that it could be a stability problem of cascaded controller gains.

They are also very unstable. Even for non-compliant position control with the standard "UR" scaled position trajectory controller. Not sure why.

That's interesting. If not already done, could you open an issue there?

(haven't tried the real robot yet)

I recommend these default gains when working with real hardware. They feel natural on URe robots when manually guiding the end-effector and are stable in most of the robot's workspace on a stiff surface. You might fine tune from there. In your configuration from here, the d gains might already be too high.

I'll see if I can have a look at Gazebo.