Unverified Commit efabf203 authored by Mayank Mittal's avatar Mayank Mittal Committed by GitHub

Adds parsing of joint parameters from USD for actuators (#258)

# Description

Currently, the joint parameters must always be specified from the
user-defined configurations. Setting them to None makes them go to zero
(for stiffness and damping) and infinity (for limits). This is not great
since many assets may have some values in the USD file that they have
tuned using inspection tools in Omniverse.

This MR changes the behavior of `None` in the actuator configuration to
load default values provided as inputs to the actuator class. It also
loads other joint drive parameters such as friction and armature.

## Type of change

- Bug fix (non-breaking change which fixes an issue)
- Breaking change (fix or feature that would cause existing
functionality to not work as expected)

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./orbit.sh --format`
- [x] I have made corresponding changes to the documentation
- [x] My changes generate no new warnings
- [x] I have added tests that prove my fix is effective or that my
feature works
- [x] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [x] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there
parent 48146c2b
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.9.52"
version = "0.9.53"
# Description
title = "ORBIT framework for Robot Learning"
......
Changelog
---------
0.9.53 (2023-11-29)
~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Changed the behavior of passing :obj:`None` to the :class:`omni.isaac.orbit.actuators.ActuatorBaseCfg`
class. Earlier, they were resolved to fixed default values. Now, they imply that the values are loaded
from the USD joint drive configuration.
Added
^^^^^
* Added setting of joint armature and friction quantities to the articulation class.
0.9.52 (2023-11-29)
~~~~~~~~~~~~~~~~~~~
......
......@@ -18,37 +18,61 @@ if TYPE_CHECKING:
class ActuatorBase(ABC):
"""Base class for applying actuator models over a collection of actuated joints in an articulation.
"""Base class for actuator models over a collection of actuated joints in an articulation.
The default actuator for applying the same actuator model over a collection of actuated joints in
an articulation.
Actuator models augment the simulated articulation joints with an external drive dynamics model.
The model is used to convert the user-provided joint commands (positions, velocities and efforts)
into the desired joint positions, velocities and efforts that are applied to the simulated articulation.
The joint names are specified in the configuration through a list of regular expressions. The regular
expressions are matched against the joint names in the articulation. The first match is used to determine
the joint indices in the articulation.
The base class provides the interface for the actuator models. It is responsible for parsing the
actuator parameters from the configuration and storing them as buffers. It also provides the
interface for resetting the actuator state and computing the desired joint commands for the simulation.
In the default actuator, no constraints or formatting is performed over the input actions. Thus, the
input actions are directly used to compute the joint actions in the :meth:`compute`.
For each actuator model, a corresponding configuration class is provided. The configuration class
is used to parse the actuator parameters from the configuration. It also specifies the joint names
for which the actuator model is applied. These names can be specified as regular expressions, which
are matched against the joint names in the articulation.
To see how the class is used, check the :class:`omni.isaac.orbit.assets.Articulation` class.
"""
computed_effort: torch.Tensor
"""The computed effort for the actuator group. Shape is ``(num_envs, num_joints)``."""
applied_effort: torch.Tensor
"""The applied effort for the actuator group. Shape is ``(num_envs, num_joints)``."""
effort_limit: float = torch.inf
effort_limit: torch.Tensor
"""The effort limit for the actuator group. Shape is ``(num_envs, num_joints)``."""
velocity_limit: float = torch.inf
velocity_limit: torch.Tensor
"""The velocity limit for the actuator group. Shape is ``(num_envs, num_joints)``."""
stiffness: torch.Tensor
"""The stiffness (P gain) of the PD controller. Shape is ``(num_envs, num_joints)``."""
damping: torch.Tensor
"""The damping (D gain) of the PD controller. Shape is ``(num_envs, num_joints)``."""
armature: torch.Tensor
"""The armature of the actuator joints. Shape is ``(num_envs, num_joints)``."""
friction: torch.Tensor
"""The joint friction of the actuator joints. Shape is ``(num_envs, num_joints)``."""
def __init__(
self, cfg: ActuatorBaseCfg, joint_names: list[str], joint_ids: Sequence[int], num_envs: int, device: str
self,
cfg: ActuatorBaseCfg,
joint_names: list[str],
joint_ids: Sequence[int],
num_envs: int,
device: str,
stiffness: torch.Tensor | float = 0.0,
damping: torch.Tensor | float = 0.0,
armature: torch.Tensor | float = 0.0,
friction: torch.Tensor | float = 0.0,
effort_limit: torch.Tensor | float = torch.inf,
velocity_limit: torch.Tensor | float = torch.inf,
):
"""Initialize the actuator.
Note:
The actuator parameters are parsed from the configuration and stored as buffers. If the parameters
are not specified in the configuration, then the default values provided in the arguments are used.
Args:
cfg: The configuration of the actuator model.
joint_names: The joint names in the articulation.
......@@ -56,6 +80,18 @@ class ActuatorBase(ABC):
the joints in the articulation are part of the group.
num_envs: Number of articulations in the view.
device: Device used for processing.
stiffness: The default joint stiffness (P gain). Defaults to 0.0.
If a tensor, then the shape is ``(num_envs, num_joints)``.
damping: The default joint damping (D gain). Defaults to 0.0.
If a tensor, then the shape is ``(num_envs, num_joints)``.
armature: The default joint armature. Defaults to 0.0.
If a tensor, then the shape is ``(num_envs, num_joints)``.
friction: The default joint friction. Defaults to 0.0.
If a tensor, then the shape is ``(num_envs, num_joints)``.
effort_limit: The default effort limit. Defaults to infinity.
If a tensor, then the shape is ``(num_envs, num_joints)``.
velocity_limit: The default velocity limit. Defaults to infinity.
If a tensor, then the shape is ``(num_envs, num_joints)``.
"""
# save parameters
self.cfg = cfg
......@@ -64,36 +100,20 @@ class ActuatorBase(ABC):
self._joint_names = joint_names
self._joint_indices = joint_ids
# parse joint stiffness and damping
self.stiffness = self._parse_joint_parameter(self.cfg.stiffness, stiffness)
self.damping = self._parse_joint_parameter(self.cfg.damping, damping)
# parse joint armature and friction
self.armature = self._parse_joint_parameter(self.cfg.armature, armature)
self.friction = self._parse_joint_parameter(self.cfg.friction, friction)
# parse joint limits
# note: for velocity limits, we don't have USD parameter, so default is infinity
self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, effort_limit)
self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, velocity_limit)
# create commands buffers for allocation
self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device)
self.applied_effort = torch.zeros_like(self.computed_effort)
self.stiffness = torch.zeros_like(self.computed_effort)
self.damping = torch.zeros_like(self.computed_effort)
# parse joint limits
if self.cfg.effort_limit is not None:
self.effort_limit = self.cfg.effort_limit
if self.cfg.velocity_limit is not None:
self.velocity_limit = self.cfg.velocity_limit
# parse joint stiffness and damping
# -- stiffness
if self.cfg.stiffness is not None:
if isinstance(self.cfg.stiffness, (float, int)):
# if float, then use the same value for all joints
self.stiffness[:] = float(self.cfg.stiffness)
else:
# if dict, then parse the regular expression
indices, _, values = string_utils.resolve_matching_names_values(self.cfg.stiffness, self.joint_names)
self.stiffness[:, indices] = torch.tensor(values, device=self._device)
# -- damping
if self.cfg.damping is not None:
if isinstance(self.cfg.damping, (float, int)):
# if float, then use the same value for all joints
self.damping[:] = float(self.cfg.damping)
else:
# if dict, then parse the regular expression
indices, _, values = string_utils.resolve_matching_names_values(self.cfg.damping, self.joint_names)
self.damping[:, indices] = torch.tensor(values, device=self._device)
def __str__(self) -> str:
"""Returns: A string representation of the actuator group."""
......@@ -164,3 +184,52 @@ class ActuatorBase(ABC):
The computed desired joint positions, joint velocities and joint efforts.
"""
raise NotImplementedError
"""
Helper functions.
"""
def _parse_joint_parameter(
self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None
) -> torch.Tensor:
"""Parse the joint parameter from the configuration.
Args:
cfg_value: The parameter value from the configuration. If None, then use the default value.
default_value: The default value to use if the parameter is None. If it is also None,
then an error is raised.
Returns:
The parsed parameter value.
Raises:
TypeError: If the parameter value is not of the expected type.
TypeError: If the default value is not of the expected type.
ValueError: If the parameter value is None and no default value is provided.
"""
# create parameter buffer
param = torch.zeros(self._num_envs, self.num_joints, device=self._device)
# parse the parameter
if cfg_value is not None:
if isinstance(cfg_value, (float, int)):
# if float, then use the same value for all joints
param[:] = float(cfg_value)
elif isinstance(cfg_value, dict):
# if dict, then parse the regular expression
indices, _, values = string_utils.resolve_matching_names_values(param, self.joint_names)
param[:, indices] = torch.tensor(values, device=self._device)
else:
raise TypeError(f"Invalid type for parameter value: {type(cfg_value)}. Expected float or dict.")
elif default_value is not None:
if isinstance(default_value, (float, int)):
# if float, then use the same value for all joints
param[:] = float(default_value)
elif isinstance(default_value, torch.Tensor):
# if tensor, then use the same tensor for all joints
param[:] = default_value
else:
raise TypeError(f"Invalid type for default value: {type(default_value)}. Expected float or Tensor.")
else:
raise ValueError("The parameter value is None and no default value is provided.")
return param
......@@ -21,7 +21,7 @@ class ActuatorBaseCfg:
class_type: type[ActuatorBase] = MISSING
"""The associated actuator class.
The class should inherit from :class:`omni.isaac.orbit.actuators.actuator_base.ActuatorBase`.
The class should inherit from :class:`omni.isaac.orbit.actuators.ActuatorBase`.
"""
joint_names_expr: list[str] = MISSING
......@@ -34,7 +34,7 @@ class ActuatorBaseCfg:
effort_limit: float | None = None
"""Force/Torque limit of the joints in the group. Defaults to :obj:`None`.
If :obj:`None`, the limit is set to infinity.
If :obj:`None`, the limit is set to the value specified in the USD joint prim.
"""
velocity_limit: float | None = None
......@@ -46,13 +46,25 @@ class ActuatorBaseCfg:
stiffness: dict[str, float] | float | None = MISSING
"""Stiffness gains (also known as p-gain) of the joints in the group.
If :obj:`None`, the stiffness is set to 0.
If :obj:`None`, the stiffness is set to the value from the USD joint prim.
"""
damping: dict[str, float] | float | None = MISSING
"""Damping gains (also known as d-gain) of the joints in the group.
If :obj:`None`, the damping is set to 0.
If :obj:`None`, the damping is set to the value from the USD joint prim.
"""
armature: dict[str, float] | float | None = None
"""Armature of the joints in the group.
If :obj:`None`, the armature is set to the value from the USD joint prim.
"""
friction: dict[str, float] | float | None = None
"""Joint friction of the joints in the group.
If :obj:`None`, the joint friction is set to the value from the USD joint prim.
"""
......
......@@ -42,10 +42,8 @@ class ActuatorNetLSTM(DCMotor):
cfg: ActuatorNetLSTMCfg
"""The configuration of the actuator model."""
def __init__(
self, cfg: ActuatorNetLSTMCfg, joint_names: list[str], joint_ids: list[int], num_envs: int, device: str
):
super().__init__(cfg, joint_names, joint_ids, num_envs, device)
def __init__(self, cfg: ActuatorNetLSTMCfg, *args, **kwargs):
super().__init__(cfg, *args, **kwargs)
# load the model from JIT file
file_bytes = read_file(self.cfg.network_file)
......@@ -65,6 +63,10 @@ class ActuatorNetLSTM(DCMotor):
self.sea_hidden_state_per_env = self.sea_hidden_state.view(layer_shape_per_env)
self.sea_cell_state_per_env = self.sea_cell_state.view(layer_shape_per_env)
"""
Operations.
"""
def reset(self, env_ids: Sequence[int]):
# reset the hidden and cell states for the specified environments
with torch.no_grad():
......@@ -119,10 +121,8 @@ class ActuatorNetMLP(DCMotor):
cfg: ActuatorNetMLPCfg
"""The configuration of the actuator model."""
def __init__(
self, cfg: ActuatorNetMLPCfg, joint_names: list[str], joint_ids: list[int], num_envs: int, device: str
):
super().__init__(cfg, joint_names, joint_ids, num_envs, device)
def __init__(self, cfg: ActuatorNetMLPCfg, *args, **kwargs):
super().__init__(cfg, *args, **kwargs)
# load the model from JIT file
file_bytes = read_file(self.cfg.network_file)
......@@ -135,6 +135,10 @@ class ActuatorNetMLP(DCMotor):
)
self._joint_vel_history = torch.zeros(self._num_envs, history_length, self.num_joints, device=self._device)
"""
Operations.
"""
def reset(self, env_ids: Sequence[int]):
# reset the history for the specified environments
self._joint_pos_error_history[env_ids] = 0.0
......
......@@ -176,8 +176,8 @@ class DCMotor(IdealPDActuator):
cfg: DCMotorCfg
"""The configuration for the actuator model."""
def __init__(self, cfg: DCMotorCfg, joint_names: list[str], joint_ids: list[int], num_envs: int, device: str):
super().__init__(cfg, joint_names, joint_ids, num_envs, device)
def __init__(self, cfg: DCMotorCfg, *args, **kwargs):
super().__init__(cfg, *args, **kwargs)
# parse configuration
if self.cfg.saturation_effort is not None:
self._saturation_effort = self.cfg.saturation_effort
......@@ -185,6 +185,8 @@ class DCMotor(IdealPDActuator):
self._saturation_effort = torch.inf
# prepare joint vel buffer for max effort computation
self._joint_vel = torch.zeros_like(self.computed_effort)
# create buffer for zeros effort
self._zeros_effort = torch.zeros_like(self.computed_effort)
# check that quantities are provided
if self.cfg.velocity_limit is None:
raise ValueError("The velocity limit must be provided for the DC motor actuator model.")
......@@ -209,10 +211,10 @@ class DCMotor(IdealPDActuator):
# compute torque limits
# -- max limit
max_effort = self.cfg.saturation_effort * (1.0 - self._joint_vel / self.velocity_limit)
max_effort = torch.clip(max_effort, min=0.0, max=self.effort_limit)
max_effort = torch.clip(max_effort, min=self._zeros_effort, max=self.effort_limit)
# -- min limit
min_effort = self.cfg.saturation_effort * (-1.0 - self._joint_vel / self.velocity_limit)
min_effort = torch.clip(min_effort, min=-self.effort_limit, max=0.0)
min_effort = torch.clip(min_effort, min=-self.effort_limit, max=self._zeros_effort)
# clip the torques based on the motor limits
return torch.clip(effort, min=min_effort, max=max_effort)
......@@ -9,6 +9,7 @@
from __future__ import annotations
import torch
from prettytable import PrettyTable
from typing import TYPE_CHECKING, Sequence
import carb
......@@ -219,7 +220,7 @@ class Articulation(RigidObject):
def write_joint_stiffness_to_sim(
self,
stiffness: torch.Tensor,
stiffness: torch.Tensor | float,
joint_ids: Sequence[int] | None = None,
env_ids: Sequence[int] | None = None,
):
......@@ -244,7 +245,10 @@ class Articulation(RigidObject):
self.root_physx_view.set_dof_stiffnesses(self._data.joint_stiffness.cpu(), indices=physx_env_ids.cpu())
def write_joint_damping_to_sim(
self, damping: torch.Tensor, joint_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None
self,
damping: torch.Tensor | float,
joint_ids: Sequence[int] | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write joint damping into the simulation.
......@@ -268,13 +272,13 @@ class Articulation(RigidObject):
# set into simulation
self.root_physx_view.set_dof_dampings(self._data.joint_damping.cpu(), indices=physx_env_ids.cpu())
def write_joint_torque_limit_to_sim(
def write_joint_effort_limit_to_sim(
self,
limits: torch.Tensor,
limits: torch.Tensor | float,
joint_ids: Sequence[int] | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write joint torque limits into the simulation.
"""Write joint effort limits into the simulation.
Args:
limits: Joint torque limits. Shape is ``(len(env_ids), len(joint_ids))``.
......@@ -289,12 +293,65 @@ class Articulation(RigidObject):
physx_env_ids = self._ALL_INDICES
if joint_ids is None:
joint_ids = slice(None)
# move tensor to cpu if needed
if isinstance(limits, torch.Tensor):
limits = limits.cpu()
# set into internal buffers
torque_limit_all = self.root_physx_view.get_dof_max_forces()
torque_limit_all[env_ids, joint_ids] = limits
# set into simulation
self.root_physx_view.set_dof_max_forces(torque_limit_all.cpu(), indices=physx_env_ids.cpu())
def write_joint_armature_to_sim(
self,
armature: torch.Tensor | float,
joint_ids: Sequence[int] | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write joint armature into the simulation.
Args:
armature: Joint armature. Shape is ``(len(env_ids), len(joint_ids))``.
joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints).
env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments).
"""
# resolve indices
physx_env_ids = env_ids
if env_ids is None:
env_ids = slice(None)
physx_env_ids = self._ALL_INDICES
if joint_ids is None:
joint_ids = slice(None)
# set into internal buffers
self._data.joint_armature[env_ids, joint_ids] = armature
# set into simulation
self.root_physx_view.set_dof_armatures(self._data.joint_armature.cpu(), indices=physx_env_ids.cpu())
def write_joint_friction_to_sim(
self,
joint_friction: torch.Tensor | float,
joint_ids: Sequence[int] | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write joint friction into the simulation.
Args:
joint_friction: Joint friction. Shape is ``(len(env_ids), len(joint_ids))``.
joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints).
env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments).
"""
# resolve indices
physx_env_ids = env_ids
if env_ids is None:
env_ids = slice(None)
physx_env_ids = self._ALL_INDICES
if joint_ids is None:
joint_ids = slice(None)
# set into internal buffers
self._data.joint_friction[env_ids, joint_ids] = joint_friction
# set into simulation
self.root_physx_view.set_dof_friction_coefficients(self._data.joint_friction.cpu(), indices=physx_env_ids.cpu())
"""
Operations - State.
"""
......@@ -435,9 +492,10 @@ class Articulation(RigidObject):
# process configuration
self._process_cfg()
self._process_actuators_cfg()
# log joint information
self._log_articulation_joint_info()
def _create_buffers(self):
"""Create buffers for storing data."""
# allocate buffers
super()._create_buffers()
# history buffers
......@@ -458,6 +516,8 @@ class Articulation(RigidObject):
self._data.joint_effort_target = torch.zeros_like(self._data.joint_pos)
self._data.joint_stiffness = torch.zeros_like(self._data.joint_pos)
self._data.joint_damping = torch.zeros_like(self._data.joint_pos)
self._data.joint_armature = torch.zeros_like(self._data.joint_pos)
self._data.joint_friction = torch.zeros_like(self._data.joint_pos)
# -- joint commands (explicit)
self._data.computed_torque = torch.zeros_like(self._data.joint_pos)
self._data.applied_torque = torch.zeros_like(self._data.joint_pos)
......@@ -517,16 +577,20 @@ class Articulation(RigidObject):
f"No joints found for actuator group: {actuator_name} with joint name expression:"
f" {actuator_cfg.joint_names_expr}."
)
# for efficiency avoid indexing when over all indices
if len(joint_names) == self.num_joints:
joint_ids = slice(None)
# create actuator collection
# note: for efficiency avoid indexing when over all indices
sim_stiffness, sim_damping = self.root_view.get_gains(joint_indices=joint_ids)
actuator: ActuatorBase = actuator_cfg.class_type(
cfg=actuator_cfg,
joint_names=joint_names,
joint_ids=joint_ids,
joint_ids=slice(None) if len(joint_names) == self.num_joints else joint_ids,
num_envs=self.root_view.count,
device=self.device,
stiffness=sim_stiffness,
damping=sim_damping,
armature=self.root_view.get_armatures(joint_indices=joint_ids),
friction=self.root_view.get_friction_coefficients(joint_indices=joint_ids),
effort_limit=self.root_view.get_max_efforts(joint_indices=joint_ids),
)
# log information on actuator groups
carb.log_info(
......@@ -541,13 +605,17 @@ class Articulation(RigidObject):
# the gains and limits are set into the simulation since actuator model is implicit
self.write_joint_stiffness_to_sim(actuator.stiffness, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(actuator.damping, joint_ids=actuator.joint_indices)
self.write_joint_torque_limit_to_sim(actuator.effort_limit, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(actuator.effort_limit, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
else:
# the gains and limits are processed by the actuator model
# we set gains to zero, and torque limit to a high value in simulation to avoid any interference
self.write_joint_stiffness_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_damping_to_sim(0.0, joint_ids=actuator.joint_indices)
self.write_joint_torque_limit_to_sim(1.0e9, joint_ids=actuator.joint_indices)
self.write_joint_effort_limit_to_sim(1.0e9, joint_ids=actuator.joint_indices)
self.write_joint_armature_to_sim(actuator.armature, joint_ids=actuator.joint_indices)
self.write_joint_friction_to_sim(actuator.friction, joint_ids=actuator.joint_indices)
# perform some sanity checks to ensure actuators are prepared correctly
total_act_joints = sum(actuator.num_joints for actuator in self.actuators.values())
......@@ -601,3 +669,39 @@ class Articulation(RigidObject):
# TODO: find a cleaner way to handle gear ratio. Only needed for variable gear ratio actuators.
if hasattr(actuator, "gear_ratio"):
self._data.gear_ratio[:, actuator.joint_indices] = actuator.gear_ratio
"""
Internal helpers -- Debugging.
"""
def _log_articulation_joint_info(self):
"""Log information about the articulation's simulated joints."""
# read out all joint parameters from simulation
gains = self.root_view.get_gains(indices=[0])
stiffnesses, dampings = gains[0].squeeze(0).tolist(), gains[1].squeeze(0).tolist()
armatures = self.root_view.get_armatures(indices=[0]).squeeze(0).tolist()
frictions = self.root_view.get_friction_coefficients(indices=[0]).squeeze(0).tolist()
effort_limits = self.root_view.get_max_efforts(indices=[0]).squeeze(0).tolist()
pos_limits = self.root_view.get_dof_limits()[0].squeeze(0).tolist()
# create table for term information
table = PrettyTable(float_format=".3f")
table.title = f"Simulation Joint Information (Prim path: {self.cfg.prim_path})"
table.field_names = ["Index", "Name", "Stiffness", "Damping", "Armature", "Friction", "Effort Limit", "Limits"]
# set alignment of table columns
table.align["Name"] = "l"
# add info on each term
for index, name in enumerate(self.joint_names):
table.add_row(
[
index,
name,
stiffnesses[index],
dampings[index],
armatures[index],
frictions[index],
effort_limits[index],
pos_limits[index],
]
)
# convert table to string
carb.log_info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + table.get_string())
......@@ -49,13 +49,28 @@ class ArticulationData(RigidObjectData):
##
joint_pos_target: torch.Tensor = None
"""Joint position targets provided to simulation. Shape is ``(count, num_joints)``."""
"""Joint position targets commanded by the user. Shape is ``(count, num_joints)``.
For an implicit actuator model, the targets are directly set into the simulation.
For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`),
which are then set into the simulation.
"""
joint_vel_target: torch.Tensor = None
"""Joint velocity targets provided to simulation. Shape is ``(count, num_joints)``."""
"""Joint velocity targets commanded by the user. Shape is ``(count, num_joints)``.
For an implicit actuator model, the targets are directly set into the simulation.
For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`),
which are then set into the simulation.
"""
joint_effort_target: torch.Tensor = None
"""Joint effort targets provided to simulation. Shape is ``(count, num_joints)``."""
"""Joint effort targets commanded by the user. Shape is ``(count, num_joints)``.
For an implicit actuator model, the targets are directly set into the simulation.
For an explicit actuator model, the targets are used to compute the joint torques (see :attr:`applied_torque`),
which are then set into the simulation.
"""
joint_stiffness: torch.Tensor = None
"""Joint stiffness provided to simulation. Shape is ``(count, num_joints)``."""
......@@ -63,20 +78,31 @@ class ArticulationData(RigidObjectData):
joint_damping: torch.Tensor = None
"""Joint damping provided to simulation. Shape is ``(count, num_joints)``."""
joint_armature: torch.Tensor = None
"""Joint armature provided to simulation. Shape is ``(count, num_joints)``."""
joint_friction: torch.Tensor = None
"""Joint friction provided to simulation. Shape is ``(count, num_joints)``."""
##
# Joint commands -- Explicit actuators.
##
computed_torque: torch.Tensor = None
"""Joint torques computed from the actuator model (before clipping).
Shape is ``(count, num_joints)``.
"""Joint torques computed from the actuator model (before clipping). Shape is ``(count, num_joints)``.
This quantity is the raw torque output from the actuator mode, before any clipping is applied.
It is exposed for users who want to inspect the computations inside the actuator model.
For instance, to penalize the learning agent for a difference between the computed and applied torques.
Note: The torques are zero for implicit actuator models.
"""
applied_torque: torch.Tensor = None
"""Joint torques applied from the actuator model (after clipping).
Shape is ``(count, num_joints)``.
"""Joint torques applied from the actuator model (after clipping). Shape is ``(count, num_joints)``.
These torques are set into the simulation, after clipping the :attr:`computed_torque` based on the
actuator model.
Note: The torques are zero for implicit actuator models.
"""
......
......@@ -27,6 +27,7 @@ import carb
import omni.isaac.core.utils.stage as stage_utils
import omni.isaac.orbit.sim as sim_utils
import omni.isaac.orbit.utils.string as string_utils
from omni.isaac.orbit.actuators import ImplicitActuatorCfg
from omni.isaac.orbit.assets import Articulation, ArticulationCfg
from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
......@@ -239,6 +240,78 @@ class TestArticulation(unittest.TestCase):
self.assertTrue(robot.data.root_ang_vel_w[0, 2].item() > 0.1)
self.assertTrue(robot.data.root_ang_vel_w[1, 2].item() > 0.1)
def test_loading_gains_from_usd(self):
"""Test that gains are loaded from USD file if actuator model has them as None."""
# Create articulation
robot_cfg = ArticulationCfg(
prim_path="/World/Robot",
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd"),
init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)),
actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=None, damping=None)},
)
robot = Articulation(cfg=robot_cfg)
# Play sim
self.sim.reset()
# Expected gains
# -- Stiffness values
expected_stiffness = {
".*_waist.*": 20.0,
".*_upper_arm.*": 10.0,
"pelvis": 10.0,
".*_lower_arm": 2.0,
".*_thigh:0": 10.0,
".*_thigh:1": 20.0,
".*_thigh:2": 10.0,
".*_shin": 5.0,
".*_foot.*": 2.0,
}
indices_list, _, values_list = string_utils.resolve_matching_names_values(expected_stiffness, robot.joint_names)
expected_stiffness = torch.zeros(robot.root_view.count, robot.num_joints, device=robot.device)
expected_stiffness[:, indices_list] = torch.tensor(values_list, device=robot.device)
# -- Damping values
expected_damping = {
".*_waist.*": 5.0,
".*_upper_arm.*": 5.0,
"pelvis": 5.0,
".*_lower_arm": 1.0,
".*_thigh:0": 5.0,
".*_thigh:1": 5.0,
".*_thigh:2": 5.0,
".*_shin": 0.1,
".*_foot.*": 1.0,
}
indices_list, _, values_list = string_utils.resolve_matching_names_values(expected_damping, robot.joint_names)
expected_damping = torch.zeros_like(expected_stiffness)
expected_damping[:, indices_list] = torch.tensor(values_list, device=robot.device)
# Check that gains are loaded from USD file
torch.testing.assert_close(robot.actuators["body"].stiffness, expected_stiffness)
torch.testing.assert_close(robot.actuators["body"].damping, expected_damping)
def test_setting_gains_from_cfg(self):
"""Test that gains are loaded from the configuration correctly."""
# Create articulation
robot_cfg = ArticulationCfg(
prim_path="/World/Robot",
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Humanoid/humanoid_instanceable.usd"),
init_state=ArticulationCfg.InitialStateCfg(pos=(0.0, 0.0, 1.34)),
actuators={"body": ImplicitActuatorCfg(joint_names_expr=[".*"], stiffness=10.0, damping=2.0)},
)
robot = Articulation(cfg=robot_cfg)
# Play sim
self.sim.reset()
# Expected gains
expected_stiffness = torch.full((robot.root_view.count, robot.num_joints), 10.0, device=robot.device)
expected_damping = torch.full_like(expected_stiffness, 2.0)
# Check that gains are loaded from USD file
torch.testing.assert_close(robot.actuators["body"].stiffness, expected_stiffness)
torch.testing.assert_close(robot.actuators["body"].damping, expected_damping)
if __name__ == "__main__":
try:
......
Markdown is supported
0% or
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment