-
Notifications
You must be signed in to change notification settings - Fork 2.8k
Description
Question
Hi, Isaac Lab team, we are working with the hardware of ur5e with robotiq 2f85 gripper and I'm trying to make the reach policy for UR robot happen in real robot. There are 2 ways to approach this as described earlier use ros2 control or develop a controller which you can closely mimic for robot as well as your simulated asset. I came across this article which was interesting and I believe this would be of great use if you could provide more information on the implementation aspect of it.
- https://developer.nvidia.com/blog/bridging-the-sim-to-real-gap-for-industrial-robotic-assembly-applications-using-nvidia-isaac-lab/
- https://github.com/isaac-sim/IsaacLab/discussions/2343
I wrote a impedence controller using ur_rtde which directly applies torque_command to real robot which works like a simple torque_pd_control in joint space. I tuned it for real robot by sending a sinusoid to each joint and getting the optimal stiffness and damping gains which gave me decent trajectory following. However I wasn't able to make the robot work with the hierarchical dual control where you run higher level impedence loop on teach pendant at 500hz and send only position setpoints from external pc like using socket.
In summary I tried two approaches,
- Single stage PD control only using rtde directly working at 60hz and communicating with real robot no hardware sync issues and has decent tracking
- Two stage where the teach pendant impedence loop listens to a socket at 60hz for desired joint position updates via socket string and executes the main control compensating for dynamics at 500hz. This approach made the controller fight itself and its struggling, partly could be because i only send joint position updates without any velocity or acceleration profile, and its data hungry too. So I was wondering how did this blog address this issue and properly ensuring this two stage approach was working the way its desired to do. ( In this loop i did numerical desired velocity and acceleration calculation { new-prev/dt }
My goal is to transfer policy with good constraints set in simulation including modeling of delay using delayed PD actuator model and make the task happen on real robot. Since the intention of the blog was to explain people how to do it, I was wondering if it would be possible to help me out with this issue.
TODO:
Make the actuator model available in Isaaclab follow same control law and model delay so I could do sim 2 real, and also ensure impedence control works as the way isaaclab would output updates.
import time
import numpy as np
from rtde_control import RTDEControlInterface as RTDEControl
from rtde_receive import RTDEReceiveInterface as RTDEReceive
def main():
# RTDE setup
rtde_frequency = 60.0
rtde_c = RTDEControl("192.168.1.102", rtde_frequency,
RTDEControl.FLAG_VERBOSE | RTDEControl.FLAG_UPLOAD_SCRIPT)
rtde_r = RTDEReceive("192.168.1.102", rtde_frequency)
# --- Joint PD torque control with ABSOLUTE positions ---
# UR5e max torques
max_torque = np.array([150.0, 150.0, 150.0, 28.0, 28.0, 28.0])
# Example gains (tune these for your application)
Kp = max_torque / np.array([0.25, 0.25, 0.5, 1, 1, 1])
Kd = max_torque / (np.pi * 0.5)
print("Connected to robot!")
print("Kp (stiffness):", Kp)
print("Kd (damping):", Kd)
print("Max torques:", max_torque)
# Get initial robot position
current_q_startup = np.array(rtde_r.getActualQ())
print("\nInitial joint angles (rad):", current_q_startup)
print("Initial joint angles (deg):", np.rad2deg(current_q_startup))
# Initialize
q_err = np.zeros(6)
q_err_prev = np.zeros(6)
dt = 1.0 / rtde_frequency
# PD Controller loop
timecounter = 0.0
print(f"\nStarting PD controller at {rtde_frequency} Hz")
print("Press Ctrl+C to stop\n")
try:
while True:
t_start = rtde_c.initPeriod()
# ===== DEFINE ABSOLUTE TARGET POSITION =====
amplitude = np.deg2rad(15)
frequency = 0.2
q_desired = current_q_startup.copy()
q_desired[0] = current_q_startup[0] + amplitude * np.sin(2 * np.pi * frequency * timecounter)
# ===== END TARGET DEFINITION =====
current_q = np.array(rtde_r.getActualQ())
q_err_prev = q_err
q_err = q_desired - current_q
q_err_d = (q_err - q_err_prev) / dt
torque_stiffness = Kp * q_err
torque_damping = Kd * q_err_d
torque_damping_clamped = np.clip(torque_damping,
a_min=0.2 * -max_torque,
a_max=0.2 * max_torque)
torque_target = torque_stiffness + torque_damping_clamped
torque_target = np.clip(torque_target, a_min=-max_torque, a_max=max_torque)
rtde_c.directTorque(torque_target.tolist())
if int(timecounter * 10) % 10 == 0:
print(f"\n=== t: {timecounter:.1f}s ===")
print(f"Target (deg): {np.rad2deg(q_desired)}")
print(f"Current (deg): {np.rad2deg(current_q)}")
print(f"Error (deg): {np.rad2deg(q_err)}")
print(f"Stiffness torque: {torque_stiffness}")
print(f"Damping torque: {torque_damping_clamped}")
print(f"Total torque: {torque_target}")
rtde_c.waitPeriod(t_start)
timecounter += dt
except KeyboardInterrupt:
print("\n\nStopping controller...")
finally:
zero_torque = [0.0, 0.0, 0.0, 0.0, 0.0, 0.0]
rtde_c.directTorque(zero_torque)
print("Controller stopped - torques set to zero")
rtde_c.stopScript()