Commit 7a176fa9 authored by oahmednv's avatar oahmednv Committed by Kelly Guo

Adds support for spatial tendons (#443)

Adds spatial tendon APIs.
TODO: unit tests will be added in a separate PR.

Fixes # (issue)

<!-- As a practice, it is recommended to open an issue to have
discussions on the proposed pull request.
This makes it easier for the community to keep track of what is being
developed or added, and if a given feature
is demanded by more than one party. -->

<!-- As you go through the list, delete the ones that are not
applicable. -->

- New feature (non-breaking change which adds functionality)

- [ ] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.sh --format`
- [ ] I have made corresponding changes to the documentation
- [ ] My changes generate no new warnings
- [ ] I have added tests that prove my fix is effective or that my
feature works
- [ ] I have updated the changelog and the corresponding version in the
extension's `config/extension.toml` file
- [ ] I have added my name to the `CONTRIBUTORS.md` or my name already
exists there

<!--
As you go through the checklist above, you can mark something as done by
putting an x character in it

For example,
- [x] I have done this task
- [ ] I have not done this task
-->

---------
Signed-off-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
Signed-off-by: 's avatarKelly Guo <kellyg@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
parent 2ebd5edb
......@@ -265,12 +265,23 @@ Changed
to make it available for mdp functions.
0.41.0 (2025-05-16)
0.41.0 (2025-05-19)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added simulation schemas for spatial tendons. These can be configured for assets imported
from file formats.
* Added support for spatial tendons.
0.40.14 (2025-05-29)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Added deprecation warning for :meth:`~isaaclab.utils.math.quat_rotate` and
:meth:`~isaaclab.utils.math.quat_rotate_inverse`
......
......@@ -126,6 +126,11 @@ class Articulation(AssetBase):
"""Number of fixed tendons in articulation."""
return self.root_physx_view.max_fixed_tendons
@property
def num_spatial_tendons(self) -> int:
"""Number of spatial tendons in articulation."""
return self.root_physx_view.max_spatial_tendons
@property
def num_bodies(self) -> int:
"""Number of bodies in articulation."""
......@@ -141,6 +146,11 @@ class Articulation(AssetBase):
"""Ordered names of fixed tendons in articulation."""
return self._fixed_tendon_names
@property
def spatial_tendon_names(self) -> list[str]:
"""Ordered names of spatial tendons in articulation."""
return self._spatial_tendon_names
@property
def body_names(self) -> list[str]:
"""Ordered names of bodies in articulation."""
......@@ -267,6 +277,28 @@ class Articulation(AssetBase):
# find tendons
return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order)
def find_spatial_tendons(
self, name_keys: str | Sequence[str], tendon_subsets: list[str] | None = None, preserve_order: bool = False
) -> tuple[list[int], list[str]]:
"""Find spatial tendons in the articulation based on the name keys.
Please see the :func:`isaaclab.utils.string.resolve_matching_names` function for more information
on the name matching.
Args:
name_keys: A regular expression or a list of regular expressions to match the tendon names.
tendon_subsets: A subset of tendons to search for. Defaults to None, which means all tendons
in the articulation are searched.
preserve_order: Whether to preserve the order of the name keys in the output. Defaults to False.
Returns:
A tuple of lists containing the tendon indices and names.
"""
if tendon_subsets is None:
tendon_subsets = self.spatial_tendon_names
# find tendons
return string_utils.resolve_matching_names(name_keys, tendon_subsets, preserve_order)
"""
Operations - State Writers.
"""
......@@ -1148,6 +1180,137 @@ class Articulation(AssetBase):
indices=physx_env_ids,
)
def set_spatial_tendon_stiffness(
self,
stiffness: torch.Tensor,
spatial_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Set spatial tendon stiffness into internal buffers.
This function does not apply the tendon stiffness to the simulation. It only fills the buffers with
the desired values. To apply the tendon stiffness, call the :meth:`write_spatial_tendon_properties_to_sim` function.
Args:
stiffness: Spatial tendon stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)).
spatial_tendon_ids: The tendon indices to set the stiffness for. Defaults to None (all spatial tendons).
env_ids: The environment indices to set the stiffness for. Defaults to None (all environments).
"""
# resolve indices
if env_ids is None:
env_ids = slice(None)
if spatial_tendon_ids is None:
spatial_tendon_ids = slice(None)
if env_ids != slice(None) and spatial_tendon_ids != slice(None):
env_ids = env_ids[:, None]
# set stiffness
self._data.spatial_tendon_stiffness[env_ids, spatial_tendon_ids] = stiffness
def set_spatial_tendon_damping(
self,
damping: torch.Tensor,
spatial_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Set spatial tendon damping into internal buffers.
This function does not apply the tendon damping to the simulation. It only fills the buffers with
the desired values. To apply the tendon damping, call the :meth:`write_spatial_tendon_properties_to_sim` function.
Args:
damping: Spatial tendon damping. Shape is (len(env_ids), len(spatial_tendon_ids)).
spatial_tendon_ids: The tendon indices to set the damping for. Defaults to None (all spatial tendons).
env_ids: The environment indices to set the damping for. Defaults to None (all environments).
"""
# resolve indices
if env_ids is None:
env_ids = slice(None)
if spatial_tendon_ids is None:
spatial_tendon_ids = slice(None)
if env_ids != slice(None) and spatial_tendon_ids != slice(None):
env_ids = env_ids[:, None]
# set damping
self._data.spatial_tendon_damping[env_ids, spatial_tendon_ids] = damping
def set_spatial_tendon_limit_stiffness(
self,
limit_stiffness: torch.Tensor,
spatial_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Set spatial tendon limit stiffness into internal buffers.
This function does not apply the tendon limit stiffness to the simulation. It only fills the buffers with
the desired values. To apply the tendon limit stiffness, call the :meth:`write_spatial_tendon_properties_to_sim` function.
Args:
limit_stiffness: Spatial tendon limit stiffness. Shape is (len(env_ids), len(spatial_tendon_ids)).
spatial_tendon_ids: The tendon indices to set the limit stiffness for. Defaults to None (all spatial tendons).
env_ids: The environment indices to set the limit stiffness for. Defaults to None (all environments).
"""
# resolve indices
if env_ids is None:
env_ids = slice(None)
if spatial_tendon_ids is None:
spatial_tendon_ids = slice(None)
if env_ids != slice(None) and spatial_tendon_ids != slice(None):
env_ids = env_ids[:, None]
# set limit stiffness
self._data.spatial_tendon_limit_stiffness[env_ids, spatial_tendon_ids] = limit_stiffness
def set_spatial_tendon_offset(
self,
offset: torch.Tensor,
spatial_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Set spatial tendon offset efforts into internal buffers.
This function does not apply the tendon offset to the simulation. It only fills the buffers with
the desired values. To apply the tendon offset, call the :meth:`write_spatial_tendon_properties_to_sim` function.
Args:
offset: Spatial tendon offset. Shape is (len(env_ids), len(spatial_tendon_ids)).
spatial_tendon_ids: The tendon indices to set the offset for. Defaults to None (all spatial tendons).
env_ids: The environment indices to set the offset for. Defaults to None (all environments).
"""
# resolve indices
if env_ids is None:
env_ids = slice(None)
if spatial_tendon_ids is None:
spatial_tendon_ids = slice(None)
if env_ids != slice(None) and spatial_tendon_ids != slice(None):
env_ids = env_ids[:, None]
# set offset
self._data.spatial_tendon_offset[env_ids, spatial_tendon_ids] = offset
def write_spatial_tendon_properties_to_sim(
self,
spatial_tendon_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None,
):
"""Write spatial tendon properties into the simulation.
Args:
spatial_tendon_ids: The spatial tendon indices to set the properties for. Defaults to None (all spatial tendons).
env_ids: The environment indices to set the properties for. Defaults to None (all environments).
"""
# resolve indices
physx_env_ids = env_ids
if env_ids is None:
physx_env_ids = self._ALL_INDICES
if spatial_tendon_ids is None:
spatial_tendon_ids = slice(None)
# set into simulation
self.root_physx_view.set_spatial_tendon_properties(
self._data.spatial_tendon_stiffness,
self._data.spatial_tendon_damping,
self._data.spatial_tendon_limit_stiffness,
self._data.spatial_tendon_offset,
indices=physx_env_ids,
)
"""
Internal helper.
"""
......@@ -1412,9 +1575,9 @@ class Articulation(AssetBase):
"""Process fixed tendons."""
# create a list to store the fixed tendon names
self._fixed_tendon_names = list()
self._spatial_tendon_names = list()
# parse fixed tendons properties if they exist
if self.num_fixed_tendons > 0:
if self.num_fixed_tendons > 0 or self.num_spatial_tendons > 0:
stage = stage_utils.get_current_stage()
joint_paths = self.root_physx_view.dof_paths[0]
......@@ -1426,10 +1589,15 @@ class Articulation(AssetBase):
if joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAxisRootAPI):
joint_name = usd_joint_path.split("/")[-1]
self._fixed_tendon_names.append(joint_name)
elif joint.GetPrim().HasAPI(PhysxSchema.PhysxTendonAttachmentRootAPI) or joint.GetPrim().HasAPI(
PhysxSchema.PhysxTendonAttachmentLeafAPI
):
joint_name = usd_joint_path.split("/")[-1]
self._spatial_tendon_names.append(joint_name)
# store the fixed tendon names
self._data.fixed_tendon_names = self._fixed_tendon_names
self._data.spatial_tendon_names = self._spatial_tendon_names
# store the current USD fixed tendon properties
self._data.default_fixed_tendon_stiffness = self.root_physx_view.get_fixed_tendon_stiffnesses().clone()
self._data.default_fixed_tendon_damping = self.root_physx_view.get_fixed_tendon_dampings().clone()
......@@ -1439,6 +1607,12 @@ class Articulation(AssetBase):
self._data.default_fixed_tendon_pos_limits = self.root_physx_view.get_fixed_tendon_limits().clone()
self._data.default_fixed_tendon_rest_length = self.root_physx_view.get_fixed_tendon_rest_lengths().clone()
self._data.default_fixed_tendon_offset = self.root_physx_view.get_fixed_tendon_offsets().clone()
self._data.default_spatial_tendon_stiffness = self.root_physx_view.get_spatial_tendon_stiffnesses().clone()
self._data.default_spatial_tendon_damping = self.root_physx_view.get_spatial_tendon_dampings().clone()
self._data.default_spatial_tendon_limit_stiffness = (
self.root_physx_view.get_spatial_tendon_limit_stiffnesses().clone()
)
self._data.default_spatial_tendon_offset = self.root_physx_view.get_spatial_tendon_offsets().clone()
# store a copy of the default values for the fixed tendons
self._data.fixed_tendon_stiffness = self._data.default_fixed_tendon_stiffness.clone()
......@@ -1447,6 +1621,10 @@ class Articulation(AssetBase):
self._data.fixed_tendon_pos_limits = self._data.default_fixed_tendon_pos_limits.clone()
self._data.fixed_tendon_rest_length = self._data.default_fixed_tendon_rest_length.clone()
self._data.fixed_tendon_offset = self._data.default_fixed_tendon_offset.clone()
self._data.spatial_tendon_stiffness = self._data.default_spatial_tendon_stiffness.clone()
self._data.spatial_tendon_damping = self._data.default_spatial_tendon_damping.clone()
self._data.spatial_tendon_limit_stiffness = self._data.default_spatial_tendon_limit_stiffness.clone()
self._data.spatial_tendon_offset = self._data.default_spatial_tendon_offset.clone()
def _apply_actuator_model(self):
"""Processes joint commands for the articulation by forwarding them to the actuators.
......@@ -1581,7 +1759,7 @@ class Articulation(AssetBase):
# convert table to string
omni.log.info(f"Simulation parameters for joints in {self.cfg.prim_path}:\n" + joint_table.get_string())
# read out all tendon parameters from simulation
# read out all fixed tendon parameters from simulation
if self.num_fixed_tendons > 0:
# -- gains
ft_stiffnesses = self.root_physx_view.get_fixed_tendon_stiffnesses()[0].tolist()
......@@ -1617,7 +1795,41 @@ class Articulation(AssetBase):
ft_offsets[index],
])
# convert table to string
omni.log.info(f"Simulation parameters for tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string())
omni.log.info(
f"Simulation parameters for fixed tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string()
)
if self.num_spatial_tendons > 0:
# -- gains
st_stiffnesses = self.root_physx_view.get_spatial_tendon_stiffnesses()[0].tolist()
st_dampings = self.root_physx_view.get_spatial_tendon_dampings()[0].tolist()
# -- limits
st_limit_stiffnesses = self.root_physx_view.get_spatial_tendon_limit_stiffnesses()[0].tolist()
st_offsets = self.root_physx_view.get_spatial_tendon_offsets()[0].tolist()
# create table for term information
tendon_table = PrettyTable()
tendon_table.title = f"Simulation Spatial Tendon Information (Prim path: {self.cfg.prim_path})"
tendon_table.field_names = [
"Index",
"Stiffness",
"Damping",
"Limit Stiffness",
"Offset",
]
tendon_table.float_format = ".3"
# add info on each term
for index in range(self.num_spatial_tendons):
tendon_table.add_row([
index,
st_stiffnesses[index],
st_dampings[index],
st_limit_stiffnesses[index],
st_offsets[index],
])
# convert table to string
omni.log.info(
f"Simulation parameters for spatial tendons in {self.cfg.prim_path}:\n" + tendon_table.get_string()
)
"""
Deprecated methods.
......
......@@ -110,6 +110,9 @@ class ArticulationData:
fixed_tendon_names: list[str] = None
"""Fixed tendon names in the order parsed by the simulation view."""
spatial_tendon_names: list[str] = None
"""Spatial tendon names in the order parsed by the simulation view."""
##
# Defaults - Initial state.
##
......@@ -199,44 +202,67 @@ class ArticulationData:
The limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_stiffness: torch.Tensor = None
"""Default tendon stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).
"""Default tendon stiffness of all fixed tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_damping: torch.Tensor = None
"""Default tendon damping of all tendons. Shape is (num_instances, num_fixed_tendons).
"""Default tendon damping of all fixed tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_limit_stiffness: torch.Tensor = None
"""Default tendon limit stiffness of all tendons. Shape is (num_instances, num_fixed_tendons).
"""Default tendon limit stiffness of all fixed tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_rest_length: torch.Tensor = None
"""Default tendon rest length of all tendons. Shape is (num_instances, num_fixed_tendons).
"""Default tendon rest length of all fixed tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_offset: torch.Tensor = None
"""Default tendon offset of all tendons. Shape is (num_instances, num_fixed_tendons).
"""Default tendon offset of all fixed tendons. Shape is (num_instances, num_fixed_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_fixed_tendon_pos_limits: torch.Tensor = None
"""Default tendon position limits of all tendons. Shape is (num_instances, num_fixed_tendons, 2).
"""Default tendon position limits of all fixed tendons. Shape is (num_instances, num_fixed_tendons, 2).
The position limits are in the order :math:`[lower, upper]`. They are parsed from the USD schema at the time of
initialization.
"""
default_spatial_tendon_stiffness: torch.Tensor = None
"""Default tendon stiffness of all spatial tendons. Shape is (num_instances, num_spatial_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_spatial_tendon_damping: torch.Tensor = None
"""Default tendon damping of all spatial tendons. Shape is (num_instances, num_spatial_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_spatial_tendon_limit_stiffness: torch.Tensor = None
"""Default tendon limit stiffness of all spatial tendons. Shape is (num_instances, num_spatial_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
default_spatial_tendon_offset: torch.Tensor = None
"""Default tendon offset of all spatial tendons. Shape is (num_instances, num_spatial_tendons).
This quantity is parsed from the USD schema at the time of initialization.
"""
##
# Joint commands -- Set into simulation.
##
......@@ -373,6 +399,22 @@ class ArticulationData:
fixed_tendon_pos_limits: torch.Tensor = None
"""Fixed tendon position limits provided to the simulation. Shape is (num_instances, num_fixed_tendons, 2)."""
##
# Spatial tendon properties.
##
spatial_tendon_stiffness: torch.Tensor = None
"""Spatial tendon stiffness provided to the simulation. Shape is (num_instances, num_spatial_tendons)."""
spatial_tendon_damping: torch.Tensor = None
"""Spatial tendon damping provided to the simulation. Shape is (num_instances, num_spatial_tendons)."""
spatial_tendon_limit_stiffness: torch.Tensor = None
"""Spatial tendon limit stiffness provided to the simulation. Shape is (num_instances, num_spatial_tendons)."""
spatial_tendon_offset: torch.Tensor = None
"""Spatial tendon offset provided to the simulation. Shape is (num_instances, num_spatial_tendons)."""
##
# Root state properties.
##
......
......@@ -46,6 +46,7 @@ from .schemas import (
modify_joint_drive_properties,
modify_mass_properties,
modify_rigid_body_properties,
modify_spatial_tendon_properties,
)
from .schemas_cfg import (
ArticulationRootPropertiesCfg,
......@@ -55,4 +56,5 @@ from .schemas_cfg import (
JointDrivePropertiesCfg,
MassPropertiesCfg,
RigidBodyPropertiesCfg,
SpatialTendonPropertiesCfg,
)
......@@ -694,6 +694,77 @@ def modify_fixed_tendon_properties(
return True
"""
Spatial tendon properties.
"""
@apply_nested
def modify_spatial_tendon_properties(
prim_path: str, cfg: schemas_cfg.SpatialTendonPropertiesCfg, stage: Usd.Stage | None = None
) -> bool:
"""Modify PhysX parameters for a spatial tendon attachment prim.
A `spatial tendon`_ can be used to link multiple degrees of freedom of articulation joints
through length and limit constraints. For instance, it can be used to set up an equality constraint
between a driven and passive revolute joints.
The schema comprises of attributes that belong to the `PhysxTendonAxisRootAPI`_ schema.
.. note::
This function is decorated with :func:`apply_nested` that sets the properties to all the prims
(that have the schema applied on them) under the input prim path.
.. _spatial tendon: https://nvidia-omniverse.github.io/PhysX/physx/5.4.1/_api_build/classPxArticulationSpatialTendon.html
.. _PhysxTendonAxisRootAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_axis_root_a_p_i.html
.. _PhysxTendonAttachmentRootAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_attachment_root_a_p_i.html
.. _PhysxTendonAttachmentLeafAPI: https://docs.omniverse.nvidia.com/kit/docs/omni_usd_schema_physics/104.2/class_physx_schema_physx_tendon_attachment_leaf_a_p_i.html
Args:
prim_path: The prim path to the tendon attachment.
cfg: The configuration for the tendon attachment.
stage: The stage where to find the prim. Defaults to None, in which case the
current stage is used.
Returns:
True if the properties were successfully set, False otherwise.
Raises:
ValueError: If the input prim path is not valid.
"""
# obtain stage
if stage is None:
stage = stage_utils.get_current_stage()
# get USD prim
tendon_prim = stage.GetPrimAtPath(prim_path)
# check if prim has spatial tendon applied on it
has_spatial_tendon = tendon_prim.HasAPI(PhysxSchema.PhysxTendonAttachmentRootAPI) or tendon_prim.HasAPI(
PhysxSchema.PhysxTendonAttachmentLeafAPI
)
if not has_spatial_tendon:
return False
# resolve all available instances of the schema since it is multi-instance
for schema_name in tendon_prim.GetAppliedSchemas():
# only consider the spatial tendon schema
# retrieve the USD tendon api
if "PhysxTendonAttachmentRootAPI" in schema_name:
instance_name = schema_name.split(":")[-1]
physx_tendon_spatial_api = PhysxSchema.PhysxTendonAttachmentRootAPI(tendon_prim, instance_name)
elif "PhysxTendonAttachmentLeafAPI" in schema_name:
instance_name = schema_name.split(":")[-1]
physx_tendon_spatial_api = PhysxSchema.PhysxTendonAttachmentLeafAPI(tendon_prim, instance_name)
else:
continue
# convert to dict
cfg = cfg.to_dict()
# set into PhysX API
for attr_name, value in cfg.items():
safe_set_attribute_on_usd_schema(physx_tendon_spatial_api, attr_name, value, camel_case=True)
# success
return True
"""
Deformable body properties.
"""
......
......@@ -264,6 +264,37 @@ class FixedTendonPropertiesCfg:
"""Spring rest length of the tendon."""
@configclass
class SpatialTendonPropertiesCfg:
"""Properties to define spatial tendons of an articulation.
See :meth:`modify_spatial_tendon_properties` for more information.
.. note::
If the values are None, they are not modified. This is useful when you want to set only a subset of
the properties and leave the rest as-is.
"""
tendon_enabled: bool | None = None
"""Whether to enable or disable the tendon."""
stiffness: float | None = None
"""Spring stiffness term acting on the tendon's length."""
damping: float | None = None
"""The damping term acting on both the tendon length and the tendon-length limits."""
limit_stiffness: float | None = None
"""Limit stiffness term acting on the tendon's length limits."""
offset: float | None = None
"""Length offset term for the tendon.
It defines an amount to be added to the accumulated length computed for the tendon. This allows the application
to actuate the tendon by shortening or lengthening it.
"""
@configclass
class DeformableBodyPropertiesCfg:
"""Properties to apply to a deformable body.
......
......@@ -268,6 +268,8 @@ def _spawn_from_usd_file(
# modify tendon properties
if cfg.fixed_tendons_props is not None:
schemas.modify_fixed_tendon_properties(prim_path, cfg.fixed_tendons_props)
if cfg.spatial_tendons_props is not None:
schemas.modify_spatial_tendon_properties(prim_path, cfg.spatial_tendons_props)
# define drive API on the joints
# note: these are only for setting low-level simulation properties. all others should be set or are
# and overridden by the articulation/actuator properties.
......
......@@ -42,6 +42,9 @@ class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg):
fixed_tendons_props: schemas.FixedTendonsPropertiesCfg | None = None
"""Properties to apply to the fixed tendons (if any)."""
spatial_tendons_props: schemas.SpatialTendonsPropertiesCfg | None = None
"""Properties to apply to the spatial tendons (if any)."""
joint_drive_props: schemas.JointDrivePropertiesCfg | None = None
"""Properties to apply to a joint.
......
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