Unverified Commit 21bcb476 authored by Giulio Romualdi's avatar Giulio Romualdi Committed by GitHub

Randomizes viscous and dynamic joint friction based on IsaacSim 5.0 (#3318)

# Description

<!--
Thank you for your interest in sending a pull request. Please make sure
to check the contribution guidelines.

Link:
https://isaac-sim.github.io/IsaacLab/main/source/refs/contributing.html
-->

This PR fixes https://github.com/isaac-sim/IsaacLab/issues/3266, adding
the possibility to randomize the friction coefficient when isaacsim is
higher than 5.0.0

<!-- 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. -->

## Type of change

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

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

## Screenshots

Not applicable 

<!--
Example:

| Before | After |
| ------ | ----- |
| _gif/png before_ | _gif/png after_ |

To upload images to a PR -- simply drag and drop an image while in edit
mode and it should upload the image directly. You can then paste that
source into the above before/after sections.
-->

## Checklist

- [x] I have run the [`pre-commit` checks](https://pre-commit.com/) with
`./isaaclab.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

<!--
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 avatarGiulio Romualdi <giulio.romualdi@gmail.com>
Signed-off-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
Co-authored-by: 's avatarJames Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Co-authored-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
parent 902bded7
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.46.2" version = "0.46.3"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.46.3 (2025-09-17)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Modified setter to support for viscous and dynamic joint friction coefficients in articulation based on IsaacSim 5.0.
* Added randomization of viscous and dynamic joint friction coefficients in event term.
0.46.2 (2025-09-13) 0.46.2 (2025-09-13)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
...@@ -31,7 +39,6 @@ Added ...@@ -31,7 +39,6 @@ Added
during the traversal. Earlier, instanced prims were skipped since :meth:`Usd.Prim.GetChildren` did not return during the traversal. Earlier, instanced prims were skipped since :meth:`Usd.Prim.GetChildren` did not return
instanced prims, which is now fixed. instanced prims, which is now fixed.
Changed Changed
^^^^^^^ ^^^^^^^
......
...@@ -827,24 +827,33 @@ class Articulation(AssetBase): ...@@ -827,24 +827,33 @@ class Articulation(AssetBase):
def write_joint_friction_coefficient_to_sim( def write_joint_friction_coefficient_to_sim(
self, self,
joint_friction_coeff: torch.Tensor | float, joint_friction_coeff: torch.Tensor | float,
joint_dynamic_friction_coeff: torch.Tensor | float | None = None,
joint_viscous_friction_coeff: torch.Tensor | float | None = None,
joint_ids: Sequence[int] | slice | None = None, joint_ids: Sequence[int] | slice | None = None,
env_ids: Sequence[int] | None = None, env_ids: Sequence[int] | None = None,
): ):
r"""Write joint static friction coefficients into the simulation. r"""Write joint friction coefficients into the simulation.
The joint static friction is a unitless quantity. It relates the magnitude of the spatial force transmitted For Isaac Sim versions below 5.0, only the static friction coefficient is set.
from the parent body to the child body to the maximal static friction force that may be applied by the solver This limits the resisting force or torque up to a maximum proportional to the transmitted
to resist the joint motion. spatial force: :math:`\|F_{resist}\| \leq \mu_s \, \|F_{spatial}\|`.
Mathematically, this means that: :math:`F_{resist} \leq \mu F_{spatial}`, where :math:`F_{resist}` For Isaac Sim versions 5.0 and above, the static, dynamic, and viscous friction coefficients
is the resisting force applied by the solver and :math:`F_{spatial}` is the spatial force are set. The model combines Coulomb (static & dynamic) friction with a viscous term:
transmitted from the parent body to the child body. The simulated static friction effect is therefore
similar to static and Coulomb static friction. - Static friction :math:`\mu_s` defines the maximum effort that prevents motion at rest.
- Dynamic friction :math:`\mu_d` applies once motion begins and remains constant during motion.
- Viscous friction :math:`c_v` is a velocity-proportional resistive term.
Args: Args:
joint_friction_coeff: Joint static friction coefficient. Shape is (len(env_ids), len(joint_ids)). joint_friction_coeff: Static friction coefficient :math:`\mu_s`.
joint_ids: The joint indices to set the joint torque limits for. Defaults to None (all joints). Shape is (len(env_ids), len(joint_ids)). Scalars are broadcast to all selections.
env_ids: The environment indices to set the joint torque limits for. Defaults to None (all environments). joint_dynamic_friction_coeff: Dynamic (Coulomb) friction coefficient :math:`\mu_d`.
Same shape as above. If None, the dynamic coefficient is not updated.
joint_viscous_friction_coeff: Viscous friction coefficient :math:`c_v`.
Same shape as above. If None, the viscous coefficient is not updated.
joint_ids: The joint indices to set the friction coefficients for. Defaults to None (all joints).
env_ids: The environment indices to set the friction coefficients for. Defaults to None (all environments).
""" """
# resolve indices # resolve indices
physx_env_ids = env_ids physx_env_ids = env_ids
...@@ -858,15 +867,38 @@ class Articulation(AssetBase): ...@@ -858,15 +867,38 @@ class Articulation(AssetBase):
env_ids = env_ids[:, None] env_ids = env_ids[:, None]
# set into internal buffers # set into internal buffers
self._data.joint_friction_coeff[env_ids, joint_ids] = joint_friction_coeff self._data.joint_friction_coeff[env_ids, joint_ids] = joint_friction_coeff
# if dynamic or viscous friction coeffs are provided, set them too
if joint_dynamic_friction_coeff is not None:
self._data.joint_dynamic_friction_coeff[env_ids, joint_ids] = joint_dynamic_friction_coeff
if joint_viscous_friction_coeff is not None:
self._data.joint_viscous_friction_coeff[env_ids, joint_ids] = joint_viscous_friction_coeff
# move the indices to cpu
physx_envs_ids_cpu = physx_env_ids.cpu()
# set into simulation # set into simulation
if int(get_version()[2]) < 5: if int(get_version()[2]) < 5:
self.root_physx_view.set_dof_friction_coefficients( self.root_physx_view.set_dof_friction_coefficients(
self._data.joint_friction_coeff.cpu(), indices=physx_env_ids.cpu() self._data.joint_friction_coeff.cpu(), indices=physx_envs_ids_cpu
) )
else: else:
friction_props = self.root_physx_view.get_dof_friction_properties() friction_props = self.root_physx_view.get_dof_friction_properties()
friction_props[physx_env_ids.cpu(), :, 0] = self._data.joint_friction_coeff[physx_env_ids, :].cpu() friction_props[physx_envs_ids_cpu, :, 0] = self._data.joint_friction_coeff[physx_envs_ids_cpu, :].cpu()
self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_env_ids.cpu())
# only set dynamic and viscous friction if provided
if joint_dynamic_friction_coeff is not None:
friction_props[physx_envs_ids_cpu, :, 1] = self._data.joint_dynamic_friction_coeff[
physx_envs_ids_cpu, :
].cpu()
# only set viscous friction if provided
if joint_viscous_friction_coeff is not None:
friction_props[physx_envs_ids_cpu, :, 2] = self._data.joint_viscous_friction_coeff[
physx_envs_ids_cpu, :
].cpu()
self.root_physx_view.set_dof_friction_properties(friction_props, indices=physx_envs_ids_cpu)
def write_joint_dynamic_friction_coefficient_to_sim( def write_joint_dynamic_friction_coefficient_to_sim(
self, self,
......
...@@ -714,8 +714,56 @@ class randomize_joint_parameters(ManagerTermBase): ...@@ -714,8 +714,56 @@ class randomize_joint_parameters(ManagerTermBase):
operation=operation, operation=operation,
distribution=distribution, distribution=distribution,
) )
# ensure the friction coefficient is non-negative
friction_coeff = torch.clamp(friction_coeff, min=0.0)
# Always set static friction (indexed once)
static_friction_coeff = friction_coeff[env_ids[:, None], joint_ids]
# if isaacsim version is lower than 5.0.0 we can set only the static friction coefficient
major_version = int(env.sim.get_version()[0])
if major_version >= 5:
# Randomize raw tensors
dynamic_friction_coeff = _randomize_prop_by_op(
self.asset.data.default_joint_dynamic_friction_coeff.clone(),
friction_distribution_params,
env_ids,
joint_ids,
operation=operation,
distribution=distribution,
)
viscous_friction_coeff = _randomize_prop_by_op(
self.asset.data.default_joint_viscous_friction_coeff.clone(),
friction_distribution_params,
env_ids,
joint_ids,
operation=operation,
distribution=distribution,
)
# Clamp to non-negative
dynamic_friction_coeff = torch.clamp(dynamic_friction_coeff, min=0.0)
viscous_friction_coeff = torch.clamp(viscous_friction_coeff, min=0.0)
# Ensure dynamic ≤ static (same shape before indexing)
dynamic_friction_coeff = torch.minimum(dynamic_friction_coeff, friction_coeff)
# Index once at the end
dynamic_friction_coeff = dynamic_friction_coeff[env_ids[:, None], joint_ids]
viscous_friction_coeff = viscous_friction_coeff[env_ids[:, None], joint_ids]
else:
# For versions < 5.0.0, we do not set these values
dynamic_friction_coeff = None
viscous_friction_coeff = None
# Single write call for all versions
self.asset.write_joint_friction_coefficient_to_sim( self.asset.write_joint_friction_coefficient_to_sim(
friction_coeff[env_ids[:, None], joint_ids], joint_ids=joint_ids, env_ids=env_ids joint_friction_coeff=static_friction_coeff,
joint_dynamic_friction_coeff=dynamic_friction_coeff,
joint_viscous_friction_coeff=viscous_friction_coeff,
joint_ids=joint_ids,
env_ids=env_ids,
) )
# joint armature # joint armature
......
...@@ -1997,10 +1997,16 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground ...@@ -1997,10 +1997,16 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground
dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device) dynamic_friction = torch.rand(num_articulations, articulation.num_joints, device=device)
viscous_friction = torch.rand(num_articulations, articulation.num_joints, device=device) viscous_friction = torch.rand(num_articulations, articulation.num_joints, device=device)
friction = torch.rand(num_articulations, articulation.num_joints, device=device) friction = torch.rand(num_articulations, articulation.num_joints, device=device)
# Guarantee that the dynamic friction is not greater than the static friction
dynamic_friction = torch.min(dynamic_friction, friction)
# The static friction must be set first to be sure the dynamic friction is not greater than static
# when both are set.
articulation.write_joint_friction_coefficient_to_sim(friction)
if int(get_version()[2]) >= 5: if int(get_version()[2]) >= 5:
articulation.write_joint_dynamic_friction_coefficient_to_sim(dynamic_friction) articulation.write_joint_dynamic_friction_coefficient_to_sim(dynamic_friction)
articulation.write_joint_viscous_friction_coefficient_to_sim(viscous_friction) articulation.write_joint_viscous_friction_coefficient_to_sim(viscous_friction)
articulation.write_joint_friction_coefficient_to_sim(friction)
articulation.write_data_to_sim() articulation.write_data_to_sim()
for _ in range(100): for _ in range(100):
...@@ -2010,9 +2016,58 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground ...@@ -2010,9 +2016,58 @@ def test_write_joint_frictions_to_sim(sim, num_articulations, device, add_ground
articulation.update(sim.cfg.dt) articulation.update(sim.cfg.dt)
if int(get_version()[2]) >= 5: if int(get_version()[2]) >= 5:
assert torch.allclose(articulation.data.joint_dynamic_friction_coeff, dynamic_friction) friction_props_from_sim = articulation.root_physx_view.get_dof_friction_properties()
assert torch.allclose(articulation.data.joint_viscous_friction_coeff, viscous_friction) joint_friction_coeff_sim = friction_props_from_sim[:, :, 0]
assert torch.allclose(articulation.data.joint_friction_coeff, friction) joint_dynamic_friction_coeff_sim = friction_props_from_sim[:, :, 1]
joint_viscous_friction_coeff_sim = friction_props_from_sim[:, :, 2]
assert torch.allclose(joint_dynamic_friction_coeff_sim, dynamic_friction.cpu())
assert torch.allclose(joint_viscous_friction_coeff_sim, viscous_friction.cpu())
else:
joint_friction_coeff_sim = articulation.root_physx_view.get_dof_friction_properties()
assert torch.allclose(joint_friction_coeff_sim, friction.cpu())
# For Isaac Sim >= 5.0: also test the combined API that can set dynamic and viscous via
# write_joint_friction_coefficient_to_sim; reset the sim to isolate this path.
if int(get_version()[2]) >= 5:
# Reset simulator to ensure a clean state for the alternative API path
sim.reset()
# Warm up a few steps to populate buffers
for _ in range(100):
sim.step()
articulation.update(sim.cfg.dt)
# New random coefficients
dynamic_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device)
viscous_friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device)
friction_2 = torch.rand(num_articulations, articulation.num_joints, device=device)
# Guarantee that the dynamic friction is not greater than the static friction
dynamic_friction_2 = torch.min(dynamic_friction_2, friction_2)
# Use the combined setter to write all three at once
articulation.write_joint_friction_coefficient_to_sim(
joint_friction_coeff=friction_2,
joint_dynamic_friction_coeff=dynamic_friction_2,
joint_viscous_friction_coeff=viscous_friction_2,
)
articulation.write_data_to_sim()
# Step to let sim ingest new params and refresh data buffers
for _ in range(100):
sim.step()
articulation.update(sim.cfg.dt)
friction_props_from_sim_2 = articulation.root_physx_view.get_dof_friction_properties()
joint_friction_coeff_sim_2 = friction_props_from_sim_2[:, :, 0]
friction_dynamic_coef_sim_2 = friction_props_from_sim_2[:, :, 1]
friction_viscous_coeff_sim_2 = friction_props_from_sim_2[:, :, 2]
# Validate values propagated
assert torch.allclose(friction_viscous_coeff_sim_2, viscous_friction_2.cpu())
assert torch.allclose(friction_dynamic_coef_sim_2, dynamic_friction_2.cpu())
assert torch.allclose(joint_friction_coeff_sim_2, friction_2.cpu())
if __name__ == "__main__": if __name__ == "__main__":
......
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