Joint torque control - no motion with small torques

Technical Questions
1

I am developing a teleoperation system for my UR5e, where the user manually guides a smaller “leader” arm while a controller is designed to make the UR5e follow the same motions. I have started using the new joint torque control interface to do so, because using joint torques allows the robot to make low-impact contact with the environment without entering a protective stop due to path deviation.

My approach is to use a PD controller on joint position and velocity errors to generate the motor torque commands. I am using the ur_rtde package in Python to communicate with the arm. The logic is roughly:

Kp = np.array([200, 150, 150, 25, 25, 20])
Kd = 0.2 * Kd

# this runs in a 500Hz control loop
# qd and vd are desired joint position and velocity, q and v are the actual values
torque_cmd = Kp * (qd - q) + Kd * (vd - v)

# send using directTorque...

After some tuning of the gains, this works quite well. The only problem we’ve run into is that there appears to be a small “deadzone” around the origin, such that small enough torques produce no movement of the arm. This means that the above controller has some steady-state joint position error. Whether the friction_comp argument of directTorque is true or false does not appear to make too much difference.

Has anyone run into this problem? What have you done about it? One possibility is to add an integral term to the controller to eliminate the steady-state error, but this requires even more tuning and concerns about stability, etc.

2

Update: I tried recalibrating the payload in case errors there were causing incorrect internal gravity compensation torques, but this did not seem to change the observed behaviour.

3

Hello,

I am testing too impedance control sending direct torque commands to UR5e using ROS2. It also appears that there might be a deadzone evevn with high stiffness gains. Not sure yet if its bec of friction or incorrect gravity compensation.

I was thinking of adding an integrate term. Have you tried that ?

4

Did you make sure to use the latest release? There has been a huge improvement on the friction model used with direct_torque:

5

Hey Felix,

Thanks for your reply.

It was improved significantly with the direct torque control v2. So, it was a friction problem. Thank you for the update.

6

Hi Nabil,

Thank you for sharing the update. It’s encouraging to hear that Direct Torque Control V2 works for you.

I’m currently working on a UR5e running PolyScope 5.25.2 and trying to use direct torque control for learned policies (VLA / Diffusion Policy). Unfortunately, I’m still facing issues with tracking error accumulation, oscillations, and maintaining stable, precise motion over longer horizons.

Would you be willing to share some details about your setup and controller implementation?

1. Control law

  • What exact torque control equation are you using?
  • Are you adding any feedforward terms?
  • Did you end up using an integral term to eliminate steady-state error?

2. Controller gains

  • What Kp and Kd values worked well for you for UR5e?

3. Communication / Control Interface

  • Are you using:
    • URScript direct_torque()
    • RTDE Python API (ur_rtde)
    • ROS2 (ur_robot_driver / ur_client_library)
  • Are torque commands computed externally on a PC or running directly on the controller?

4. Robot configuration

  • Any specific Payload or TCP configuration?
  • Any calibration steps that were important for achieving good performance?

5. Performance

  • What tracking accuracy are you seeing?
  • Is there any noticeable dead zone with V2?

Any details, gain values, code snippets, would be helpful.

Thank you!

7

Hello Shahrukh,

I am using CRISP controllers (Getting started controller details - CRISP Controllers - Learning Systems and Robotics Lab) in ROS2. If you want to use it with UR: GitHub - lvjonok/pixi_ur_ros2: UR ready-to-run pixi environment · GitHub.

  1. I tested impedance and an admittance-impedance control schemes. No I didnt use an integral term. It might be useful but I thought that in situations where you have localization error and you need compliance to place an object, then this integral term might build up force and affect the “compliance” of the robot. But that surely depends on the application and might be worth it to test. I currently dont use any feedforward term although its available.
  2. I use the CRISP controller in ROS2. So it might be different equation with different values but anyway for impedance Kp [900,900,1100,70,50,1] and damping is based on critical damping formula with zeto of 1. (I am prefer to make it more overdamped zeto > 1 to remove oscillations)
  3. using ROS2 where it uses the torque hardware command interface in ros2 control and underneath it, it uses the same direct_torque() function.
  4. I have a gripper so TCP is shifted and added its payload.
  5. Not sure about tracking exactly but I think around 5 mm (can differ between axes) and few degrees when using high stiffness. Depends how compliant you want as that affects the tracking accuracy. Yes, there is still some kind of deadzone. But I believe thats the nature of compliance.

In case you are ok with using ROS2, feel free to check the pixi ur repo on github (it need a bit of update to use the latest features from crisp controllers main package but you can easily change the version of the crisp_controllers to the latest one. I am planning a PR soon to update it).

Also, did you try the policy without the compliance to know if the oscillations are from the policy or the controller ? and vice versa the controller without the policy?

I hope this helps

8

Thank you for your reply.

I took a look at the pixi_ur_ros2 implementation and went through the controller code. For now, however, I am mainly focusing on a URScript-based approach where I stream joint position targets and use the direct torque control interface to track them.

My current implementation is a simple joint-space PD controller:

tau[j] = Kp[j] * (q_ref[j] - q[j]) - Kd[j] * qd[j]

I am not using a mass matrix, Coriolis compensation, or explicit gravity compensation since direct_torque() already performs gravity compensation internally.

I have also tested a fairly large range of gains. For joints J1-J6, I have experimented with Kp values roughly between 700 and 1500, but I consistently run into the same trade-off:

  1. If I increase gains to reduce oscillations and tracking error, the robot starts producing noticeable humming/noise and appears to be under higher motor load.

  2. If I reduce gains to avoid the humming and make the motion smoother, the tracking error increases significantly, sometimes growing from sub-degree levels to several degrees.

I also tested your gain set:

Kp = [900, 900, 1100, 70, 50, 1] with damping computed from the critical damping formula (ζ = 1).

One thing I was curious about: is there a specific reason you chose Kp = 1 for Wrist 3 (J6)? In my experiments, such a low gain on J6 results in very large tracking errors for that joint.

The bigger challenge is when I run actual VLA / diffusion-policy rollouts. ServoJ performs very well and the robot almost always completes the task successfully. With direct torque control, however, I typically observe gradual drift after the initial steps. The accumulated tracking error eventually causes task failure, even though the commanded trajectory itself is reasonable.

I also experimented with the pixi_ur_ros2 setup. While it works, I still found that Kp/Kv tuning is critical.

I think someone from the UR team or the community may be able to provide better guidance on tuning gains for the new direct_torque() API. Since gravity compensation and other low-level dynamics are handled internally by the UR controller, there may be a more effective tuning approach. My main challenge is to make long-horizon learning-based policies execute reliably.
Thank you!

there my 20 waypoint tracking logs , sorry for being too long.

`Waypoints: 20
Trajectory: 5693 steps (113.9s at 50Hz)
Max step: 0.05 deg/joint
Settle time: 1.0s per waypoint
Controllers to test: [‘1’, ‘2’, ‘servoj’]
Total time: ~5.7 min

======================================================================
TESTING: C1
Uploading URScript (4345 bytes)…
┌─ UR Official PD + Alpha @500Hz ─────────────────┐
│ MODE: POSITION (absolute joint targets) │
│ q_smooth += alpha*(q_target - q_smooth) │
│ tau = Kp*(q_smooth - q) - Kd*qd │
│ tau = clamp(tau, max_torque) │
│ Kp = [1500.0, 1500.0, 1000.0, 2000.0, 1000.0, 1000.0]
│ Kd = [77.5, 115.6, 86.7, 72.1, 51.0, 56.6]
│ max_torque = [150.0, 150.0, 150.0, 28.0, 28.0, 28.0]
│ alpha = 1.0 | coriolis=0 | M(q)=0
│ max_delta_tau = 0.5
│ max_speed = 1.5 rad/s (86 deg/s)
└─────────────────────────────────────────────────┘
Running 5693 steps (113.9s at 50Hz)…
step 250/5693 (5.0s) max=0.180° | J0=0.076 J1=0.000 J2=0.082 J3=0.049 J4=0.058 J5=0.180
step 500/5693 (10.0s) max=0.209° | J0=0.209 J1=0.097 J2=0.106 J3=0.057 J4=0.080 J5=0.175
step 750/5693 (15.0s) max=0.181° | J0=0.119 J1=0.027 J2=0.059 J3=0.052 J4=0.089 J5=0.181
step 1000/5693 (20.0s) max=0.177° | J0=0.042 J1=0.139 J2=0.086 J3=0.056 J4=0.060 J5=0.177
step 1250/5693 (25.0s) max=0.183° | J0=0.143 J1=0.095 J2=0.103 J3=0.062 J4=0.051 J5=0.183
step 1500/5693 (30.0s) max=0.130° | J0=0.028 J1=0.130 J2=0.026 J3=0.020 J4=0.043 J5=0.021
step 1750/5693 (35.0s) max=0.042° | J0=0.006 J1=0.006 J2=0.030 J3=0.042 J4=0.011 J5=0.022
step 2000/5693 (40.0s) max=0.265° | J0=0.265 J1=0.106 J2=0.164 J3=0.044 J4=0.020 J5=0.019
step 2250/5693 (45.0s) max=0.300° | J0=0.055 J1=0.234 J2=0.300 J3=0.125 J4=0.026 J5=0.019
step 2500/5693 (50.0s) max=0.119° | J0=0.030 J1=0.077 J2=0.119 J3=0.020 J4=0.060 J5=0.018
step 2750/5693 (55.0s) max=0.219° | J0=0.015 J1=0.009 J2=0.219 J3=0.023 J4=0.085 J5=0.019
step 3000/5693 (60.0s) max=0.262° | J0=0.262 J1=0.024 J2=0.212 J3=0.023 J4=0.026 J5=0.018
step 3250/5693 (65.0s) max=0.246° | J0=0.246 J1=0.073 J2=0.192 J3=0.009 J4=0.027 J5=0.020
step 3500/5693 (70.0s) max=0.068° | J0=0.036 J1=0.068 J2=0.065 J3=0.014 J4=0.059 J5=0.022
step 3750/5693 (75.0s) max=0.172° | J0=0.172 J1=0.150 J2=0.080 J3=0.018 J4=0.169 J5=0.023
step 4000/5693 (80.0s) max=0.309° | J0=0.309 J1=0.205 J2=0.285 J3=0.131 J4=0.025 J5=0.020
step 4250/5693 (85.0s) max=0.250° | J0=0.250 J1=0.056 J2=0.060 J3=0.014 J4=0.024 J5=0.022
step 4500/5693 (90.0s) max=0.181° | J0=0.071 J1=0.126 J2=0.151 J3=0.079 J4=0.074 J5=0.181
step 4750/5693 (95.0s) max=0.183° | J0=0.110 J1=0.064 J2=0.165 J3=0.078 J4=0.048 J5=0.183
step 5000/5693 (100.0s) max=0.324° | J0=0.107 J1=0.120 J2=0.324 J3=0.075 J4=0.093 J5=0.178
step 5250/5693 (105.0s) max=0.180° | J0=0.098 J1=0.104 J2=0.134 J3=0.079 J4=0.052 J5=0.180
step 5500/5693 (110.0s) max=0.186° | J0=0.109 J1=0.044 J2=0.115 J3=0.086 J4=0.042 J5=0.186

======================================================================
TESTING: C2
Uploading URScript (4336 bytes)…
┌─ UR Official PD + A@500Hz ─────────────────┐
│ MODE: POSITION (absolute joint targets) │
│ q_smooth += alpha*(q_target - q_smooth) │
│ tau = Kp*(q_smooth - q) - Kd*qd │
│ tau = clamp(tau, max_torque) │
│ Kp = [900.0, 900.0, 1100.0, 70.0, 50.0, 1.0]
│ Kd = [72.0, 107.51892856609017, 109.1406432086599, 16.18888507587845, 13.682105101189654,
2.146625258399798]
│ max_torque = [150.0, 150.0, 150.0, 28.0, 28.0, 28.0]
│ alpha = 1.0 | coriolis=0 | M(q)=0
│ max_delta_tau = 0.5
│ max_speed = 1.5 rad/s (86 deg/s)
└─────────────────────────────────────────────────┘
Running 5693 steps (113.9s at 50Hz)…
step 250/5693 (5.0s) max=12.352° | J0=0.124 J1=0.146 J2=0.088 J3=1.020 J4=0.155 J5=12.352
step 500/5693 (10.0s) max=24.851° | J0=0.188 J1=0.099 J2=0.108 J3=0.490 J4=0.242 J5=24.851
step 750/5693 (15.0s) max=34.970° | J0=0.438 J1=0.266 J2=0.072 J3=1.479 J4=0.331 J5=34.970
step 1000/5693 (20.0s) max=14.569° | J0=0.119 J1=0.046 J2=0.086 J3=0.658 J4=0.415 J5=14.569
step 1250/5693 (25.0s) max=18.936° | J0=0.369 J1=0.211 J2=0.121 J3=0.643 J4=0.504 J5=18.936
step 1500/5693 (30.0s) max=22.935° | J0=0.102 J1=0.041 J2=0.010 J3=0.375 J4=0.499 J5=22.935
step 1750/5693 (35.0s) max=22.935° | J0=0.102 J1=0.075 J2=0.015 J3=0.407 J4=0.468 J5=22.935
step 2000/5693 (40.0s) max=22.932° | J0=0.466 J1=0.126 J2=0.148 J3=0.491 J4=0.194 J5=22.932
step 2250/5693 (45.0s) max=22.931° | J0=0.138 J1=0.340 J2=0.291 J3=0.990 J4=0.147 J5=22.931
step 2500/5693 (50.0s) max=22.930° | J0=0.082 J1=0.311 J2=0.154 J3=0.512 J4=0.593 J5=22.930
step 2750/5693 (55.0s) max=22.930° | J0=0.108 J1=0.114 J2=0.069 J3=1.109 J4=0.687 J5=22.930
step 3000/5693 (60.0s) max=22.932° | J0=0.397 J1=0.110 J2=0.212 J3=1.108 J4=0.368 J5=22.932
step 3250/5693 (65.0s) max=22.934° | J0=0.373 J1=0.249 J2=0.189 J3=1.093 J4=0.371 J5=22.934
step 3500/5693 (70.0s) max=22.934° | J0=0.082 J1=0.088 J2=0.077 J3=0.150 J4=0.899 J5=22.934
step 3750/5693 (75.0s) max=22.936° | J0=0.256 J1=0.209 J2=0.092 J3=0.186 J4=1.165 J5=22.936
step 4000/5693 (80.0s) max=22.934° | J0=0.333 J1=0.218 J2=0.289 J3=0.818 J4=0.217 J5=22.934
step 4250/5693 (85.0s) max=22.934° | J0=0.383 J1=0.063 J2=0.054 J3=0.315 J4=0.227 J5=22.934
step 4500/5693 (90.0s) max=13.484° | J0=0.217 J1=0.145 J2=0.144 J3=0.642 J4=0.730 J5=13.484
step 4750/5693 (95.0s) max=1.128° | J0=0.373 J1=0.083 J2=0.139 J3=0.520 J4=1.128 J5=0.988
step 5000/5693 (100.0s) max=11.506° | J0=0.141 J1=0.202 J2=0.298 J3=0.540 J4=0.608 J5=11.506
step 5250/5693 (105.0s) max=23.353° | J0=0.173 J1=0.346 J2=0.128 J3=0.548 J4=1.279 J5=23.353
step 5500/5693 (110.0s) max=35.853° | J0=0.344 J1=0.068 J2=0.113 J3=0.581 J4=1.020 J5=35.853

======================================================================
TESTING: ServoJ
Running 5693 steps (113.9s at 50Hz)…
step 250/5693 (5.0s) max=0.350° | J0=0.055 J1=0.006 J2=0.146 J3=0.133 J4=0.001 J5=0.350
step 500/5693 (10.0s) max=0.350° | J0=0.056 J1=0.007 J2=0.148 J3=0.133 J4=0.004 J5=0.350
step 750/5693 (15.0s) max=0.350° | J0=0.054 J1=0.006 J2=0.143 J3=0.133 J4=0.005 J5=0.350
step 1000/5693 (20.0s) max=0.352° | J0=0.054 J1=0.008 J2=0.146 J3=0.133 J4=0.003 J5=0.352
step 1250/5693 (25.0s) max=0.351° | J0=0.055 J1=0.005 J2=0.149 J3=0.131 J4=0.002 J5=0.351
step 1500/5693 (30.0s) max=0.002° | J0=0.001 J1=0.002 J2=0.000 J3=0.001 J4=0.001 J5=0.000
step 1750/5693 (35.0s) max=0.002° | J0=0.001 J1=0.001 J2=0.002 J3=0.000 J4=0.001 J5=0.001
step 2000/5693 (40.0s) max=0.350° | J0=0.350 J1=0.011 J2=0.018 J3=0.000 J4=0.004 J5=0.000
step 2250/5693 (45.0s) max=0.352° | J0=0.027 J1=0.352 J2=0.004 J3=0.336 J4=0.001 J5=0.001
step 2500/5693 (50.0s) max=0.001° | J0=0.001 J1=0.001 J2=0.001 J3=0.001 J4=0.001 J5=0.000
step 2750/5693 (55.0s) max=0.024° | J0=0.024 J1=0.014 J2=0.018 J3=0.000 J4=0.001 J5=0.000
step 3000/5693 (60.0s) max=0.348° | J0=0.348 J1=0.093 J2=0.123 J3=0.001 J4=0.001 J5=0.000
step 3250/5693 (65.0s) max=0.349° | J0=0.349 J1=0.090 J2=0.144 J3=0.001 J4=0.001 J5=0.001
step 3500/5693 (70.0s) max=0.002° | J0=0.000 J1=0.001 J2=0.002 J3=0.000 J4=0.001 J5=0.000
step 3750/5693 (75.0s) max=0.350° | J0=0.173 J1=0.255 J2=0.165 J3=0.006 J4=0.350 J5=0.001
step 4000/5693 (80.0s) max=0.308° | J0=0.205 J1=0.124 J2=0.260 J3=0.308 J4=0.032 J5=0.000
step 4250/5693 (85.0s) max=0.350° | J0=0.350 J1=0.001 J2=0.002 J3=0.001 J4=0.001 J5=0.000
step 4500/5693 (90.0s) max=0.350° | J0=0.095 J1=0.137 J2=0.096 J3=0.190 J4=0.019 J5=0.350
step 4750/5693 (95.0s) max=0.349° | J0=0.094 J1=0.134 J2=0.092 J3=0.191 J4=0.020 J5=0.349
step 5000/5693 (100.0s) max=0.350° | J0=0.099 J1=0.135 J2=0.096 J3=0.190 J4=0.019 J5=0.350
step 5250/5693 (105.0s) max=0.352° | J0=0.095 J1=0.135 J2=0.094 J3=0.192 J4=0.019 J5=0.352
step 5500/5693 (110.0s) max=0.351° | J0=0.096 J1=0.135 J2=0.094 J3=0.192 J4=0.021 J5=0.351
| Controller | RMS (all) | Max (all) | RMS J1 | RMS J2 | RMS J3 | RMS J4 | RMS J5 | RMS J6 | TCP Avg | TCP Max |
| ---------- | --------- | --------- | ------ | ------ | ------ | ------ | ------ | ------- | ------- | ------- |
| C1 | 0.128° | 0.459° | 0.176° | 0.108° | 0.180° | 0.057° | 0.064° | 0.126° | 0.36 mm | 0.87 mm |
| C2 | 9.119° | 42.951° | 0.293° | 0.170° | 0.163° | 0.754° | 0.650° | 22.311° | 4.32 mm | 8.54 mm |
| ServoJ | 0.153° | 0.357° | 0.193° | 0.107° | 0.110° | 0.136° | 0.062° | 0.240° | 0.30 mm | 0.68 mm |

9

I finally had a chance to try out direct torque control v2 and from what I’ve seen so far it is indeed quite an improvement over v1.

10

Hi Shahrukh,

I will chime in here with my own experience using the direct torque interface for tracking VLA policies with a UR5e. So far I have only used direct torque control v1 for this purpose, and I expect v2 would be an improvement, but I’ll share some details about our setup that work well for us.

We running a ~500Hz control loop in Python via the ur_rtde package; our control law is

torque_cmd = Kp * (qd - q) + Kd * (vd - v) + Kf * sign(vd)

where Kp=[400, 300, 300, 50, 50, 40], Kd=0.2*Kp, and Kf=[2, 2, 2, 0.5, 0.5, 0.5]. The last term is a friction compensation term, which may now be unnecessary with direct torque v2. Note that we are also obtained a desired velocity term vd by interpolating the VLA chunk as a spline.

11

I also have similar humming noise if the robot is moving fast. I changed recently some stuff in the code so not sure if its from this or the v2 update.

Regarding kp=1 for W3, for my application I dont give any command to it and I ideally want to be compliance and move based on the env forces but its still stiff so I am using an outer admittance loop and made increased the Kp.

BTW the controller as I understand internally take into consideration the gravity and friction but not Coriolis and mass of the robot.

Maybe its worth trying to change a bit in the friction coefficients to make its tracking better under high gains. I am not sure about your application but maybe you can use admittance to get more accurate tracking.

12

It would be great if you could share your experience after trying v2 with our setup. Thanks.

13

Yes, that’s correct — the controller handles gravity and friction internally, not Coriolis or the mass matrix. From my 3 weeks of experimentation, simple PD is actually safer and more practical than trying to incorporate the mass matrix. I tested both full and diagonal mass matrix formulations and both performed worse than plain PD. The V2 friction API also behaves reasonably once you’re on 5.25 . As per me, tuning the gains, leave the other parameters alone is better . Modifying friction coefficients or adding inertia compensation introduces unpredictability that’s hard to reason about during contact.

14

Hi Adam,

Thanks for the detailed breakdown of your V1 setup — the control law and spline interpolation approach is very close to what I’m trying to implement.
A few questions if you don’t mind:

  1. On your VLA setup: I’m guessing you’re using an action chunking policy (e.g. ACT or similar) where the VLA predicts a chunk of ~20–30 steps, and you’re doing spline interpolation over that predicted chunk to get smooth 500Hz torque commands? Would be great to confirm the architecture.
  2. On the V1 friction + spline approach: If your V1 setup is working reliably, I’d love to try it. The part I’m less clear on is boundary handling — when you finish executing one predicted chunk and the next one arrives, how are you ensuring continuity at the seam? Are you blending, discarding the tail, or just letting the spline re-anchor at the current state?
  3. On V2 migration: If you’ve managed to get V2 working with your tuned gains and control law, that would save me an enormous amount of time — I’ve been struggling with the V2 friction API behavior and gain re-tuning .
    If you’re open to it, sharing either a code snippet or a repo link for both version would be incredibly valuable. I’m actively working on this for a VLA-based tasks on a UR5e and the alignment with your setup is very close.
15

I agree. I also tried adding mass matrix to use OSC but it was very unstable. Dropping it (not applying inertia shaping) is usually common. Coriolis might be easy to implement and helpful.

16

Our V1 friction + spline approach looks roughly like the following Python code. In particular, we slow to a stop between each executed chunk, so we can easily interpolate from one chunk to the next.

# quintic trajectory to enforce zero velocity and acceleration at
# endpoints
# ts and qs (times and joint positions) are provided by a policy's predicted 
# action chunk, with the current time and position prepended
bc = [(1, np.zeros(6)), (2, np.zeros(6))]  # vel and acc both zero
qspline = make_interp_spline(ts, qs, k=5, axis=0, bc_type=(bc, bc))  # from scipy.interpolate
vspline = qspline.derivative()

t = 0
while True:
  q = rtde_r.getActualQ()
  v = rtde_r.getActualQd()
  qd = qspline(t)
  vd = vspline(t)
  torque_cmd = Kp * (qd - q) + Kd * (vd - v) + Kf * np.sign(vd)
  rtde_c.directTorque(torque_cmd)
  t += dt
  # sleep

I have not integrated direct torque v2 into our pipeline yet, and it will probably be some time before I do so.

18

Hi Adam,

Thanks for sharing your controller implementation.

I tried both your spline-based controller and a simple PD controller with direct_torque V2. The robot is stable, but I always see a small joint position tracking error, so the joints never fully converge to the commanded positions. For contact-rich manipulation, this small error accumulates and eventually causes the learned policy to fail.

I think the main challenge is gain tuning. I’ve tried many Kp/Kd combinations but haven’t found a good sweet spot for direct_torque V2. Interestingly, the same policy works well with servoJ; the only reason I’m moving away from it is because contact forces eventually trigger protective stops.

If you later experiment with direct_torque V2 and have any gain tuning recommendations, I’d be very interested to hear your experience.