Implementation:ARISE Initiative Robosuite ControlUtils
| Knowledge Sources | |
|---|---|
| Domains | Robotics, Robot Control |
| Last Updated | 2026-02-15 07:00 GMT |
Overview
The control_utils module provides core functions for operational space control including nullspace torque computation, operational space matrix calculation, orientation error computation, and goal position/orientation setting with limit enforcement.
Description
This module implements the mathematical components required for task-space (operational space) robot controllers, particularly impedance-based controllers.
The nullspace_torques function calculates torques that attempt to maintain a reference joint configuration within the null space of the robot's Jacobian. It uses the mass matrix and nullspace matrix to project proportional-derivative control torques into the subspace orthogonal to the task-space motion, using critically damped gains (kv = 2 * sqrt(kp)). This is essential for redundant robots that have more DOFs than task-space dimensions.
The opspace_matrices function computes the four key matrices used in operational space control: the full task-space inertia matrix (lambda), the position-only inertia matrix, the orientation-only inertia matrix, and the dynamically consistent nullspace matrix. It inverts the mass-weighted Jacobian products using pseudoinverse for numerical stability (zeroing out small singular values), and computes the dynamically consistent generalized inverse (Jbar) to form the nullspace projector.
The orientation_error function computes a 3D orientation error between desired and current rotation matrices using the cross-product method, returning an axis-angle-like error vector suitable for impedance control feedback.
The set_goal_position and set_goal_orientation functions compute goal setpoints from either delta commands (relative to current state) or absolute commands, with optional clipping to position and orientation limits. The orientation goal setter converts axis-angle deltas to rotation matrices, applies them to the current orientation, and clips the result by converting to Euler angles and enforcing per-axis limits (supporting both normal and inverted angle sectors).
Both nullspace_torques and opspace_matrices use the @jit_decorator for optional Numba JIT compilation.
Usage
Use these functions when implementing operational space controllers (OSC) for robot manipulators. The functions are called by the controller classes during the control loop to compute desired torques from Cartesian pose error, nullspace regulation, and task-space dynamics.
Code Reference
Source Location
- Repository: ARISE_Initiative_Robosuite
- File: robosuite/utils/control_utils.py
Signature
@jit_decorator
def nullspace_torques(mass_matrix, nullspace_matrix, initial_joint,
joint_pos, joint_vel, joint_kp=10) -> np.array: ...
@jit_decorator
def opspace_matrices(mass_matrix, J_full, J_pos, J_ori) -> tuple: ...
@jit_decorator
def orientation_error(desired, current) -> np.array: ...
def set_goal_position(delta, current_position, position_limit=None,
set_pos=None) -> np.array: ...
def set_goal_orientation(delta, current_orientation, orientation_limit=None,
set_ori=None) -> np.array: ...
Import
from robosuite.utils.control_utils import (
nullspace_torques,
opspace_matrices,
orientation_error,
set_goal_position,
set_goal_orientation,
)
I/O Contract
Inputs
| Name | Type | Required | Description |
|---|---|---|---|
| mass_matrix | np.array (n,n) | Yes | Robot mass (inertia) matrix |
| nullspace_matrix | np.array (n,n) | Yes | Nullspace projection matrix |
| initial_joint | np.array (n,) | Yes | Reference joint configuration for nullspace regulation |
| joint_pos | np.array (n,) | Yes | Current joint positions |
| joint_vel | np.array (n,) | Yes | Current joint velocities |
| joint_kp | float | No | Proportional gain for nullspace control (default: 10) |
| J_full | np.array (6,n) | Yes | Full 6xn Jacobian matrix |
| J_pos | np.array (3,n) | Yes | Position-only 3xn Jacobian |
| J_ori | np.array (3,n) | Yes | Orientation-only 3xn Jacobian |
| desired (orientation_error) | np.array (3,3) | Yes | Target orientation as rotation matrix |
| current (orientation_error) | np.array (3,3) | Yes | Current orientation as rotation matrix |
| delta (set_goal_position) | np.array (n,) | Yes | Relative position change |
| position_limit | np.array (2,n) or None | No | (min, max) per-dimension position limits |
| delta (set_goal_orientation) | np.array (3,) | Yes | Axis-angle orientation delta |
| orientation_limit | np.array (2,3) or None | No | (min, max) per-axis Euler angle limits |
Outputs
| Name | Type | Description |
|---|---|---|
| nullspace_torques | np.array (n,) | Joint torques in the nullspace that bias toward reference config |
| opspace_matrices | 4-tuple of np.array | (lambda_full (6,6), lambda_pos (3,3), lambda_ori (3,3), nullspace_matrix (n,n)) |
| orientation_error | np.array (3,) | 3D orientation error vector (cross-product-based) |
| set_goal_position | np.array (n,) | Goal position in absolute coordinates, clipped to limits |
| set_goal_orientation | np.array (3,3) | Goal orientation as rotation matrix, clipped to Euler limits |
Usage Examples
import numpy as np
from robosuite.utils.control_utils import (
nullspace_torques,
opspace_matrices,
orientation_error,
set_goal_position,
set_goal_orientation,
)
# Compute operational space matrices from robot dynamics
n_joints = 7
mass_matrix = np.eye(n_joints)
J_full = np.random.randn(6, n_joints)
J_pos = J_full[:3, :]
J_ori = J_full[3:, :]
lambda_full, lambda_pos, lambda_ori, ns_matrix = opspace_matrices(
mass_matrix, J_full, J_pos, J_ori
)
# Compute nullspace torques to maintain a reference configuration
q0 = np.zeros(n_joints) # reference configuration
q = np.random.randn(n_joints) * 0.1 # current joint positions
dq = np.random.randn(n_joints) * 0.01 # current velocities
ns_torques = nullspace_torques(mass_matrix, ns_matrix, q0, q, dq, joint_kp=10)
# Compute orientation error between two rotation matrices
desired_rot = np.eye(3)
current_rot = np.eye(3) + np.random.randn(3, 3) * 0.01
ori_err = orientation_error(desired_rot, current_rot)
# Set a goal position with limits
current_pos = np.array([0.5, 0.0, 0.5])
delta_pos = np.array([0.1, 0.05, -0.02])
limits = np.array([[0.2, -0.3, 0.3], [0.8, 0.3, 0.7]])
goal_pos = set_goal_position(delta_pos, current_pos, position_limit=limits)