Skip to content

Extreme Difficulty Tuning PID Gains for UR10e Robot with PositionJointInterface in Gazebo #708

@Vaisakhshan

Description

@Vaisakhshan

Environment

  • Robot: UR10e
  • ROS Version: Noetic
  • Gazebo Version: Gazebo multi-robot simulator, version 11.15.1
  • Package: universal_robot / ur_description
  • Use Case: Gazebo simulation with position control

Issue

I'm experiencing severe PID tuning difficulties when simulating a UR10e in Gazebo using the URDF from this repository. The robot requires extremely conservative PID gains (P=15, D=250) to remain stable, and even then exhibits poor tracking performance. I'm wondering if the inertial parameters in the URDF are optimized for Gazebo simulation or only represent the physical robot.

Current URDF Inertial Parameters

From the ur10e.urdf.xacro:

<!-- Main arm links -->
<link name="shoulder_link">
  <inertial>
    <mass value="7.369" />
    <origin rpy="1.570796326794897 0 0" xyz="0.021 -0.027 0.0" />
    <inertia ixx="0.03408" ixy="2e-05" ixz="-0.00425" 
             iyy="0.03529" iyz="8e-05" izz="0.02156" />
  </inertial>
</link>

<link name="upper_arm_link">
<inertial>
<mass value="13.051" />
<origin rpy="0 0 0" xyz="-0.2327 0.0 0.158" />
<inertia ixx="0.02814" ixy="5e-05" ixz="-0.01561"
iyy="0.77068" iyz="2e-05" izz="0.76943" />
</inertial>
</link>

<!-- Wrist links - very light -->
<link name="wrist_1_link">
<inertial>
<mass value="2.1" />
<inertia ixx="0.00296" iyy="0.00222" izz="0.00258" />
</inertial>
</link>

<link name="wrist_3_link">
<inertial>
<mass value="0.615" /> <!-- Lightest link -->
<inertia ixx="0.0004" iyy="0.00041" izz="0.00034" />
</inertial>
</link>

Joint Dynamics Configuration

<joint name="shoulder_pan_joint" type="revolute">
  <limit effort="330.0" lower="-6.28" upper="6.28" velocity="2.09" />
  <dynamics damping="5" friction="1" />
</joint>

<!-- Wrist joints -->
<joint name="wrist_3_joint" type="revolute">
<limit effort="54.0" lower="-6.28" upper="6.28" velocity="3.14" />
<dynamics damping="5" friction="1" />
</joint>

Simulation Behavior Problems

1. Wrist Joint Instability

The lightweight wrist links (especially wrist_3_link at 0.615 kg) cause extreme instability:

  • Peak velocity: 77 rad/s (far exceeds 3.14 rad/s limit)
  • Requires P=3-5, D=400-500 to stabilize
  • Normal PID values (P=100, D=10) cause violent oscillation

2. PID Tuning Requirements

Joint Group Required PID Standard PID Issue
Main joints P=15, D=250 P=100-500, D=10-50 Need 10x higher D gain
Wrist 1-2 P=8, D=200 P=100, D=10 Need 20x higher D gain
Wrist 3 P=5, D=400 P=100, D=10 Need 40x higher D gain!

3. Mass Ratio Issue

The mass ratio between links might be problematic for simulation:

  • upper_arm_link: 13.051 kg
  • wrist_3_link: 0.615 kg
  • Ratio: 21:1 - Could this cause numerical issues in Gazebo?

Questions

  1. Are these inertial values intended for Gazebo simulation? Or are they purely from CAD/real robot specs?

  2. Should wrist link masses be artificially increased for stable simulation? For example, increasing wrist_3_link mass from 0.615 kg to 1.5-2.0 kg?

  3. Are the inertia tensors correct? Some values seem very small (e.g., wrist_3_link ixx=0.0004). Could these cause integration issues?

  4. Should joint damping be higher for simulation? Current damping="5" seems low given the PID requirements. Should this be 20-50 for simulation?

  5. Is there a "simulation-optimized" version of the URDF? Some robot packages provide separate URDFs for hardware vs simulation.

Comparison with Real Hardware

Colleagues using the real UR10e report using standard PID values:

  • P: 100-300
  • I: 0.1-1.0
  • D: 10-50

This suggests the simulation behavior doesn't match reality.

Controller Configuration

# Using PositionJointInterface
joint_group_pos_controller:
  type: position_controllers/JointGroupPositionController
  joints: [shoulder_pan_joint, shoulder_lift_joint, elbow_joint, 
           wrist_1_joint, wrist_2_joint, wrist_3_joint]

gazebo_ros_control:
pid_gains:
shoulder_pan_joint: {p: 15.0, i: 0.3, d: 250.0}
# ... (very conservative values)

Current Workaround

Modified inertial properties (testing):

<!-- Artificially increase wrist masses for simulation -->
<link name="wrist_3_link">
  <inertial>
    <mass value="2.0" />  <!-- Increased from 0.615 -->
    <inertia ixx="0.002" iyy="0.002" izz="0.002" />  <!-- Scaled up -->
  </inertial>
</link>

This improves stability but I'm concerned about:

  • Breaking kinematic calibration
  • Affecting dynamics calculations
  • Deviating from real robot behavior

Request for Guidance

Primary Questions:

  1. Should I modify the URDF inertial parameters for simulation stability?
  2. Are there recommended best practices for using UR URDFs in Gazebo?
  3. Is there existing documentation on simulation vs hardware parameter differences?

Alternatively:

  • Should I switch to EffortJointInterface instead of PositionJointInterface?
  • Would using the ur_robot_driver with a simulated robot (not Gazebo) work better?

Any guidance from the community on simulating UR robots with stable control would be greatly appreciated!

Additional Context

  • Physics engine: ODE with 0.001s timestep
  • Using standard gazebo_ros_control plugin
  • Transmission: SimpleTransmission with 1:1 mechanical reduction
  • Same issue occurs with UR5e model (similar mass ratios)

Metadata

Metadata

Assignees

No one assigned

    Labels

    No labels
    No labels

    Type

    No type

    Projects

    No projects

    Milestone

    No milestone

    Relationships

    None yet

    Development

    No branches or pull requests

    Issue actions