isaaclab_contrib.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
Base class for Lee-style geometric controllers. |
|
Base configuration for Lee-style geometric quadrotor controllers. |
|
Lee acceleration controller for multirotor tracking acceleration setpoints. |
|
Configuration for a Lee-style geometric quadrotor acceleration controller. |
|
Lee attitude controller for multirotor tracking attitude setpoints. |
|
Configuration for a Lee-style geometric quadrotor attitude controller. |
|
Lee position controller for multirotor tracking position setpoints. |
|
Configuration for a Lee-style geometric quadrotor position controller. |
|
Lee velocity controller for multirotor tracking velocity setpoints. |
|
Configuration for a Lee-style geometric quadrotor velocity controller. |
Lee Base Controller#
- class isaaclab_contrib.controllers.LeeControllerBase[source]#
Bases:
objectBase class for Lee-style geometric controllers.
Methods:
__init__(cfg, asset, num_envs, device)Initialize controller buffers and pre-compute aggregate inertias.
reset()Reset controller state for all environments.
reset_idx(env_ids)Reset controller state (and optionally randomize gains) for selected environments.
compute(command)Compute wrench command from input command.
- __init__(cfg: LeeControllerBaseCfg, asset: Multirotor, num_envs: int, device: str)[source]#
Initialize controller buffers and pre-compute aggregate inertias.
- Parameters:
cfg – Controller configuration.
asset – Multirotor asset to control.
num_envs – Number of environments.
device – Device to run computations on.
- reset_idx(env_ids: torch.Tensor | None)[source]#
Reset controller state (and optionally randomize gains) for selected environments.
- Parameters:
env_ids – Tensor of environment indices, or
Nonefor all.
- compute(command: torch.Tensor) torch.Tensor[source]#
Compute wrench command from input command.
- Parameters:
command – Input command (shape depends on controller type).
- Returns:
(num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame.
- class isaaclab_contrib.controllers.LeeControllerBaseCfg[source]#
Bases:
objectBase configuration for Lee-style geometric quadrotor controllers.
Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. The controller gains are sampled uniformly per environment between their corresponding
*_minand*_maxbounds at reset.Note
To disable randomization, set the min and max values to be identical. For example: K_rot_range = ((1.85, 1.85, 0.4), (1.85, 1.85, 0.4))
Attributes:
Orientation (rotation) error proportional gain range about body axes [unitless].
Body angular-velocity error proportional gain range [unitless].
Maximum allowed roll/pitch magnitude (inclination) in radians.
Maximum allowed yaw rate command [rad/s].
- K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Orientation (rotation) error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1
- Example (without randomization):
((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains
- K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Body angular-velocity error proportional gain range [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1
- Example (without randomization):
((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains
Lee Acceleration Controller#
- class isaaclab_contrib.controllers.LeeAccController[source]#
Bases:
LeeControllerBaseLee acceleration controller for multirotor tracking acceleration setpoints.
Computes a body-frame wrench command
[Fx, Fy, Fz, Tx, Ty, Tz]from an acceleration setpoint in the world frame. Gains may be randomized per environment if enabled in the configuration.Methods:
__init__(cfg, asset, num_envs, device)Initialize controller.
compute(command)Compute wrench command from acceleration setpoint.
reset()Reset controller state for all environments.
reset_idx(env_ids)Reset controller state (and optionally randomize gains) for selected environments.
- __init__(cfg: LeeAccControllerCfg, asset: Multirotor, num_envs: int, device: str)[source]#
Initialize controller.
- Parameters:
cfg – Controller configuration.
asset – Multirotor asset to control.
num_envs – Number of environments.
device – Device to run computations on.
- compute(command: torch.Tensor) torch.Tensor[source]#
Compute wrench command from acceleration setpoint.
- Parameters:
command – (num_envs, 4) acceleration command command [ax, ay, az, yaw_rate] in body frame.
- Returns:
(num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame.
- reset()#
Reset controller state for all environments.
- reset_idx(env_ids: torch.Tensor | None)#
Reset controller state (and optionally randomize gains) for selected environments.
- Parameters:
env_ids – Tensor of environment indices, or
Nonefor all.
- class isaaclab_contrib.controllers.LeeAccControllerCfg[source]#
Bases:
LeeControllerBaseCfgConfiguration for a Lee-style geometric quadrotor acceleration controller.
Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. The acceleration controller gains are sampled uniformly per environment between their corresponding
*_minand*_maxbounds at reset.Attributes:
Orientation (rotation) error proportional gain range about body axes [unitless].
Body angular-velocity error proportional gain range [unitless].
Maximum allowed roll/pitch magnitude (inclination) in radians.
Maximum allowed yaw rate command [rad/s].
- K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Orientation (rotation) error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1
- Example (without randomization):
((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains
- K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Body angular-velocity error proportional gain range [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1
- Example (without randomization):
((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains
Lee Attitude Controller#
- class isaaclab_contrib.controllers.LeeAttController[source]#
Bases:
LeeControllerBaseLee attitude controller for multirotor tracking attitude setpoints.
Computes a body-frame wrench command
[Fx, Fy, Fz, Tx, Ty, Tz]from an attitude setpoint in the world frame. Gains may be randomized per environment if enabled in the configuration.Methods:
__init__(cfg, asset, num_envs, device)Initialize controller.
compute(command)Compute wrench command from attitude setpoint.
reset()Reset controller state for all environments.
reset_idx(env_ids)Reset controller state (and optionally randomize gains) for selected environments.
- __init__(cfg: LeeAttControllerCfg, asset: Multirotor, num_envs: int, device: str)[source]#
Initialize controller.
- Parameters:
cfg – Controller configuration.
asset – Multirotor asset to control.
num_envs – Number of environments.
device – Device to run computations on.
- compute(command: torch.Tensor) torch.Tensor[source]#
Compute wrench command from attitude setpoint.
- Parameters:
command – (num_envs, 4) attitude command command [thrust, roll, pitch, yaw_rate] in body frame.
- Returns:
(num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame.
- reset()#
Reset controller state for all environments.
- reset_idx(env_ids: torch.Tensor | None)#
Reset controller state (and optionally randomize gains) for selected environments.
- Parameters:
env_ids – Tensor of environment indices, or
Nonefor all.
- class isaaclab_contrib.controllers.LeeAttControllerCfg[source]#
Bases:
LeeControllerBaseCfgConfiguration for a Lee-style geometric quadrotor attitude controller.
Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. The attitude controller gains are sampled uniformly per environment between their corresponding
*_minand*_maxbounds at reset.Attributes:
Orientation (rotation) error proportional gain range about body axes [unitless].
Body angular-velocity error proportional gain range [unitless].
Maximum allowed roll/pitch magnitude (inclination) in radians.
Maximum allowed yaw rate command [rad/s].
- K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Orientation (rotation) error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1
- Example (without randomization):
((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains
- K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Body angular-velocity error proportional gain range [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1
- Example (without randomization):
((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains
Lee Position Controller#
- class isaaclab_contrib.controllers.LeePosController[source]#
Bases:
LeeControllerBaseLee position controller for multirotor tracking position setpoints.
Computes a body-frame wrench command
[Fx, Fy, Fz, Tx, Ty, Tz]from a position setpoint in the world frame. Gains may be randomized per environment if enabled in the configuration.Methods:
__init__(cfg, asset, num_envs, device)Initialize controller.
compute(command)Compute wrench command from position setpoint.
reset()Reset controller state for all environments.
reset_idx(env_ids)Reset controller state (and optionally randomize gains) for selected environments.
- __init__(cfg: LeePosControllerCfg, asset: Multirotor, num_envs: int, device: str)[source]#
Initialize controller.
- Parameters:
cfg – Controller configuration.
asset – Multirotor asset to control.
num_envs – Number of environments.
device – Device to run computations on.
- compute(command: torch.Tensor) torch.Tensor[source]#
Compute wrench command from position setpoint.
- Parameters:
command – (num_envs, 4) [x, y, z, yaw] in body frame.
- Returns:
(num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame.
- reset()#
Reset controller state for all environments.
- reset_idx(env_ids: torch.Tensor | None)#
Reset controller state (and optionally randomize gains) for selected environments.
- Parameters:
env_ids – Tensor of environment indices, or
Nonefor all.
- class isaaclab_contrib.controllers.LeePosControllerCfg[source]#
Bases:
LeeControllerBaseCfgConfiguration for a Lee-style geometric quadrotor position controller.
Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. The position controller gains are sampled uniformly per environment between their corresponding
*_minand*_maxbounds at reset.Attributes:
Orientation (rotation) error proportional gain range about body axes [unitless].
Body angular-velocity error proportional gain range [unitless].
Maximum allowed roll/pitch magnitude (inclination) in radians.
Maximum allowed yaw rate command [rad/s].
Position error proportional gain range about body axes [unitless].
Velocity error proportional gain range about body axes [unitless].
- K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Orientation (rotation) error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1
- Example (without randomization):
((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains
- K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Body angular-velocity error proportional gain range [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1
- Example (without randomization):
((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains
- max_inclination_angle_rad: float#
Maximum allowed roll/pitch magnitude (inclination) in radians.
This limits the maximum tilt angle of the quadrotor during control. Typical range: 0.5 to 1.57 radians (30° to 90°)
Example
1.0471975511965976 (60° in radians) for ARL Robot 1
- max_yaw_rate: float#
Maximum allowed yaw rate command [rad/s].
This limits the maximum rotational velocity about the z-axis. Typical range: 0.5 to 2.0 rad/s
Example
1.0471975511965976 (60°/s in radians) for ARL Robot 1
- K_pos_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Position error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). Format: ((min_x, min_y, min_z), (max_x, max_y, max_z))
Example
((3.0, 3.0, 2.0), (4.0, 4.0, 2.5)) for ARL Robot 1
- K_vel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Velocity error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). Format: ((min_x, min_y, min_z), (max_x, max_y, max_z))
Example
((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)) for ARL Robot 1
Lee Velocity Controller#
- class isaaclab_contrib.controllers.LeeVelController[source]#
Bases:
LeeControllerBaseLee velocity controller for multirotor tracking velocity setpoints.
Computes a body-frame wrench command
[Fx, Fy, Fz, Tx, Ty, Tz]from a velocity setpoint: [vx, vy, vz, yaw_rate]. Gains may be randomized per environment if enabled in the configuration.Methods:
__init__(cfg, asset, num_envs, device)Initialize controller.
compute(command)Compute wrench command from velocity setpoint.
reset()Reset controller state for all environments.
reset_idx(env_ids)Reset controller state (and optionally randomize gains) for selected environments.
- __init__(cfg: LeeVelControllerCfg, asset: Multirotor, num_envs: int, device: str)[source]#
Initialize controller.
- Parameters:
cfg – Controller configuration.
asset – Multirotor asset to control.
num_envs – Number of environments.
device – Device to run computations on.
- compute(command: torch.Tensor) torch.Tensor[source]#
Compute wrench command from velocity setpoint.
- Parameters:
command – (num_envs, 4) velocity command [vx, vy, vz, yaw_rate] in body frame.
- Returns:
(num_envs, 6) wrench command [fx, fy, fz, tx, ty, tz] in body frame.
- reset()#
Reset controller state for all environments.
- reset_idx(env_ids: torch.Tensor | None)#
Reset controller state (and optionally randomize gains) for selected environments.
- Parameters:
env_ids – Tensor of environment indices, or
Nonefor all.
- class isaaclab_contrib.controllers.LeeVelControllerCfg[source]#
Bases:
LeeControllerBaseCfgConfiguration for a Lee-style geometric quadrotor velocity controller.
Unless otherwise noted, vectors are ordered as (x, y, z) in the simulation world/body frames. The velocity controller gains are sampled uniformly per environment between their corresponding
*_minand*_maxbounds at reset.Attributes:
Orientation (rotation) error proportional gain range about body axes [unitless].
Body angular-velocity error proportional gain range [unitless].
Maximum allowed roll/pitch magnitude (inclination) in radians.
Maximum allowed yaw rate command [rad/s].
Velocity error proportional gain range about body axes [unitless].
- K_rot_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Orientation (rotation) error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw. Format: ((min_roll, min_pitch, min_yaw), (max_roll, max_pitch, max_yaw))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((1.6, 1.6, 0.25), (1.85, 1.85, 0.4)) for ARL Robot 1
- Example (without randomization):
((1.85, 1.85, 0.4), (1.85, 1.85, 0.4)) for fixed gains
- K_angvel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Body angular-velocity error proportional gain range [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for roll, pitch, and yaw rates. Format: ((min_roll_rate, min_pitch_rate, min_yaw_rate), (max_roll_rate, max_pitch_rate, max_yaw_rate))
To disable randomization, set both tuples to the same values.
- Example (with randomization):
((0.4, 0.4, 0.075), (0.5, 0.5, 0.09)) for ARL Robot 1
- Example (without randomization):
((0.5, 0.5, 0.09), (0.5, 0.5, 0.09)) for fixed gains
- max_inclination_angle_rad: float#
Maximum allowed roll/pitch magnitude (inclination) in radians.
This limits the maximum tilt angle of the quadrotor during control. Typical range: 0.5 to 1.57 radians (30° to 90°)
Example
1.0471975511965976 (60° in radians) for ARL Robot 1
- max_yaw_rate: float#
Maximum allowed yaw rate command [rad/s].
This limits the maximum rotational velocity about the z-axis. Typical range: 0.5 to 2.0 rad/s
Example
1.0471975511965976 (60°/s in radians) for ARL Robot 1
- K_vel_range: tuple[tuple[float, float, float], tuple[float, float, float]]#
Velocity error proportional gain range about body axes [unitless].
This is a tuple of two tuples containing the minimum and maximum gains for each axis (x, y, z). Format: ((min_x, min_y, min_z), (max_x, max_y, max_z))
Example
((2.5, 2.5, 1.5), (3.5, 3.5, 2.0)) for ARL Robot 1