isaaclab.controllers

目录

isaaclab.controllers#

Sub-package for different controllers and motion-generators.

Controllers or motion generators are responsible for closed-loop tracking of a given command. The controller can be a simple PID controller or a more complex controller such as impedance control or inverse kinematics control. The controller is responsible for generating the desired joint-level commands to be sent to the robot.

Classes

DifferentialIKController

Differential inverse kinematics (IK) controller.

DifferentialIKControllerCfg

Configuration for differential inverse kinematics controller.

OperationalSpaceController

Operational-space controller.

OperationalSpaceControllerCfg

Configuration for operational-space controller.

pink_ik.PinkIKController

Integration of Pink IK controller with Isaac Lab.

pink_ik.PinkIKControllerCfg

Configuration settings for the Pink IK Controller.

pink_ik.NullSpacePostureTask

Pink-based task that adds a posture objective that is in the null space projection of other tasks.

Differential Inverse Kinematics#

class isaaclab.controllers.DifferentialIKController[源代码]#

基类:object

Differential inverse kinematics (IK) controller.

This controller is based on the concept of differential inverse kinematics [1, 2] which is a method for computing the change in joint positions that yields the desired change in pose.

\[\begin{split}\Delta \mathbf{q} &= \mathbf{J}^{\dagger} \Delta \mathbf{x} \\ \mathbf{q}_{\text{desired}} &= \mathbf{q}_{\text{current}} + \Delta \mathbf{q}\end{split}\]

where \(\mathbf{J}^{\dagger}\) is the pseudo-inverse of the Jacobian matrix \(\mathbf{J}\), \(\Delta \mathbf{x}\) is the desired change in pose, and \(\mathbf{q}_{\text{current}}\) is the current joint positions.

To deal with singularity in Jacobian, the following methods are supported for computing inverse of the Jacobian:

  • “pinv”: Moore-Penrose pseudo-inverse

  • “svd”: Adaptive singular-value decomposition (SVD)

  • “trans”: Transpose of matrix

  • “dls”: Damped version of Moore-Penrose pseudo-inverse (also called Levenberg-Marquardt)

小心

The controller does not assume anything about the frames of the current and desired end-effector pose, or the joint-space velocities. It is up to the user to ensure that these quantities are given in the correct format.

Reference:

  1. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich)

  2. Introduction to Inverse Kinematics by Samuel R. Buss (University of California, San Diego)

Methods:

__init__(cfg, num_envs, device)

Initialize the controller.

reset([env_ids])

Reset the internals.

set_command(command[, ee_pos, ee_quat])

Set target end-effector pose command.

compute(ee_pos, ee_quat, jacobian, joint_pos)

Computes the target joint positions that will yield the desired end effector pose.

Attributes:

action_dim

Dimension of the controller's input command.

__init__(cfg: DifferentialIKControllerCfg, num_envs: int, device: str)[源代码]#

Initialize the controller.

参数:
  • cfg – The configuration for the controller.

  • num_envs – The number of environments.

  • device – The device to use for computations.

property action_dim: int#

Dimension of the controller’s input command.

reset(env_ids: torch.Tensor = None)[源代码]#

Reset the internals.

参数:

env_ids – The environment indices to reset. If None, then all environments are reset.

set_command(command: torch.Tensor, ee_pos: torch.Tensor | None = None, ee_quat: torch.Tensor | None = None)[源代码]#

Set target end-effector pose command.

Based on the configured command type and relative mode, the method computes the desired end-effector pose. It is up to the user to ensure that the command is given in the correct frame. The method only applies the relative mode if the command type is position_rel or pose_rel.

参数:
  • command – The input command in shape (N, 3) or (N, 6) or (N, 7).

  • ee_pos – The current end-effector position in shape (N, 3). This is only needed if the command type is position_rel or pose_rel.

  • ee_quat – The current end-effector orientation (w, x, y, z) in shape (N, 4). This is only needed if the command type is position_* or pose_rel.

抛出:
  • ValueError – If the command type is position_* and ee_quat is None.

  • ValueError – If the command type is position_rel and ee_pos is None.

  • ValueError – If the command type is pose_rel and either ee_pos or ee_quat is None.

compute(ee_pos: torch.Tensor, ee_quat: torch.Tensor, jacobian: torch.Tensor, joint_pos: torch.Tensor) torch.Tensor[源代码]#

Computes the target joint positions that will yield the desired end effector pose.

参数:
  • ee_pos – The current end-effector position in shape (N, 3).

  • ee_quat – The current end-effector orientation in shape (N, 4).

  • jacobian – The geometric jacobian matrix in shape (N, 6, num_joints).

  • joint_pos – The current joint positions in shape (N, num_joints).

返回:

The target joint positions commands in shape (N, num_joints).

class isaaclab.controllers.DifferentialIKControllerCfg[源代码]#

基类:object

Configuration for differential inverse kinematics controller.

Attributes:

command_type

Type of task-space command to control the articulation's body.

use_relative_mode

Whether to use relative mode for the controller.

ik_method

Method for computing inverse of Jacobian.

ik_params

Parameters for the inverse-kinematics method.

command_type: Literal['position', 'pose']#

Type of task-space command to control the articulation’s body.

If “position”, then the controller only controls the position of the articulation’s body. Otherwise, the controller controls the pose of the articulation’s body.

use_relative_mode: bool#

Whether to use relative mode for the controller. Defaults to False.

If True, then the controller treats the input command as a delta change in the position/pose. Otherwise, the controller treats the input command as the absolute position/pose.

ik_method: Literal['pinv', 'svd', 'trans', 'dls']#

Method for computing inverse of Jacobian.

ik_params: dict[str, float] | None#

Parameters for the inverse-kinematics method. Defaults to None, in which case the default parameters for the method are used.

  • Moore-Penrose pseudo-inverse (“pinv”):
    • “k_val”: Scaling of computed delta-joint positions (default: 1.0).

  • Adaptive Singular Value Decomposition (“svd”):
    • “k_val”: Scaling of computed delta-joint positions (default: 1.0).

    • “min_singular_value”: Single values less than this are suppressed to zero (default: 1e-5).

  • Jacobian transpose (“trans”):
    • “k_val”: Scaling of computed delta-joint positions (default: 1.0).

  • Damped Moore-Penrose pseudo-inverse (“dls”):
    • “lambda_val”: Damping coefficient (default: 0.01).

Operational Space controllers#

class isaaclab.controllers.OperationalSpaceController[源代码]#

基类:object

Operational-space controller.

Reference:

  1. A unified approach for motion and force control of robot manipulators: The operational space formulation by Oussama Khatib (Stanford University)

  2. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich)

Methods:

__init__(cfg, num_envs, device)

Initialize operational-space controller.

reset()

Reset the internals.

set_command(command[, current_ee_pose_b, ...])

Set the task-space targets and impedance parameters.

compute(jacobian_b[, current_ee_pose_b, ...])

Performs inference with the controller.

Attributes:

action_dim

Dimension of the action space of controller.

__init__(cfg: OperationalSpaceControllerCfg, num_envs: int, device: str)[源代码]#

Initialize operational-space controller.

参数:
  • cfg – The configuration for operational-space controller.

  • num_envs – The number of environments.

  • device – The device to use for computations.

抛出:

ValueError – When invalid control command is provided.

property action_dim: int#

Dimension of the action space of controller.

reset()[源代码]#

Reset the internals.

set_command(command: torch.Tensor, current_ee_pose_b: torch.Tensor | None = None, current_task_frame_pose_b: torch.Tensor | None = None)[源代码]#

Set the task-space targets and impedance parameters.

参数:
  • command (torch.Tensor) – A concatenated tensor of shape (num_envs, action_dim) containing task-space targets (i.e., pose/wrench) and impedance parameters.

  • current_ee_pose_b (torch.Tensor, optional) – Current end-effector pose, in root frame, of shape (num_envs, 7), containing position and quaternion (w, x, y, z). Required for relative commands. Defaults to None.

  • current_task_frame_pose_b – Current pose of the task frame, in root frame, in which the targets and the (motion/wrench) control axes are defined. It is a tensor of shape (num_envs, 7), containing position and the quaternion (w, x, y, z). Defaults to None.

Format:

Task-space targets, ordered according to ‘command_types’:

Absolute pose: shape (num_envs, 7), containing position and quaternion (w, x, y, z). Relative pose: shape (num_envs, 6), containing delta position and rotation in axis-angle form. Absolute wrench: shape (num_envs, 6), containing force and torque.

Impedance parameters: stiffness for variable_kp, or stiffness, followed by damping ratio for variable:

Stiffness: shape (num_envs, 6) Damping ratio: shape (num_envs, 6)

抛出:
  • ValueError – When the command dimensions are invalid.

  • ValueError – When an invalid impedance mode is provided.

  • ValueError – When the current end-effector pose is not provided for the pose_rel command.

  • ValueError – When an invalid control command is provided.

compute(jacobian_b: torch.Tensor, current_ee_pose_b: torch.Tensor | None = None, current_ee_vel_b: torch.Tensor | None = None, current_ee_force_b: torch.Tensor | None = None, mass_matrix: torch.Tensor | None = None, gravity: torch.Tensor | None = None, current_joint_pos: torch.Tensor | None = None, current_joint_vel: torch.Tensor | None = None, nullspace_joint_pos_target: torch.Tensor | None = None) torch.Tensor[源代码]#

Performs inference with the controller.

参数:
  • jacobian_b – The Jacobian matrix of the end-effector in root frame. It is a tensor of shape (num_envs, 6, num_DoF).

  • current_ee_pose_b – The current end-effector pose in root frame. It is a tensor of shape (num_envs, 7), which contains the position and quaternion (w, x, y, z). Defaults to None.

  • current_ee_vel_b – The current end-effector velocity in root frame. It is a tensor of shape (num_envs, 6), which contains the linear and angular velocities. Defaults to None.

  • current_ee_force_b – The current external force on the end-effector in root frame. It is a tensor of shape (num_envs, 3), which contains the linear force. Defaults to None.

  • mass_matrix – The joint-space mass/inertia matrix. It is a tensor of shape (num_envs, num_DoF, num_DoF). Defaults to None.

  • gravity – The joint-space gravity vector. It is a tensor of shape (num_envs, num_DoF). Defaults to None.

  • current_joint_pos – The current joint positions. It is a tensor of shape (num_envs, num_DoF). Defaults to None.

  • current_joint_vel – The current joint velocities. It is a tensor of shape (num_envs, num_DoF). Defaults to None.

  • nullspace_joint_pos_target – The target joint positions the null space controller is trying to enforce, when possible. It is a tensor of shape (num_envs, num_DoF).

抛出:
  • ValueError – When motion-control is enabled but the current end-effector pose or velocity is not provided.

  • ValueError – When inertial dynamics decoupling is enabled but the mass matrix is not provided.

  • ValueError – When the current end-effector pose is not provided for the pose_rel command.

  • ValueError – When closed-loop force control is enabled but the current end-effector force is not provided.

  • ValueError – When gravity compensation is enabled but the gravity vector is not provided.

  • ValueError – When null-space control is enabled but the system is not redundant.

  • ValueError – When dynamically consistent pseudo-inverse is enabled but the mass matrix inverse is not provided.

  • ValueError – When null-space control is enabled but the current joint positions and velocities are not provided.

  • ValueError – When target joint positions are provided for null-space control but their dimensions do not match the current joint positions.

  • ValueError – When an invalid null-space control method is provided.

返回:

The joint efforts computed by the controller. It is a tensor of shape (num_envs, num_DoF).

返回类型:

Tensor

class isaaclab.controllers.OperationalSpaceControllerCfg[源代码]#

基类:object

Configuration for operational-space controller.

Attributes:

target_types

Type of task-space targets.

motion_control_axes_task

Motion direction to control in task reference frame.

contact_wrench_control_axes_task

Contact wrench direction to control in task reference frame.

inertial_dynamics_decoupling

Whether to perform inertial dynamics decoupling for motion control (inverse dynamics).

partial_inertial_dynamics_decoupling

Whether to ignore the inertial coupling between the translational & rotational motions.

gravity_compensation

Whether to perform gravity compensation.

impedance_mode

"fixed", "variable", "variable_kp".

motion_stiffness_task

The positional gain for determining operational space command forces based on task-space pose error.

motion_damping_ratio_task

The damping ratio is used in-conjunction with positional gain to compute operational space command forces based on task-space velocity error.

motion_stiffness_limits_task

Minimum and maximum values for positional gains.

motion_damping_ratio_limits_task

Minimum and maximum values for damping ratios used to compute velocity gains.

contact_wrench_stiffness_task

The proportional gain for determining operational space command forces for closed-loop contact force control.

nullspace_control

"none", "position".

nullspace_stiffness

The stiffness for null space control.

nullspace_damping_ratio

The damping ratio for null space control.

target_types: Sequence[str]#

Type of task-space targets.

It has two sub-strings joined by underscore:
  • type of task-space target: "pose", "wrench"

  • reference for the task-space targets: "abs" (absolute), "rel" (relative, only for pose)

motion_control_axes_task: Sequence[int]#

Motion direction to control in task reference frame. Mark as 0/1 for each axis.

contact_wrench_control_axes_task: Sequence[int]#

Contact wrench direction to control in task reference frame. Mark as 0/1 for each axis.

inertial_dynamics_decoupling: bool#

Whether to perform inertial dynamics decoupling for motion control (inverse dynamics).

partial_inertial_dynamics_decoupling: bool#

Whether to ignore the inertial coupling between the translational & rotational motions.

gravity_compensation: bool#

Whether to perform gravity compensation.

impedance_mode: str#

"fixed", "variable", "variable_kp".

Type:

Type of gains for motion control

motion_stiffness_task: float | Sequence[float]#

The positional gain for determining operational space command forces based on task-space pose error.

motion_damping_ratio_task: float | Sequence[float]#

The damping ratio is used in-conjunction with positional gain to compute operational space command forces based on task-space velocity error.

The following math operation is performed for computing velocity gains:

\(d_gains = 2 * sqrt(p_gains) * damping_ratio\).

motion_stiffness_limits_task: tuple[float, float]#

Minimum and maximum values for positional gains.

Note: Used only when impedance_mode is "variable" or "variable_kp".

motion_damping_ratio_limits_task: tuple[float, float]#

Minimum and maximum values for damping ratios used to compute velocity gains.

Note: Used only when impedance_mode is "variable".

contact_wrench_stiffness_task: float | Sequence[float] | None#

The proportional gain for determining operational space command forces for closed-loop contact force control.

If None, then open-loop control of desired contact wrench is performed.

Note: since only the linear forces could be measured at the moment, only the first three elements are used for the feedback loop.

nullspace_control: str#

"none", "position".

Note: "position" is used to drive the redundant manipulator to zero configuration by default. If target_joint_pos is provided in the compute() method, it will be driven to this configuration.

Type:

The null space control method for redundant manipulators

nullspace_stiffness: float#

The stiffness for null space control.

nullspace_damping_ratio: float#

The damping ratio for null space control.

Pink IK Controller#

Pink IK controller package for IsaacLab.

This package provides integration between Pink inverse kinematics solver and IsaacLab.

class isaaclab.controllers.pink_ik.PinkIKController[源代码]#

基类:object

Integration of Pink IK controller with Isaac Lab.

The Pink IK controller solves differential inverse kinematics through weighted tasks. Each task is defined by a residual function e(q) that is driven to zero (e.g., e(q) = p_target - p_ee(q) for end-effector positioning). The controller computes joint velocities v satisfying J_e(q)v = -αe(q), where J_e(q) is the task Jacobian. Multiple tasks are resolved through weighted optimization, formulating a quadratic program that minimizes weighted task errors while respecting joint velocity limits.

It supports user defined tasks, and we have provided a NullSpacePostureTask for maintaining desired joint configurations.

Reference:

Pink IK Solver: stephane-caron/pink

Methods:

__init__(cfg, robot_cfg, device)

Initialize the Pink IK Controller.

update_null_space_joint_targets(curr_joint_pos)

Update the null space joint targets.

compute(curr_joint_pos, dt)

Compute the target joint positions based on current state and tasks.

__init__(cfg: PinkIKControllerCfg, robot_cfg: ArticulationCfg, device: str)[源代码]#

Initialize the Pink IK Controller.

参数:
  • cfg – The configuration for the Pink IK controller containing task definitions, solver parameters, and joint configurations.

  • robot_cfg – The robot articulation configuration containing initial joint positions and robot specifications.

  • device – The device to use for computations (e.g., ‘cuda:0’, ‘cpu’).

抛出:

KeyError – When Pink joint names cannot be matched to robot configuration joint positions.

update_null_space_joint_targets(curr_joint_pos: numpy.ndarray)[源代码]#

Update the null space joint targets.

This method updates the target joint positions for null space posture tasks based on the current joint configuration. This is useful for maintaining desired joint configurations when the primary task allows redundancy.

参数:

curr_joint_pos – The current joint positions of shape (num_joints,).

compute(curr_joint_pos: numpy.ndarray, dt: float) torch.Tensor[源代码]#

Compute the target joint positions based on current state and tasks.

Performs inverse kinematics using the Pink solver to compute target joint positions that satisfy the defined tasks. The solver uses quadratic programming to find optimal joint velocities that minimize task errors while respecting constraints.

参数:
  • curr_joint_pos – The current joint positions of shape (num_joints,).

  • dt – The time step for computing joint position changes in seconds.

返回:

The target joint positions as a tensor of shape (num_joints,) on the specified device. If the IK solver fails, returns the current joint positions unchanged to maintain stability.

class isaaclab.controllers.pink_ik.PinkIKControllerCfg[源代码]#

基类:object

Configuration settings for the Pink IK Controller.

The Pink IK controller can be found at: stephane-caron/pink

Attributes:

urdf_path

Path to the robot's URDF file.

mesh_path

Path to the mesh files associated with the robot.

num_hand_joints

The number of hand joints in the robot.

variable_input_tasks

A list of tasks for the Pink IK controller.

fixed_input_tasks

A list of tasks for the Pink IK controller.

joint_names

A list of joint names in the USD asset.

articulation_name

The name of the articulation USD asset in the scene.

base_link_name

The name of the base link in the USD asset.

show_ik_warnings

Show warning if IK solver fails to find a solution.

fail_on_joint_limit_violation

If True, the Pink IK solver will fail and raise an error if any joint limit is violated during optimization.

xr_enabled

If True, the Pink IK controller will send information to the XRVisualization.

urdf_path: str | None#

Path to the robot’s URDF file. This file is used by Pinocchio’s robot_wrapper.BuildFromURDF to load the robot model.

mesh_path: str | None#

Path to the mesh files associated with the robot. These files are also loaded by Pinocchio’s robot_wrapper.BuildFromURDF.

num_hand_joints: int#

The number of hand joints in the robot. The action space for the controller contains the pose_dim(7)*num_controlled_frames + num_hand_joints. The last num_hand_joints values of the action are the hand joint angles.

variable_input_tasks: list[pink.tasks.FrameTask]#

A list of tasks for the Pink IK controller. These tasks are controllable by the env action.

These tasks can be used to control the pose of a frame or the angles of joints. For more details, visit: stephane-caron/pink

fixed_input_tasks: list[pink.tasks.FrameTask]#

A list of tasks for the Pink IK controller. These tasks are fixed and not controllable by the env action.

These tasks can be used to fix the pose of a frame or the angles of joints to a desired configuration. For more details, visit: stephane-caron/pink

joint_names: list[str] | None#

A list of joint names in the USD asset. This is required because the joint naming conventions differ between USD and URDF files. This value is currently designed to be automatically populated by the action term in a manager based environment.

articulation_name: str#

The name of the articulation USD asset in the scene.

The name of the base link in the USD asset.

show_ik_warnings: bool#

Show warning if IK solver fails to find a solution.

fail_on_joint_limit_violation: bool#

If True, the Pink IK solver will fail and raise an error if any joint limit is violated during optimization. PinkIKController will handle the error by setting the last joint positions. If False, the solver will ignore joint limit violations and return the closest solution found.

xr_enabled: bool#

If True, the Pink IK controller will send information to the XRVisualization.

Available Pink IK Tasks#

class isaaclab.controllers.pink_ik.NullSpacePostureTask[源代码]#

Pink-based task that adds a posture objective that is in the null space projection of other tasks.

This task implements posture control in the null space of higher priority tasks (typically end-effector pose tasks) within the Pink inverse kinematics framework.

Mathematical Formulation:

For details on Pink Inverse Kinematics optimization formulation visit: stephane-caron/pink

Null Space Posture Task Implementation:

This task consists of two components:

  1. Error Function: The posture error is computed as:

\[\mathbf{e}(\mathbf{q}) = \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q})\]
where:
  • \(\mathbf{q}^*\) is the target joint configuration

  • \(\mathbf{q}\) is the current joint configuration

  • \(\mathbf{M}\) is a joint selection mask matrix

  1. Jacobian Matrix: The task Jacobian is the null space projector:

\[\mathbf{J}_{\text{posture}}(\mathbf{q}) = \mathbf{N}(\mathbf{q}) = \mathbf{I} - \mathbf{J}_{\text{primary}}^+ \mathbf{J}_{\text{primary}}\]
where:
  • \(\mathbf{J}_{\text{primary}}\) is the combined Jacobian of all higher priority tasks

  • \(\mathbf{J}_{\text{primary}}^+\) is the pseudoinverse of the primary task Jacobian

  • \(\mathbf{N}(\mathbf{q})\) is the null space projector matrix

For example, if there are two frame tasks (e.g., controlling the pose of two end-effectors), the combined Jacobian \(\mathbf{J}_{\text{primary}}\) is constructed by stacking the individual Jacobians for each frame vertically:

\[\begin{split}\mathbf{J}_{\text{primary}} = \begin{bmatrix} \mathbf{J}_1(\mathbf{q}) \\ \mathbf{J}_2(\mathbf{q}) \end{bmatrix}\end{split}\]

where \(\mathbf{J}_1(\mathbf{q})\) and \(\mathbf{J}_2(\mathbf{q})\) are the Jacobians for the first and second frame tasks, respectively.

The null space projector ensures that joint velocities in the null space produce zero velocity for the primary tasks: \(\mathbf{J}_{\text{primary}} \cdot \dot{\mathbf{q}}_{\text{null}} = \mathbf{0}\).

Task Integration:

When integrated into the Pink framework, this task contributes to the optimization as:

\[\left\| \mathbf{N}(\mathbf{q}) \mathbf{v} + \mathbf{M} \cdot (\mathbf{q}^* - \mathbf{q}) \right\|_{W_{\text{posture}}}^2\]

This formulation allows the robot to maintain a desired posture while respecting the constraints imposed by higher priority tasks (e.g., end-effector positioning).

Methods:

__init__(cost[, lm_damping, gain, ...])

Initialize the null space posture task.

__new__(cls, *args, **kwargs)

__init__(cost: float, lm_damping: float = 0.0, gain: float = 1.0, controlled_frames: list[str] | None = None, controlled_joints: list[str] | None = None) None[源代码]#

Initialize the null space posture task.

This task maintains a desired joint posture in the null space of higher-priority frame tasks. Joint selection allows excluding specific joints (e.g., wrist joints in humanoid manipulation) to prevent large rotational ranges from overwhelming errors in critical joints like shoulders and waist.

参数:
  • cost – Task weighting factor in the optimization objective. Units: \([\text{cost}] / [\text{rad}]\).

  • lm_damping – Levenberg-Marquardt regularization scale (unitless). Defaults to 0.0.

  • gain – Task gain \(\alpha \in [0, 1]\) for low-pass filtering. Defaults to 1.0 (no filtering).

  • controlled_frames – Frame names whose Jacobians define the primary tasks for null space projection. If None or empty, no projection is applied.

  • controlled_joints – Joint names to control in the posture task. If None or empty, all actuated joints are controlled.

static __new__(cls, *args: Any, **kwargs: Any) Any#