NativeFrankaController

NativeFrankaController is the controller with awaitable methods, for asyncio code; Controller (Robot and Controller) runs it with plain calls. It has FrankaController’s constructor, methods and attributes (switch(), set(), move(), activate(), identify_payload(), …), documented in FrankaController. Below is what it adds: its loop in C++, record() and control laws.

The native control loop: NativeFrankaController, a drop-in for FrankaController.

FrankaController runs its 1 kHz control loop on the asyncio event loop, in Python. Anything else that runs there delays the next torque command: a planner, loading a model, garbage collection, the server’s command thread holding the GIL. When a command is late, the robot stops with communication_constraints_violation. NativeFrankaController runs the same loop in C++, in a thread that never waits for Python, and has FrankaController’s constructor, methods and attributes, so porting code means swapping the class:

>>> robot = RobotInterface("172.16.0.2")
>>> controller = NativeFrankaController(robot)  # instead of FrankaController(robot)
>>> await controller.start()
>>> controller.switch("osc")
>>> await controller.set("ee_desired", target)

Its impedance, pid, osc and torque laws are ports of FrankaController’s and send the same torques, to rounding. New laws are written in Python and compiled with Numba, see control_law().

What changes:

• The loop runs in C++, so a subclass’s step() would never run: NativeFrankaController refuses one. Write the step as a control law instead, and log every cycle with record().

• Attributes the loop reads (kp, q_desired, ee_desired, …) are views of its memory. Assigning one copies the value in, and the loop takes it whole at its next cycle.

• While the loop runs, robot.data and robot.robot_state follow it from the event loop, about every millisecond. In simulation the loop owns the simulated arm.

aiofranka.native.load_native()

Import the native control loop, aiofranka._native.

It uses pylibfranka’s libfranka, which it must have been built for.

Returns:

aiofranka._native

Return type:

module

Raises:

ImportError – If the extension is not built, or was built for another libfranka

class aiofranka.native.ControlLaw(function, params=None, memory=None, name=None)

Bases: object

A control law for NativeFrankaController, made with control_law().

name

The controller type that runs it, for switch()

Type:

str

function

The Python function

params

Shapes of its parameters, by name

Type:

dict

memory

Shapes of its memory, by name

Type:

dict

__init__(function, params=None, memory=None, name=None)

aiofranka.native.control_law(function=None, *, params=None, memory=None, name=None)

Make a Python function a control law of NativeFrankaController.

The native loop calls it at 1 kHz, compiled with Numba, so it runs without Python: write it with numpy and math only (Numba’s nopython mode). It gets four arguments:

• s: what the loop read at this cycle, FrankaController.state’s fields: qpos, qvel, ee (flange pose, 4x4), jac (6x7, linear rows first), mm (7x7), last_torque, and cycle, time [s] and dt (1e-3)

• p: the controller’s attributes: kp, kd, ki, ee_kp, ee_kd, null_kp, null_kd, q_desired, ee_desired, torque, initial_qpos, control_transform, torque_limit, torque_diff_limit, plus the law’s own params

• m: the law’s memory, zeroed when switch() selects the law; write to it

• tau: the 7 torques to fill [Nm]

Return nothing, or a nonzero integer to stop the loop with an error. With clip (the default), the loop then limits the torque rate and clips the torques, as the impedance law does.

Each of params becomes a controller attribute (zeros at first), which set() and activate() can change like kp. Laws that name the same parameter share it.

Parameters:

• params (dict) – Shapes of the law’s parameters, by name, e.g. {“stiffness”: 7}

• memory (dict) – Shapes of what the law keeps between cycles, e.g. {“integral”: 7}

• name (str) – The controller type for switch() (default: the function’s name)

Example

>>> @control_law(params={"stiffness": 7, "damping": 7}, memory={"integral": 7})
... def pi_damping(s, p, m, tau):
...     error = p.q_desired - s.qpos
...     m.integral[:] += error * s.dt
...     tau[:] = p.stiffness * error + 2.0 * m.integral - p.damping * s.qvel
>>> controller.switch(pi_damping)  # compiles it, a few seconds the first time
>>> controller.stiffness = np.full(7, 60.0)
>>> await controller.set("q_desired", target)

class aiofranka.native.Recording(loop, recorder, fields, dtype, path)

Bases: object

Every cycle of the native loop, from NativeFrankaController.record().

The loop writes a row per cycle into a buffer in C++, which the controller moves to Python about every 10 ms. stop() ends the recording; using it in a with block stops it at the end of the block.

fields

The recorded fields

Type:

tuple

path

Where stop() saves the rows (.npz), or None

Type:

Path

__init__(loop, recorder, fields, dtype, path)

property recording

Whether the loop still records into it.

property rows

Rows the loop recorded so far.

property dropped

Rows the loop dropped because Python had not taken the earlier ones in time.

data()

The rows so far.

Returns:

{field: numpy array with one row per cycle, oldest first}

Return type:

dict

save(path=None)

Save the rows so far to an .npz file, with one array per field.

Parameters:

path (str or Path) – Where (default: the recording’s path)

Returns:

The file

Return type:

Path

stop()

Stop recording, and save to the recording’s path if it has one.

Returns:

data()

Return type:

dict

class aiofranka.native.NativeFrankaController(robot)

Bases: FrankaController

FrankaController with its 1 kHz control loop in C++.

Same constructor, methods and attributes as FrankaController; see aiofranka.native for what changes. Requires the extension aiofranka._native.

Example

>>> robot = RobotInterface("172.16.0.2")
>>> controller = NativeFrankaController(robot)
>>> await controller.start()
>>> await controller.move()
>>> controller.switch("impedance")
>>> controller.set_freq(50)
>>> await controller.set("q_desired", target)
>>> await controller.stop()

kp

a view of its staging memory.

Type:

A controller attribute that the native loop reads

kd

a view of its staging memory.

Type:

A controller attribute that the native loop reads

ki

a view of its staging memory.

Type:

A controller attribute that the native loop reads

ee_kp

a view of its staging memory.

Type:

A controller attribute that the native loop reads

ee_kd

a view of its staging memory.

Type:

A controller attribute that the native loop reads

null_kp

a view of its staging memory.

Type:

A controller attribute that the native loop reads

null_kd

a view of its staging memory.

Type:

A controller attribute that the native loop reads

q_desired

a view of its staging memory.

Type:

A controller attribute that the native loop reads

ee_desired

a view of its staging memory.

Type:

A controller attribute that the native loop reads

initial_qpos

a view of its staging memory.

Type:

A controller attribute that the native loop reads

control_transform

a view of its staging memory.

Type:

A controller attribute that the native loop reads

torque_limit

a view of its staging memory.

Type:

A controller attribute that the native loop reads

realtime_priority = 80

realtime_cpu = None

__init__(robot)

Initialize controller with robot interface.

Parameters:

robot (RobotInterface) – Initialized robot interface

Note

Controller is initialized in “impedance” mode with conservative gains. Call start() to begin the control loop, then switch() to change modes.

Default Gains:

• Impedance: kp=80, kd=4 (per joint)

• OSC: ee_kp=[100]*6, ee_kd=[4]*6

• Null-space: null_kp=1, null_kd=1

property type

“impedance”, “pid”, “osc”, “torque”, or a custom law’s name.

Type:

Controller type

property clip

Whether the loop limits the torque rate and clips the torques.

property torque_diff_limit

Max torque rate [Nm/s], a number or one per joint.

property torque

The torque command of torque mode [Nm] (7,). In impedance and PID mode, the torque that the last cycle computed, as in FrankaController.

property running

Whether the native loop runs. Setting False stops it.

property state

qpos, qvel, ee, jac, mm and last_torque.

Type:

What the last cycle read

property last_command

The torque the last cycle sent, after the rate limit and clip [Nm] (7,).

property error_integral

The PID controller’s integral of the joint error (7,). Setting it resets it.

property law_memory

The memory of the running custom law, a copy with its fields, or None.

register_law(law)

Compile a control law and make it a controller type.

switch() then takes its name; switch(law) registers it too. Compiling takes a few seconds the first time, during which the running loop carries on.

Parameters:

law (ControlLaw) – A function decorated with control_law()

Returns:

The law’s name

Return type:

str

switch(controller_type)

FrankaController.switch(), which also takes a control law (see control_law()).

Like the PID integral, a custom law’s memory starts from zero.

Parameters:

controller_type (str | ControlLaw)

record(fields=None, seconds=60.0, path=None)

Record every cycle of the native loop, from its next one on.

Each cycle, after sending the torques, the loop writes the fields into a buffer in C++ without waiting for Python, and the controller moves them to Python about every 10 ms. If the event loop is blocked for longer than seconds, the loop drops rows and Recording.dropped counts them.

Fields, by name:

• of the cycle: cycle; time, the robot’s [s] (in simulation, the simulation’s); wall_time, the host’s time.time() when the robot state arrived [s]; busy, from then to the torque command [s]; q, dq, ee (flange pose), tcp (ee @ control_transform), jac, mm; last_torque, the robot’s tau_J_d (in simulation, the last command); tau, the torque sent, after the rate limit and clip; robot_mode (-1 in simulation)

• the controller’s attributes as the cycle used them: q_desired, ee_desired, kp, kd, ee_kp, …, mode, and the parameters of registered control laws

• every number of the robot state (NaN in simulation): tau_J, tau_J_d, dtau_J, q_d, dq_d, theta, dtheta, tau_ext_hat_filtered, O_F_ext_hat_K, K_F_ext_hat_K, control_command_success_rate, O_T_EE (flat and column-major, as in pylibfranka), …

Parameters:

• fields (list) – Fields to record (default: aiofranka.native.RECORD_FIELDS)

• seconds (float) – Size of the buffer in C++, in seconds of cycles

• path (str or Path) – An .npz file to save the rows to when the recording stops

Returns:

The rows; stop() it, or use it in a with block

Return type:

Recording

Raises:

• ValueError – If a field is unknown

• RuntimeError – If another recording is running

Example

>>> with controller.record(["time", "q", "tau", "tau_J_d"], path="control.npz"):
...     await run_policy(controller)

loop_stats(reset=False)

Timing of the native loop since start, or since the last call with reset.

Parameters:

reset (bool) – Start a new window

Returns:

count, mean, std, min and max of the period between cycles [s]; busy_mean

and busy_max, from a cycle’s start to its torque command [s]; what the robot reported (real robot only): robot_gap_max [s], the states missed in between, and success_min, its lowest command success rate. Since start: warn and error, periods off 1 ms by more than 0.1 ms or longer than 10 ms, and max_all [s].

Return type:

dict

async start()

Start the native 1 kHz control loop (FrankaController.start()).

Returns:

Watches the loop, and ends with it

Return type:

asyncio.Task

step()

Run one control cycle in the calling thread (FrankaController.step()).

For stepping a simulation, or the robot, by hand while the loop is not running.

initialize()

set_tcp(transform)

Set the tool center point (TCP) that the OSC controls, relative to the flange.

The OSC then tracks ee_desired with this frame instead of the flange. Express it in the flange frame (the attachment_site of the MuJoCo model): its origin is the center of the flange face, z points out of the flange, and the box on the side of link 7 points between +x and +y. The docs show it on the robot (Controllers > Operational Space Control > Tool Center Point). ee_desired becomes the current pose of the new TCP, so the arm holds still.

The TCP of the end-effector profile in Desk is not used here.

Parameters:

transform (array-like) – Pose of the TCP in the flange frame as a 4x4 transform, or its translation [m] (3,) for the flange’s orientation

Raises:

ValueError – If transform is not a translation or a 4x4 pose

Example

>>> controller.switch("osc")
>>> controller.set_tcp([0, 0, 0.1034])  # Franka Hand fingertips
>>> target = controller.ee_desired.copy()
>>> target[2, 3] -= 0.05  # 5 cm down, in the base frame
>>> await controller.set("ee_desired", target)

activate(config, check_tool=True, check_null_target=True)

Apply a controller configuration: its mode, gains and policy rate.

For impedance it sets kp and kd; for osc the TCP, ee_kp, ee_kd, null_kp, null_kd and the null-space target. It then sets the rate of set() to the configuration’s frequency. Like switch(), it holds the current joint positions (impedance) or TCP pose (osc). See aiofranka.config for the file format.

An OSC configuration without a null_target keeps the joint positions at activation as the null-space target, as switch() does. One with a null_target (e.g. a policy’s) is only activated with the arm already there, every joint within arrival_tolerance, the criterion move() stops at; otherwise the null space would swing the arm toward it at once. Move there first:

>>> config = aiofranka.load_config("configs/pocky/lv1_osc.yaml")
>>> await controller.move(config["null_target"])
>>> controller.activate(config)

Reading a file takes a few milliseconds, which delays the 1 kHz loop that long; while it runs, pass a configuration read before with aiofranka.load_config().

Parameters:

• config (str | Path | dict) – YAML file, or its contents

• check_tool (bool) – Refuse a configuration whose tool is not the end effector active in Desk (see check_tool())

• check_null_target (bool) – Refuse an OSC configuration whose null_target the arm is not at. Only skip it where the null space is known to be at rest, e.g. at a pose where the TCP Jacobian’s null space holds no error toward the target.

Returns:

The configuration, as aiofranka.load_config() reads it

Return type:

dict

Raises:

• ValueError – If the configuration is not valid

• RuntimeError – If its tool is not the one active in Desk, or the arm is not at its null_target

Example

>>> controller.activate("configs/osc.yaml")
>>> target = controller.ee_desired.copy()
>>> target[2, 3] -= 0.05
>>> await controller.set("ee_desired", target)  # at the configured frequency

async move(qpos=[0, 0, 0.0, -1.57079, 0, 1.57079, -0.7853])

Move robot to target joint position using smooth trajectory.

Generates a time-optimal, jerk-limited trajectory using Ruckig online trajectory generation, then executes it at 50 Hz. Automatically switches to impedance mode if not already active.

Parameters:

qpos (list | np.ndarray) – Target joint positions [rad] (7,) Default: Home position

Note

• Uses Ruckig for smooth, time-optimal trajectories

• Respects velocity, acceleration, and jerk limits

• Switches to impedance control automatically

• Executes trajectory at 50 Hz (20ms updates)

• Duration depends on distance and limits

Trajectory Limits:

• Max velocity: 10 rad/s per joint

• Max acceleration: 5 rad/s² per joint

• Max jerk: 1 rad/s³ per joint

Examples

Move to home position:

>>> await controller.move()

Move to custom position:

>>> target = [0, -0.785, 0, -2.356, 0, 1.571, 0.785]
>>> await controller.move(target)

Move to current position + offset:

>>> current = controller.state['qpos']
>>> await controller.move(current + np.array([0.1, 0, 0, 0, 0, 0, 0]))

Caveats:

• Large motions take longer (trajectory is time-optimal)

• Don’t call while other control is active

• Blocks until motion completes: until every joint is within arrival_tolerance (0.03 rad by default) of the target, or 3 s after the trajectory if friction keeps it farther (it then prints how far)

• Switches to impedance mode (resets controller state)

• May fail if target is at joint limits or in collision

The aiofranka server with the native control loop.

NativeServerController is ServerController on NativeFrankaController: the server process runs the 1 kHz loop in C++ and keeps writing the robot state to shared memory, so clients (FrankaRemoteController) work unchanged. The server runs it by default:

$ aiofranka start-server

as do aiofranka.start() and FrankaRemoteController. aiofranka start-server –python, aiofranka.start(native=False) and FrankaRemoteController(native=False) run the legacy ServerController, with the loop in Python.

class aiofranka.server_native.NativeServerController(robot, shm_block)

Bases: NativeFrankaController

The server’s controller with the native loop, a drop-in for ServerController.

Writes the state to shared memory about every millisecond, from the event loop. On an error it reports to shared memory and returns, without exiting, so the server can retry.

Parameters:

• robot (RobotInterface)

• shm_block (StateBlock)

__init__(robot, shm_block)

Initialize controller with robot interface.

Parameters:

• robot (RobotInterface) – Initialized robot interface

• shm_block (StateBlock)

Note

Controller is initialized in “impedance” mode with conservative gains. Call start() to begin the control loop, then switch() to change modes.

Default Gains:

• Impedance: kp=80, kd=4 (per joint)

• OSC: ee_kp=[100]*6, ee_kd=[4]*6

• Null-space: null_kp=1, null_kd=1

async start()

Start torque control in a thread with a timeout, then the native loop.