High Frequency Torque Control

@Ossama_Ahmed @toni.sm I’m trying to achieve custom torque controller in IsaacSim using standalone workflow, where torque control is only supported in python api called ‘set_articulation_dof_efforts’. However, in my test the control frequencey is limited up to 250-300hz because of ‘world.step()’ takes some time. The 250-300hz is not enough for my custom real-time closed-loop torque controller(1khz-10khz). How to improve the IsaacSim control frequency?

Hi Newuhe,
I have tried to implement a torque control with a standalone application before, however, I used the physics callback function to write the command to the joints. I did not make a test the frequency but it might be worth to try

Do you mean put the ‘set_articulation_dof_efforts’ into physics callback function will make world.step() faster?

No, Do not try to control when to take the step from the simulation. Instead, add a callback function that reacts to the physics steps.

self._world.add_physics_callback("sending_actions", callback_fn=self.send_robot_actions)

def send_robot_actions(self, step_size):
            self._igo.get_articulation_controller().switch_control_mode(mode='effort')
            self._igo.apply_wheel_actions(ArticulationAction( ... # place the joints action here # ... ))
        return

Something close to this form

Can an Isaac developer confirm whether there is a difference (in terms of performance, physics accuracy,…) between :

  1. Applying torque by using robot.set_joint_efforts(efforts) before calling world.step(render=False)
  2. Doing the same thing but in a physic callback.

Note : the render=False in the step call is to do only one physic step as the effort needs to be applied each physic step and not each render step.

Thanks,
Titouan Le Marec
CEO Nimble One

I’ve tried the two ways but there is no difference between the control frequency.

Hi @newuhe

Mmmm, I’m probably missing something but… what about modifying physics_dt to adjust the frequency and program the control to write the command using the physics callback function as @Ossama_Ahmed comments

I’ve already set the physics_dt to 1/10000 but the frequency doesn’t improve. I notice that the backend physx is similar to bullet. However, in my test bullet has the ability to be controlled to 10khz while IsaacSim is limited to 300hz.

Hi @newuhe

Just to be clear, are you referring to performing control only in simulation using Isaac Sim (and then evaluating it in the real world in a separate process) or to controlling, from simulation, a real system?

I’m aimed to control only in simulation using a custom controller already applied in real robot. The controller sends torque command in 10khz.

What is the timestep size you get from inside the physics callback when physics_dt is 1/10000?

Quick tests with the standalone_examples/api/omni.isaac.core/time_stepping.py script give me the expected simulation time step:

simulate with step: 9.999999747378752e-05

I got ‘simulate with step: 9.999999747378752e-05’. And I think the control frequnce limit is due to world.step() taskes too much time.

I have carried out the following test to do a position control with a (non-tuned) PID applying efforts to the DOFs.
From the physics simulation perspective, it controls the robot at 10KHz (physics step 1/10000).
From the real-time simulation perspective, it is not a real-time simulation (the computer model does not run at the same rate as the real world).

Simulation/computation time is too expensive to simulate in real-time at this frequency…

Results with rendering disabled and in headless mode (Unplugged laptop with RTX 3080)

  • simulated time (s): 1.0
  • real-world time required (s): 18.60733389854431
  • speedup: 0.053742250526188064

But… from the simulation/control perspective, the action is sent at 10KHz…


import time
import numpy as np
from omni.isaac.kit import SimulationApp

simulation_app = SimulationApp({"headless": False})

from omni.isaac.core import SimulationContext
from omni.isaac.core.utils.nucleus import get_assets_root_path
from omni.isaac.core.utils.stage import add_reference_to_stage
from omni.isaac.dynamic_control import _dynamic_control

assets_root_path = get_assets_root_path()
asset_path = assets_root_path + "/Isaac/Robots/Franka/franka_alt_fingers.usd"

simulation_context = SimulationContext(stage_units_in_meters=1.0)
add_reference_to_stage(asset_path, "/Franka")
simulation_context.initialize_physics()

dci = _dynamic_control.acquire_dynamic_control_interface()
articulation = dci.get_articulation("/Franka")
assert articulation != _dynamic_control.INVALID_HANDLE

target = np.array([0, -1.57, 0, -1.57, 0, 1.57, 0, 0, 0], dtype=np.float32)
previous_error = np.zeros_like(target)
integral = 0

def control(current, dt, Kp=1000, Ki=100, Kd=10):
    global integral, previous_error

    error = target - current
    integral += error * dt
    derivative = (error - previous_error) / dt
    previous_error = error

    return Kp * error + Ki * integral + Kd * derivative

def step_callback(step_size):
    # print("simulate with step: ", step_size)

    dof_states = dci.get_articulation_dof_states(articulation, _dynamic_control.STATE_POS)
    effort = control(dof_states["pos"], step_size)

    dci.wake_up_articulation(articulation)
    dci.set_articulation_dof_efforts(articulation, effort)

def render_callback(event):
    # print("update app with step: ", event.payload["dt"])
    pass


simulation_context.add_physics_callback("physics_callback", step_callback)
simulation_context.add_render_callback("render_callback", render_callback)
simulation_context.stop()
simulation_context.play()

dof_props = dci.get_articulation_dof_properties(articulation)
print("dof props: ", dof_props)

for i in range(dci.get_articulation_dof_count(articulation)):
    dof_props["driveMode"][i] = _dynamic_control.DriveMode.DRIVE_FORCE
    dof_props["stiffness"][i] = 0.0
    dof_props["damping"][i] = 0.0
dci.set_articulation_dof_properties(articulation, dof_props)

dof_props = dci.get_articulation_dof_properties(articulation)
print("dof props: ", dof_props)

simulation_steps = 100000
frequency = 10000.0
print("step physics at a 1/10000s per step and rendering at 1.0/50s (divisible values)")

start = time.time()
for i in range(simulation_steps):
    simulation_context.set_simulation_dt(physics_dt=1.0 / frequency, rendering_dt=1.0 / 50.0)
    simulation_context.step(render=True)
end = time.time()

print()
print("simulated time (s): ", simulation_steps / frequency)
print("real time (s): ", end - start)
print("speedup: ", simulation_steps / frequency / (end - start))
print()

print("cleanup and exit")
simulation_context.stop()
simulation_app.close()

Target position:


Upsss, the video was recorded with an error, already fixed, in the code: previous_error = target.copy()

I test the script in my env and it seems the simulation receives outward commands at a maximum frequency of 1khz. If there is demand to control it at 10khz, i have to turn to underlying physx c++ controller for help.