Controllers

aiofranka supports three control modes, each suited for different applications.

Impedance Control

Joint-space impedance control implements a spring-damper system in joint space:

\[\tau = K_{\text{p}} (\mathbf{q}_{\text{desired}} - \mathbf{q}) - K_{\text{d}} \dot{\mathbf{q}}\]

where:

  • \(\tau\): Joint torques [Nm] (torque)

  • \(K_{\text{p}}\): Position stiffness gains [Nm/rad] (kp)

  • \(K_{\text{d}}\): Damping gains [Nm⋅s/rad] (kd)

  • \(\mathbf{q}_{\text{desired}}\): Desired joint positions [rad] (q_desired)

  • \(\mathbf{q}\): Current joint positions [rad] (q)

  • \(\dot{\mathbf{q}}\): Joint velocities [rad/s] (qd)

Usage

controller.switch("impedance")
controller.kp = np.ones(7) * 80.0  # Stiffness
controller.kd = np.ones(7) * 4.0   # Damping
controller.set_freq(50)

for i in range(200):
    target = compute_target(i)
    await controller.set("q_desired", target)

Operational Space Control (OSC)

OSC controls the end-effector in Cartesian space while managing null-space behavior:

\[\tau = \mathbf{J}^T \mathbf{M}_{\mathbf{x}} (K_{\text{p}}^{\text{ee}} \mathbf{e} - K_{\text{d}}^{\text{ee}} \dot{\mathbf{x}}) + (\mathbf{I} - \mathbf{J}^T \bar{\mathbf{J}}^T) (K_{\text{p}}^{\text{null}} (\mathbf{q}_0 - \mathbf{q}) - K_{\text{d}}^{\text{null}} \dot{\mathbf{q}})\]

where:

  • \(\mathbf{J}\): End-effector Jacobian (state[jac])

  • \(\mathbf{M}_{\mathbf{x}}x = (\mathbf{J} \mathbf{M}^{-1} \mathbf{J}^T)^{-1}\): Operational space inertia matrix

  • \(\mathbf{M}\): Joint-space mass matrix (state[‘mm’])

  • \(\mathbf{T}_{\text{goal}} = \begin{bmatrix} \mathbf{R}_{\text{goal}} & \mathbf{p}_{\text{goal}} \\ 0 & 1 \end{bmatrix}\): Desired end-effector pose (ee_desired)

  • \(\mathbf{e} = \begin{bmatrix} \mathbf{p}_{\text{goal}} - \mathbf{p} \\ \text{Log}(\mathbf{R}_{\text{goal}} \mathbf{R}^{-1}) \end{bmatrix}\): End-effector pose error (position + rotation as axis-angle)

  • \(\dot{\mathbf{x}} = \mathbf{J} \dot{\mathbf{q}}\): End-effector velocity

  • \(\bar{\mathbf{J}} = \mathbf{M}^{-1} \mathbf{J}^T \mathbf{M}_{\mathbf{x}}\): Dynamically consistent pseudoinverse

  • \((\mathbf{I} - \mathbf{J}^T \bar{\mathbf{J}}^T)\): Null-space projection matrix

  • \(\mathbf{q}_0\): Null-space reference configuration (initial joint positions)

  • \(\mathbf{q}\): Current joint positions (state[‘qpos’])

  • \(\dot{\mathbf{q}}\): Joint velocities (state[‘qvel’])

  • \(K_{\text{p}}^{\text{ee}}\): Task-space stiffness gains [N/m, Nm/rad] (ee_kp)

  • \(K_{\text{d}}^{\text{ee}}\): Task-space damping gains [N⋅s/m, Nm⋅s/rad] (ee_kd)

Usage

controller.switch("osc")

# Task-space gains [x, y, z, roll, pitch, yaw]
controller.ee_kp = np.array([300, 300, 300, 1000, 1000, 1000])
controller.ee_kd = np.ones(6) * 10.0

# Null-space gains (keeps robot away from limits)
controller.null_kp = np.ones(7) * 10.0
controller.null_kd = np.ones(7) * 1.0

controller.set_freq(50)

# Create desired pose (4x4 homogeneous transform)
desired_ee = np.eye(4)
desired_ee[:3, :3] = rotation_matrix  # 3x3 rotation
desired_ee[:3, 3] = [x, y, z]         # position

await controller.set("ee_desired", desired_ee)

End-Effector Pose Format

The end-effector pose is a 4x4 homogeneous transformation matrix:

ee = [[R | p],
      [0 | 1]]

# Where:
# R: 3x3 rotation matrix (SO(3))
# p: 3x1 position vector [x, y, z] in meters

Example with scipy:

from scipy.spatial.transform import Rotation as R

ee = np.eye(4)
# Set rotation (e.g., 180° around X)
ee[:3, :3] = R.from_euler('xyz', [180, 0, 0], degrees=True).as_matrix()
# Set position
ee[:3, 3] = [0.5, 0.0, 0.4]  # meters

Switching Controllers

You can switch between controllers at runtime:

# Start with impedance
controller.switch("impedance")
controller.kp = np.ones(7) * 80.0
await controller.set("q_desired", target1)

# Switch to OSC
controller.switch("osc")
controller.ee_kp = np.array([300, 300, 300, 1000, 1000, 1000])
await controller.set("ee_desired", target2)

# Switch to torque
controller.switch("torque")
controller.torque = np.zeros(7)

Note: Switching resets initial states (initial_qpos, initial_ee) and clears timing state.