-
Notifications
You must be signed in to change notification settings - Fork 2.6k
[SIM-950] Actuator Drive Model Improvements #3942
New issue
Have a question about this project? Sign up for a free GitHub account to open an issue and contact its maintainers and the community.
By clicking “Sign up for GitHub”, you agree to our terms of service and privacy statement. We’ll occasionally send you account related emails.
Already on GitHub? Sign in to your account
base: main
Are you sure you want to change the base?
Changes from 2 commits
9da2352
6b86a5c
979d56a
3c67467
File filter
Filter by extension
Conversations
Jump to
Diff view
Diff view
There are no files selected for viewing
| Original file line number | Diff line number | Diff line change |
|---|---|---|
| @@ -0,0 +1,15 @@ | ||
| # Copyright (c) 2022-2025, The Isaac Lab Project Developers (https://github.com/isaac-sim/IsaacLab/blob/main/CONTRIBUTORS.md). | ||
| # All rights reserved. | ||
| # | ||
| # SPDX-License-Identifier: BSD-3-Clause | ||
|
|
||
| import omni.kit.app | ||
|
|
||
| from isaaclab.app import AppLauncher | ||
|
|
||
| app_launcher = AppLauncher(headless=True, enable_cameras=True) | ||
| simulation_app = app_launcher.app | ||
|
|
||
| app = omni.kit.app.get_app() | ||
| kit_version = app.get_kit_version() | ||
| print(kit_version) |
| Original file line number | Diff line number | Diff line change | ||||||||||||||
|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|---|
|
|
@@ -8,13 +8,12 @@ | |||||||||||||||
| import torch | ||||||||||||||||
| from abc import ABC, abstractmethod | ||||||||||||||||
| from collections.abc import Sequence | ||||||||||||||||
| from typing import TYPE_CHECKING, ClassVar | ||||||||||||||||
| from typing import ClassVar | ||||||||||||||||
|
|
||||||||||||||||
| import isaaclab.utils.string as string_utils | ||||||||||||||||
| from isaaclab.utils.types import ArticulationActions | ||||||||||||||||
|
|
||||||||||||||||
| if TYPE_CHECKING: | ||||||||||||||||
| from .actuator_cfg import ActuatorBaseCfg | ||||||||||||||||
| from .actuator_base_cfg import ActuatorBaseCfg | ||||||||||||||||
|
|
||||||||||||||||
|
|
||||||||||||||||
| class ActuatorBase(ABC): | ||||||||||||||||
|
|
@@ -76,6 +75,18 @@ class ActuatorBase(ABC): | |||||||||||||||
| For implicit actuators, the :attr:`velocity_limit` and :attr:`velocity_limit_sim` are the same. | ||||||||||||||||
| """ | ||||||||||||||||
|
|
||||||||||||||||
| drive_model: torch.Tensor | ||||||||||||||||
| """Three parameters for each joint/env defining the: | ||||||||||||||||
| (1) [:,:,0] speed_effort_gradient : float = 1 (default), | ||||||||||||||||
| (2) [:,:,1] maximum_actuator_velocity : float = torch.inf (default), and | ||||||||||||||||
| (3) [:,:,2] velocity_dependent_resistance : float = 1 (default) | ||||||||||||||||
|
|
||||||||||||||||
| which define velocity and effort dependent constraints on the motor's performance. | ||||||||||||||||
|
|
||||||||||||||||
| This feature is only implemented in IsaacSim v5.0. | ||||||||||||||||
|
|
||||||||||||||||
| The shape is (num_envs, num_joints, 3).""" | ||||||||||||||||
|
|
||||||||||||||||
| stiffness: torch.Tensor | ||||||||||||||||
| """The stiffness (P gain) of the PD controller. Shape is (num_envs, num_joints).""" | ||||||||||||||||
|
|
||||||||||||||||
|
|
@@ -116,6 +127,7 @@ def __init__( | |||||||||||||||
| viscous_friction: torch.Tensor | float = 0.0, | ||||||||||||||||
| effort_limit: torch.Tensor | float = torch.inf, | ||||||||||||||||
| velocity_limit: torch.Tensor | float = torch.inf, | ||||||||||||||||
| drive_model: torch.Tensor | tuple[float, float, float] = ActuatorBaseCfg.DriveModelCfg(), | ||||||||||||||||
| ): | ||||||||||||||||
| """Initialize the actuator. | ||||||||||||||||
|
|
||||||||||||||||
|
|
@@ -149,6 +161,9 @@ def __init__( | |||||||||||||||
| 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). | ||||||||||||||||
| drive_model: Drive model for the actuator including speed_effort_gradient, max_actuator_velocity, and | ||||||||||||||||
| velocity_dependent_resistance in that order. Defaults to (0.0, torch.inf, 0.0). | ||||||||||||||||
| If a tensor then the shape is (num_envs, num_joints, 3). | ||||||||||||||||
| """ | ||||||||||||||||
| # save parameters | ||||||||||||||||
| self.cfg = cfg | ||||||||||||||||
|
|
@@ -176,27 +191,44 @@ def __init__( | |||||||||||||||
| ("friction", friction), | ||||||||||||||||
| ("dynamic_friction", dynamic_friction), | ||||||||||||||||
| ("viscous_friction", viscous_friction), | ||||||||||||||||
| ("drive_model", drive_model, 3), | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. if you choose to create the There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. There might be a way to extract it from the DriveModelCfg. I know it automatically imposes the tuple[float,float,float] constraint. I agree, the magic number isn't ideal. |
||||||||||||||||
| ] | ||||||||||||||||
| for param_name, usd_val in to_check: | ||||||||||||||||
| for param_name, usd_val, *tuple_len in to_check: | ||||||||||||||||
| # check if the parameter requires a tuple or a single float | ||||||||||||||||
| if len(tuple_len) > 0: | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. since the only param with a There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. The goal was to implement a more general pattern which other related parameters can use. |
||||||||||||||||
| shape = (self._num_envs, self.num_joints, tuple_len[0]) | ||||||||||||||||
| else: | ||||||||||||||||
| shape = (self._num_envs, self.num_joints) | ||||||||||||||||
|
|
||||||||||||||||
| cfg_val = getattr(self.cfg, param_name) | ||||||||||||||||
| setattr(self, param_name, self._parse_joint_parameter(cfg_val, usd_val)) | ||||||||||||||||
| setattr(self, param_name, self._parse_joint_parameter(cfg_val, usd_val, shape, param_name=param_name)) | ||||||||||||||||
| new_val = getattr(self, param_name) | ||||||||||||||||
|
|
||||||||||||||||
| allclose = ( | ||||||||||||||||
| torch.all(new_val == usd_val) if isinstance(usd_val, (float, int)) else torch.allclose(new_val, usd_val) | ||||||||||||||||
| torch.all(new_val == usd_val) | ||||||||||||||||
| if isinstance(usd_val, (float, int)) | ||||||||||||||||
| else ( | ||||||||||||||||
| all([torch.all(new_val[:, :, i] == float(v)) for i, v in enumerate(usd_val)]) | ||||||||||||||||
| if isinstance(usd_val, tuple) | ||||||||||||||||
| else torch.allclose(new_val, usd_val) | ||||||||||||||||
| ) | ||||||||||||||||
| ) | ||||||||||||||||
| if cfg_val is None or not allclose: | ||||||||||||||||
| self._record_actuator_resolution( | ||||||||||||||||
| cfg_val=getattr(self.cfg, param_name), | ||||||||||||||||
| new_val=new_val[0], # new val always has the shape of (num_envs, num_joints) | ||||||||||||||||
| new_val=new_val[0], | ||||||||||||||||
| usd_val=usd_val, | ||||||||||||||||
| joint_names=joint_names, | ||||||||||||||||
| joint_ids=joint_ids, | ||||||||||||||||
| actuator_param=param_name, | ||||||||||||||||
| ) | ||||||||||||||||
|
|
||||||||||||||||
| self.velocity_limit = self._parse_joint_parameter(self.cfg.velocity_limit, self.velocity_limit_sim) | ||||||||||||||||
| self.effort_limit = self._parse_joint_parameter(self.cfg.effort_limit, self.effort_limit_sim) | ||||||||||||||||
| self.velocity_limit = self._parse_joint_parameter( | ||||||||||||||||
| self.cfg.velocity_limit, self.velocity_limit_sim, param_name="velocity_limit" | ||||||||||||||||
| ) | ||||||||||||||||
| self.effort_limit = self._parse_joint_parameter( | ||||||||||||||||
| self.cfg.effort_limit, self.effort_limit_sim, param_name="effort_limit" | ||||||||||||||||
| ) | ||||||||||||||||
|
|
||||||||||||||||
| # create commands buffers for allocation | ||||||||||||||||
| self.computed_effort = torch.zeros(self._num_envs, self.num_joints, device=self._device) | ||||||||||||||||
|
|
@@ -287,20 +319,35 @@ def _record_actuator_resolution(self, cfg_val, new_val, usd_val, joint_names, jo | |||||||||||||||
|
|
||||||||||||||||
| ids = joint_ids if isinstance(joint_ids, torch.Tensor) else list(range(len(joint_names))) | ||||||||||||||||
| for idx, name in enumerate(joint_names): | ||||||||||||||||
| cfg_val_log = "Not Specified" if cfg_val is None else float(new_val[idx]) | ||||||||||||||||
| default_usd_val = usd_val if isinstance(usd_val, (float, int)) else float(usd_val[0][idx]) | ||||||||||||||||
| applied_val_log = default_usd_val if cfg_val is None else float(new_val[idx]) | ||||||||||||||||
| table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) | ||||||||||||||||
| if len(new_val.shape) == 1: | ||||||||||||||||
| cfg_val_log = "Not Specified" if cfg_val is None else float(new_val[idx]) | ||||||||||||||||
| default_usd_val = usd_val if isinstance(usd_val, (float, int)) else float(usd_val[0][idx]) | ||||||||||||||||
| applied_val_log = default_usd_val if cfg_val is None else float(new_val[idx]) | ||||||||||||||||
| table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) | ||||||||||||||||
| else: | ||||||||||||||||
| cfg_val_log = "Not Specified" if cfg_val is None else tuple(new_val[idx]) | ||||||||||||||||
| default_usd_val = usd_val if isinstance(usd_val, (tuple)) else tuple(usd_val[0][idx][:]) | ||||||||||||||||
| applied_val_log = default_usd_val if cfg_val is None else tuple(new_val[idx]) | ||||||||||||||||
| table.append([name, int(ids[idx]), default_usd_val, cfg_val_log, applied_val_log]) | ||||||||||||||||
|
|
||||||||||||||||
| def _parse_joint_parameter( | ||||||||||||||||
| self, cfg_value: float | dict[str, float] | None, default_value: float | torch.Tensor | None | ||||||||||||||||
| self, | ||||||||||||||||
| cfg_value: tuple[float, ...] | dict[str, tuple[float, ...]] | float | dict[str, float] | None, | ||||||||||||||||
| default_value: tuple[float, ...] | float | torch.Tensor | None, | ||||||||||||||||
| expected_shape: tuple[int, ...] | None = None, | ||||||||||||||||
| *, | ||||||||||||||||
| param_name: str = "No name specified", | ||||||||||||||||
| ) -> 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. | ||||||||||||||||
| expected_shape: The expected shape for the tensor buffer. Usually defaults to (num_envs, num_joints). | ||||||||||||||||
|
|
||||||||||||||||
| Kwargs: | ||||||||||||||||
| param_name: a string with the parameter name. (Optional used only in exception messages). | ||||||||||||||||
|
|
||||||||||||||||
| Returns: | ||||||||||||||||
| The parsed parameter value. | ||||||||||||||||
|
|
@@ -309,38 +356,87 @@ def _parse_joint_parameter( | |||||||||||||||
| 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. | ||||||||||||||||
| ValueError: If the default value tensor is the wrong shape. | ||||||||||||||||
| ValueError: If a tensor or tuple is the wrong shape. | ||||||||||||||||
| """ | ||||||||||||||||
| if expected_shape is None: | ||||||||||||||||
| expected_shape = (self._num_envs, self.num_joints) | ||||||||||||||||
| # create parameter buffer | ||||||||||||||||
| param = torch.zeros(self._num_envs, self.num_joints, device=self._device) | ||||||||||||||||
| param = torch.zeros(*expected_shape, 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, tuple): | ||||||||||||||||
| # if tuple, ensure we expect a tuple for this parameter | ||||||||||||||||
| if len(expected_shape) < 3: | ||||||||||||||||
| raise TypeError( | ||||||||||||||||
| f"Invalid type for parameter value: {type(cfg_value)} for parameter {param_name}" | ||||||||||||||||
| + f"actuator on joints {self.joint_names}. Expected float or dict, got tuple" | ||||||||||||||||
| ) | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. style: Check uses
Suggested change
Comment on lines
+376
to
+377
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. syntax: Missing space before "actuator" in error message
Suggested change
|
||||||||||||||||
| # ensure the tuple is the correct length, and assign to the last tensor dimensions across all joints | ||||||||||||||||
| if not len(cfg_value) is expected_shape[2]: | ||||||||||||||||
| raise ValueError( | ||||||||||||||||
| f"Invalid tuple length for parameter {param_name}, got {len(cfg_value)}, expected" | ||||||||||||||||
| + f" {expected_shape[2]}" | ||||||||||||||||
| ) | ||||||||||||||||
| for i, v in enumerate(cfg_value): | ||||||||||||||||
| param[:, :, i] = float(v) | ||||||||||||||||
| elif isinstance(cfg_value, dict): | ||||||||||||||||
| # if dict, then parse the regular expression | ||||||||||||||||
| indices, _, values = string_utils.resolve_matching_names_values(cfg_value, self.joint_names) | ||||||||||||||||
| # note: need to specify type to be safe (e.g. values are ints, but we want floats) | ||||||||||||||||
| param[:, indices] = torch.tensor(values, dtype=torch.float, device=self._device) | ||||||||||||||||
| indices, j, values = string_utils.resolve_matching_names_values(cfg_value, self.joint_names) | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. syntax: Variable
Suggested change
|
||||||||||||||||
| # if the expected shape has two dimensions, we expect floats | ||||||||||||||||
| if len(expected_shape) < 3: | ||||||||||||||||
| # note: need to specify type to be safe (e.g. values are ints, but we want floats) | ||||||||||||||||
| param[:, indices] = torch.tensor(values, dtype=torch.float, device=self._device) | ||||||||||||||||
| # otherwise, we expect tuples | ||||||||||||||||
| else: | ||||||||||||||||
| # We can't directly assign tuples to tensors, so iterate through them | ||||||||||||||||
| for i, v in enumerate(values): | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. style: Check uses
Suggested change
|
||||||||||||||||
| # Raise an exception if the tuple is the incorrect length | ||||||||||||||||
| if len(v) is not expected_shape[2]: | ||||||||||||||||
| raise ValueError( | ||||||||||||||||
| f"Invalid tuple length for parameter {param_name} on joint {j[i]} at index" | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. syntax: Use
Suggested change
|
||||||||||||||||
| f" {indices[i]}," | ||||||||||||||||
| + f" expected {expected_shape[2]} got {len(v)}." | ||||||||||||||||
| ) | ||||||||||||||||
| # Otherwise iterate through the tuple, and assign the values in order. | ||||||||||||||||
| for i2, v2 in enumerate(v): | ||||||||||||||||
| param[:, indices[i], i2] = float(v2) | ||||||||||||||||
| else: | ||||||||||||||||
| raise TypeError( | ||||||||||||||||
| f"Invalid type for parameter value: {type(cfg_value)} for " | ||||||||||||||||
| + f"actuator on joints {self.joint_names}. Expected float or dict." | ||||||||||||||||
| + f"actuator on joints {self.joint_names}. Expected tuple, 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, tuple): | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. i might be missing context here, but im seeing a lot of checks for There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. Unfortunately not, this is where we parse the configuration file, and a design constraint was that we use simple types in the configuration classes. I originally proposed using tensors directly, but they asked me to stay with vanilla types. There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. oh yeah, not suggesting changing anything about the config class itself. my thinking was, if the output/result of the function is the same, just with branching logic based on the type, it might be simpler to convert the There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. I see. There's an additional complication which is that here What might work is to create a more general base class for tuple parameters. The NamedTuple is a good start, but maybe something that 1) exposes an enum to directly map indices (as you suggest), and 2) has something like a to_tensor() call to support casting to something that can be broadcast like a slice, which can't be done with tuples. At least then the branching logic would be more concise, and look more like a simple assignment than admittedly opaque nested loops. My concern was creating code that relies on a more complex object, and forces other code to rely on a class when a tuple will do. We could do something like cast the tuple to the tuple parameter class early in the function as you suggest. At least then the exposed api could use either a tuple, or the convenience object depending on the consumer's preference. |
||||||||||||||||
| # if tuple, ensure we expect a tuple for this parameter | ||||||||||||||||
| if len(expected_shape) < 3: | ||||||||||||||||
| raise TypeError( | ||||||||||||||||
| f"Invalid default type for parameter value: {type(default_value)} for " | ||||||||||||||||
| + f"actuator on joints {self.joint_names}. Expected float or dict, got tuple" | ||||||||||||||||
| ) | ||||||||||||||||
| # ensure the tuple is the correct length, and assign to the last tensor dimensions across all joints | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. style: Check uses
Suggested change
|
||||||||||||||||
| if not len(default_value) is expected_shape[2]: | ||||||||||||||||
|
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. style: Use
Suggested change
|
||||||||||||||||
| raise ValueError( | ||||||||||||||||
| f"Invalid tuple length for parameter {param_name}, got {len(default_value)}, expected" | ||||||||||||||||
| + f" {expected_shape[2]}" | ||||||||||||||||
| ) | ||||||||||||||||
|
Comment on lines
+426
to
+428
There was a problem hiding this comment. Choose a reason for hiding this commentThe reason will be displayed to describe this comment to others. Learn more. syntax: Missing comma in error message
Suggested change
|
||||||||||||||||
| for i, v in enumerate(default_value): | ||||||||||||||||
| param[:, :, i] = float(v) | ||||||||||||||||
| elif isinstance(default_value, torch.Tensor): | ||||||||||||||||
| # if tensor, then use the same tensor for all joints | ||||||||||||||||
| if default_value.shape == (self._num_envs, self.num_joints): | ||||||||||||||||
| if tuple(default_value.shape) == expected_shape: | ||||||||||||||||
| param = default_value.float() | ||||||||||||||||
| else: | ||||||||||||||||
| raise ValueError( | ||||||||||||||||
| "Invalid default value tensor shape.\n" | ||||||||||||||||
| f"Got: {default_value.shape}\n" | ||||||||||||||||
| f"Expected: {(self._num_envs, self.num_joints)}" | ||||||||||||||||
| + f"Got: {tuple(default_value.shape)}\n" | ||||||||||||||||
| + f"Expected: {expected_shape}" | ||||||||||||||||
| ) | ||||||||||||||||
| else: | ||||||||||||||||
| raise TypeError( | ||||||||||||||||
|
|
||||||||||||||||
There was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
a
DriveModelParamenum with a mapping ofparam -> indexwould avoid requiring the user to remember the indices of each drive model paramThere was a problem hiding this comment.
Choose a reason for hiding this comment
The reason will be displayed to describe this comment to others. Learn more.
I was trying to do this through the NamedTuple class. At least for the ActuatorBaseCfg.DriveModelCfg, they can treat the cfg either as a tuple, or a dataclass with named attributes.