Unverified Commit d7da02da authored by James Tigue's avatar James Tigue Committed by GitHub

Fixes default effort limit behavior for implicit actuators (#2098)

# Description

This MR fixes the default behavior of implicit actuators if no effort limit is set. Previously, the check
was using the class variable value instead of self which led to the wrong values getting propogated.

Fixes #2054 

## Type of change

- Bug fix (non-breaking change which fixes an issue)

## 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
- [ ] 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

---------
Signed-off-by: 's avatarJames Tigue <166445701+jtigue-bdai@users.noreply.github.com>
Co-authored-by: 's avatarMayank Mittal <12863862+Mayankm96@users.noreply.github.com>
parent 84e73359
[package] [package]
# Note: Semantic Versioning is used: https://semver.org/ # Note: Semantic Versioning is used: https://semver.org/
version = "0.36.2" version = "0.36.3"
# Description # Description
title = "Isaac Lab framework for Robot Learning" title = "Isaac Lab framework for Robot Learning"
......
Changelog Changelog
--------- ---------
0.36.3 (2025-03-17)
~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* Fixed default behavior of :class:`~isaaclab.actuators.ImplicitActuator` if no :attr:`effort_limits_sim` or
:attr:`effort_limit` is set.
0.36.2 (2025-03-12) 0.36.2 (2025-03-12)
~~~~~~~~~~~~~~~~~~~ ~~~~~~~~~~~~~~~~~~~
......
...@@ -88,6 +88,13 @@ class ActuatorBase(ABC): ...@@ -88,6 +88,13 @@ class ActuatorBase(ABC):
friction: torch.Tensor friction: torch.Tensor
"""The joint friction of the actuator joints. Shape is (num_envs, num_joints).""" """The joint friction of the actuator joints. Shape is (num_envs, num_joints)."""
_DEFAULT_MAX_EFFORT_SIM: ClassVar[float] = 1.0e9
"""The default maximum effort for the actuator joints in the simulation. Defaults to 1.0e9.
If the :attr:`ActuatorBaseCfg.effort_limit_sim` is not specified and the actuator is an explicit
actuator, then this value is used.
"""
def __init__( def __init__(
self, self,
cfg: ActuatorBaseCfg, cfg: ActuatorBaseCfg,
...@@ -140,8 +147,8 @@ class ActuatorBase(ABC): ...@@ -140,8 +147,8 @@ class ActuatorBase(ABC):
# For explicit models, we do not want to enforce the effort limit through the solver # For explicit models, we do not want to enforce the effort limit through the solver
# (unless it is explicitly set) # (unless it is explicitly set)
if not ActuatorBase.is_implicit_model and self.cfg.effort_limit_sim is None: if not self.is_implicit_model and self.cfg.effort_limit_sim is None:
self.cfg.effort_limit_sim = 1.0e9 self.cfg.effort_limit_sim = self._DEFAULT_MAX_EFFORT_SIM
# parse joint stiffness and damping # parse joint stiffness and damping
self.stiffness = self._parse_joint_parameter(self.cfg.stiffness, stiffness) self.stiffness = self._parse_joint_parameter(self.cfg.stiffness, stiffness)
......
...@@ -6,6 +6,8 @@ ...@@ -6,6 +6,8 @@
# needed to import for allowing type-hinting: Usd.Stage | None # needed to import for allowing type-hinting: Usd.Stage | None
from __future__ import annotations from __future__ import annotations
import math
import isaacsim.core.utils.stage as stage_utils import isaacsim.core.utils.stage as stage_utils
import omni.log import omni.log
import omni.physx.scripts.utils as physx_utils import omni.physx.scripts.utils as physx_utils
...@@ -529,7 +531,7 @@ Joint drive properties. ...@@ -529,7 +531,7 @@ Joint drive properties.
@apply_nested @apply_nested
def modify_joint_drive_properties( def modify_joint_drive_properties(
prim_path: str, drive_props: schemas_cfg.JointDrivePropertiesCfg, stage: Usd.Stage | None = None prim_path: str, cfg: schemas_cfg.JointDrivePropertiesCfg, stage: Usd.Stage | None = None
) -> bool: ) -> bool:
"""Modify PhysX parameters for a joint prim. """Modify PhysX parameters for a joint prim.
...@@ -552,7 +554,7 @@ def modify_joint_drive_properties( ...@@ -552,7 +554,7 @@ def modify_joint_drive_properties(
Args: Args:
prim_path: The prim path where to apply the joint drive schema. prim_path: The prim path where to apply the joint drive schema.
drive_props: The configuration for the joint drive. cfg: The configuration for the joint drive.
stage: The stage where to find the prim. Defaults to None, in which case the stage: The stage where to find the prim. Defaults to None, in which case the
current stage is used. current stage is used.
...@@ -587,10 +589,43 @@ def modify_joint_drive_properties( ...@@ -587,10 +589,43 @@ def modify_joint_drive_properties(
usd_drive_api = UsdPhysics.DriveAPI(prim, drive_api_name) usd_drive_api = UsdPhysics.DriveAPI(prim, drive_api_name)
if not usd_drive_api: if not usd_drive_api:
usd_drive_api = UsdPhysics.DriveAPI.Apply(prim, drive_api_name) usd_drive_api = UsdPhysics.DriveAPI.Apply(prim, drive_api_name)
# check if prim has Physx joint drive applied on it
physx_joint_api = PhysxSchema.PhysxJointAPI(prim)
if not physx_joint_api:
physx_joint_api = PhysxSchema.PhysxJointAPI.Apply(prim)
# mapping from configuration name to USD attribute name
cfg_to_usd_map = {
"max_velocity": "max_joint_velocity",
"max_effort": "max_force",
"drive_type": "type",
}
# convert to dict
cfg = cfg.to_dict()
# change the drive type to input # check if linear drive
if drive_props.drive_type is not None: is_linear_drive = prim.IsA(UsdPhysics.PrismaticJoint)
usd_drive_api.CreateTypeAttr().Set(drive_props.drive_type) # convert values for angular drives from radians to degrees units
if not is_linear_drive:
if cfg["max_velocity"] is not None:
# rad / s --> deg / s
cfg["max_velocity"] = cfg["max_velocity"] * 180.0 / math.pi
if cfg["stiffness"] is not None:
# N-m/rad --> N-m/deg
cfg["stiffness"] = cfg["stiffness"] * math.pi / 180.0
if cfg["damping"] is not None:
# N-m-s/rad --> N-m-s/deg
cfg["damping"] = cfg["damping"] * math.pi / 180.0
# set into PhysX API
for attr_name in ["max_velocity"]:
value = cfg.pop(attr_name, None)
attr_name = cfg_to_usd_map[attr_name]
safe_set_attribute_on_usd_schema(physx_joint_api, attr_name, value, camel_case=True)
# set into USD API
for attr_name, attr_value in cfg.items():
attr_name = cfg_to_usd_map.get(attr_name, attr_name)
safe_set_attribute_on_usd_schema(usd_drive_api, attr_name, attr_value, camel_case=True)
return True return True
......
...@@ -199,6 +199,36 @@ class JointDrivePropertiesCfg: ...@@ -199,6 +199,36 @@ class JointDrivePropertiesCfg:
then the joint is driven by an acceleration (usually used for kinematic joints). then the joint is driven by an acceleration (usually used for kinematic joints).
""" """
max_effort: float | None = None
"""Maximum effort that can be applied to the joint (in kg-m^2/s^2)."""
max_velocity: float | None = None
"""Maximum velocity of the joint.
The unit depends on the joint model:
* For linear joints, the unit is m/s.
* For angular joints, the unit is rad/s.
"""
stiffness: float | None = None
"""Stiffness of the joint drive.
The unit depends on the joint model:
* For linear joints, the unit is kg-m/s^2 (N/m).
* For angular joints, the unit is kg-m^2/s^2/rad (N-m/rad).
"""
damping: float | None = None
"""Damping of the joint drive.
The unit depends on the joint model:
* For linear joints, the unit is kg-m/s (N-s/m).
* For angular joints, the unit is kg-m^2/s/rad (N-m-s/rad).
"""
@configclass @configclass
class FixedTendonPropertiesCfg: class FixedTendonPropertiesCfg:
......
...@@ -43,7 +43,14 @@ class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg): ...@@ -43,7 +43,14 @@ class FileCfg(RigidObjectSpawnerCfg, DeformableObjectSpawnerCfg):
"""Properties to apply to the fixed tendons (if any).""" """Properties to apply to the fixed tendons (if any)."""
joint_drive_props: schemas.JointDrivePropertiesCfg | None = None joint_drive_props: schemas.JointDrivePropertiesCfg | None = None
"""Properties to apply to a joint.""" """Properties to apply to a joint.
.. note::
The joint drive properties set the USD attributes of all the joint drives in the asset.
We recommend using this attribute sparingly and only when necessary. Instead, please use the
:attr:`~isaaclab.assets.ArticulationCfg.actuators` parameter to set the joint drive properties
for specific joints in an articulation.
"""
visual_material_path: str = "material" visual_material_path: str = "material"
"""Path to the visual material to use for the prim. Defaults to "material". """Path to the visual material to use for the prim. Defaults to "material".
......
...@@ -27,7 +27,7 @@ import isaacsim.core.utils.prims as prim_utils ...@@ -27,7 +27,7 @@ import isaacsim.core.utils.prims as prim_utils
import isaaclab.sim as sim_utils import isaaclab.sim as sim_utils
import isaaclab.utils.math as math_utils import isaaclab.utils.math as math_utils
import isaaclab.utils.string as string_utils import isaaclab.utils.string as string_utils
from isaaclab.actuators import IdealPDActuatorCfg, ImplicitActuatorCfg from isaaclab.actuators import ActuatorBase, IdealPDActuatorCfg, ImplicitActuatorCfg
from isaaclab.assets import Articulation, ArticulationCfg from isaaclab.assets import Articulation, ArticulationCfg
from isaaclab.sim import build_simulation_context from isaaclab.sim import build_simulation_context
from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR from isaaclab.utils.assets import ISAAC_NUCLEUS_DIR
...@@ -84,7 +84,11 @@ def generate_articulation_cfg( ...@@ -84,7 +84,11 @@ def generate_articulation_cfg(
articulation_cfg = SHADOW_HAND_CFG articulation_cfg = SHADOW_HAND_CFG
elif articulation_type == "single_joint_implicit": elif articulation_type == "single_joint_implicit":
articulation_cfg = ArticulationCfg( articulation_cfg = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd"), # we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying.
spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd",
joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0),
),
actuators={ actuators={
"joint": ImplicitActuatorCfg( "joint": ImplicitActuatorCfg(
joint_names_expr=[".*"], joint_names_expr=[".*"],
...@@ -98,8 +102,12 @@ def generate_articulation_cfg( ...@@ -98,8 +102,12 @@ def generate_articulation_cfg(
}, },
) )
elif articulation_type == "single_joint_explicit": elif articulation_type == "single_joint_explicit":
# we set 80.0 default for max force because default in USD is 10e10 which makes testing annoying.
articulation_cfg = ArticulationCfg( articulation_cfg = ArticulationCfg(
spawn=sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd"), spawn=sim_utils.UsdFileCfg(
usd_path=f"{ISAAC_NUCLEUS_DIR}/Robots/Simple/revolute_articulation.usd",
joint_drive_props=sim_utils.JointDrivePropertiesCfg(max_effort=80.0, max_velocity=5.0),
),
actuators={ actuators={
"joint": IdealPDActuatorCfg( "joint": IdealPDActuatorCfg(
joint_names_expr=[".*"], joint_names_expr=[".*"],
...@@ -115,7 +123,7 @@ def generate_articulation_cfg( ...@@ -115,7 +123,7 @@ def generate_articulation_cfg(
else: else:
raise ValueError( raise ValueError(
f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal'," f"Invalid articulation type: {articulation_type}, valid options are 'humanoid', 'panda', 'anymal',"
" 'shadow_hand' and 'single_joint'." " 'shadow_hand', 'single_joint_implicit' or 'single_joint_explicit'."
) )
return articulation_cfg return articulation_cfg
...@@ -409,6 +417,19 @@ class TestArticulation(unittest.TestCase): ...@@ -409,6 +417,19 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close(articulation.data.root_state_w, default_root_state) torch.testing.assert_close(articulation.data.root_state_w, default_root_state)
# check that the max force is what we set
physx_effort_limit = articulation.root_physx_view.get_dof_max_forces().to(device)
expected_joint_effort_limit = torch.full_like(
physx_effort_limit, articulation_cfg.spawn.joint_drive_props.max_effort
)
torch.testing.assert_close(physx_effort_limit, expected_joint_effort_limit)
# check that the max velocity is what we set
physx_vel_limit = articulation.root_physx_view.get_dof_max_velocities().to(device)
expected_joint_vel_limit = torch.full_like(
physx_vel_limit, articulation_cfg.spawn.joint_drive_props.max_velocity
)
torch.testing.assert_close(physx_vel_limit, expected_joint_vel_limit)
def test_initialization_hand_with_tendons(self): def test_initialization_hand_with_tendons(self):
"""Test initialization for fixed base articulated hand with tendons.""" """Test initialization for fixed base articulated hand with tendons."""
for num_articulations in (1, 2): for num_articulations in (1, 2):
...@@ -615,7 +636,7 @@ class TestArticulation(unittest.TestCase): ...@@ -615,7 +636,7 @@ class TestArticulation(unittest.TestCase):
self.assertFalse(articulation._is_initialized) self.assertFalse(articulation._is_initialized)
def test_joint_pos_limits(self): def test_joint_pos_limits(self):
"""Test write_joint_position_limit_to_sim API and when default pos falls outside of the new limits.""" """Test write_joint_position_limit_to_sim API and when default position falls outside of the new limits."""
for num_articulations in (1, 2): for num_articulations in (1, 2):
for device in ("cuda:0", "cpu"): for device in ("cuda:0", "cpu"):
with self.subTest(num_articulations=num_articulations, device=device): with self.subTest(num_articulations=num_articulations, device=device):
...@@ -1012,43 +1033,27 @@ class TestArticulation(unittest.TestCase): ...@@ -1012,43 +1033,27 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close( torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim, physx_vel_limit articulation.actuators["joint"].velocity_limit_sim, physx_vel_limit
) )
# check that both values match for velocity limit
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim,
articulation.actuators["joint"].velocity_limit,
)
if vel_limit_sim is None: if vel_limit_sim is None:
# Case 2: both velocity limit and velocity limit sim are not set # Case 2: both velocity limit and velocity limit sim are not set
# This is the case where the velocity limit is set to the USD default value # This is the case where the velocity limit keeps its USD default value
# Case 3: velocity limit sim is not set but velocity limit is set # Case 3: velocity limit sim is not set but velocity limit is set
# For backwards compatibility, we do not set velocity limit to simulation # For backwards compatibility, we do not set velocity limit to simulation
# Thus, both default to USD default value. # Thus, both default to USD default value.
limit = articulation_cfg.spawn.joint_drive_props.max_velocity
# check to make sure the root_physx_view dof limit is not modified
self.assertGreater(physx_vel_limit[0].item(), 1e10)
# check the two are parsed and equal
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit_sim,
articulation.actuators["joint"].velocity_limit,
)
# check the value is set to USD default
torch.testing.assert_close(
articulation.actuators["joint"].velocity_limit, physx_vel_limit
)
else: else:
# Case 4: only velocity limit sim is set # Case 4: only velocity limit sim is set
# In this case, the velocity limit is set to the USD default value # In this case, the velocity limit is set to the USD value
# read the values set into the simulation limit = vel_limit_sim
expected_velocity_limit = torch.full(
(articulation.num_instances, articulation.num_joints),
vel_limit_sim,
device=articulation.device,
)
# check root_physx_view
torch.testing.assert_close(physx_vel_limit, expected_velocity_limit)
torch.testing.assert_close( # check max velocity is what we set
articulation.actuators["joint"].velocity_limit_sim, expected_velocity_limit = torch.full_like(physx_vel_limit, limit)
articulation.actuators["joint"].velocity_limit, torch.testing.assert_close(physx_vel_limit, expected_velocity_limit)
)
def test_setting_velocity_limit_explicit(self): def test_setting_velocity_limit_explicit(self):
"""Test setting of velocity limit for explicit actuators. """Test setting of velocity limit for explicit actuators.
...@@ -1097,31 +1102,24 @@ class TestArticulation(unittest.TestCase): ...@@ -1097,31 +1102,24 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close(actuator_vel_limit_sim, physx_vel_limit) torch.testing.assert_close(actuator_vel_limit_sim, physx_vel_limit)
if vel_limit is not None: if vel_limit is not None:
expected_actuator_vel_limit = torch.full( expected_actuator_vel_limit = torch.full_like(actuator_vel_limit, vel_limit)
(articulation.num_instances, articulation.num_joints),
vel_limit,
device=articulation.device,
)
# check actuator is set # check actuator is set
torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit) torch.testing.assert_close(actuator_vel_limit, expected_actuator_vel_limit)
# check physx is not velocity_limit # check physx is not velocity_limit
self.assertFalse(torch.allclose(actuator_vel_limit, physx_vel_limit)) self.assertFalse(torch.allclose(actuator_vel_limit, physx_vel_limit))
else: else:
# check actuator velocity_limit is the same as the PhysX default # check actuator velocity_limit is the same as the PhysX default
torch.testing.assert_close(actuator_vel_limit, physx_vel_limit) torch.testing.assert_close(actuator_vel_limit, physx_vel_limit)
# simulation velocity limit is set to USD value unless user overrides
if vel_limit_sim is not None: if vel_limit_sim is not None:
expected_vel_limit = torch.full( limit = vel_limit_sim
(articulation.num_instances, articulation.num_joints), else:
vel_limit_sim, limit = articulation_cfg.spawn.joint_drive_props.max_velocity
device=articulation.device,
)
# check actuator is set
# check physx is set to expected value # check physx is set to expected value
expected_vel_limit = torch.full_like(physx_vel_limit, limit)
torch.testing.assert_close(physx_vel_limit, expected_vel_limit) torch.testing.assert_close(physx_vel_limit, expected_vel_limit)
else:
# check physx is not set by vel_limit_sim
self.assertGreater(physx_vel_limit[0].item(), 1.0e9)
def test_setting_effort_limit_implicit(self): def test_setting_effort_limit_implicit(self):
"""Test setting of the effort limit for implicit actuators. """Test setting of the effort limit for implicit actuators.
...@@ -1175,24 +1173,16 @@ class TestArticulation(unittest.TestCase): ...@@ -1175,24 +1173,16 @@ class TestArticulation(unittest.TestCase):
articulation.actuators["joint"].effort_limit_sim, physx_effort_limit articulation.actuators["joint"].effort_limit_sim, physx_effort_limit
) )
if effort_limit_sim is None and effort_limit is None:
# check to make sure the root_physx_view does not match either:
# effort_limit or effort_limit_sim
self.assertNotEqual(physx_effort_limit[0].item(), effort_limit_sim)
self.assertNotEqual(physx_effort_limit[0].item(), effort_limit)
else:
# decide the limit based on what is set # decide the limit based on what is set
if effort_limit_sim is not None and effort_limit is None: if effort_limit_sim is None and effort_limit is None:
limit = articulation_cfg.spawn.joint_drive_props.max_effort
elif effort_limit_sim is not None and effort_limit is None:
limit = effort_limit_sim limit = effort_limit_sim
elif effort_limit_sim is None and effort_limit is not None: elif effort_limit_sim is None and effort_limit is not None:
limit = effort_limit limit = effort_limit
expected_effort_limit = torch.full( # check that the max force is what we set
(articulation.num_instances, articulation.num_joints), expected_effort_limit = torch.full_like(physx_effort_limit, limit)
limit,
device=articulation.device,
)
# check root_physx_view
torch.testing.assert_close(physx_effort_limit, expected_effort_limit) torch.testing.assert_close(physx_effort_limit, expected_effort_limit)
def test_setting_effort_limit_explicit(self): def test_setting_effort_limit_explicit(self):
...@@ -1240,31 +1230,26 @@ class TestArticulation(unittest.TestCase): ...@@ -1240,31 +1230,26 @@ class TestArticulation(unittest.TestCase):
torch.testing.assert_close(actuator_effort_limit_sim, physx_effort_limit) torch.testing.assert_close(actuator_effort_limit_sim, physx_effort_limit)
if effort_limit is not None: if effort_limit is not None:
expected_actuator_effort_limit = torch.full( expected_actuator_effort_limit = torch.full_like(
(articulation.num_instances, articulation.num_joints), actuator_effort_limit, effort_limit
effort_limit,
device=articulation.device,
) )
# check actuator is set # check actuator is set
torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit) torch.testing.assert_close(actuator_effort_limit, expected_actuator_effort_limit)
# check physx is not effort_limit
# both USD and effort_limit_sim are larger than effort_limit # check physx effort limit does not match the one explicit actuator has
self.assertGreater(physx_effort_limit[0].item(), effort_limit) self.assertFalse(torch.allclose(actuator_effort_limit, physx_effort_limit))
else: else:
# check actuator effort_limit is the same as the PhysX default # check actuator effort_limit is the same as the PhysX default
torch.testing.assert_close(actuator_effort_limit, physx_effort_limit) torch.testing.assert_close(actuator_effort_limit, physx_effort_limit)
# when using explicit actuators, the limits are set to high unless user overrides
if effort_limit_sim is not None: if effort_limit_sim is not None:
expected_effort_limit = torch.full( limit = effort_limit_sim
(articulation.num_instances, articulation.num_joints),
effort_limit_sim,
device=articulation.device,
)
# check physx is set to expected value
torch.testing.assert_close(physx_effort_limit, expected_effort_limit)
else: else:
# check physx is not set by vel_limit_sim limit = ActuatorBase._DEFAULT_MAX_EFFORT_SIM # type: ignore
self.assertAlmostEqual(physx_effort_limit[0].item(), 1.0e9) # check physx internal value matches the expected sim value
expected_effort_limit = torch.full_like(physx_effort_limit, limit)
torch.testing.assert_close(physx_effort_limit, expected_effort_limit)
def test_reset(self): def test_reset(self):
"""Test that reset method works properly. """Test that reset method works properly.
...@@ -1320,7 +1305,7 @@ class TestArticulation(unittest.TestCase): ...@@ -1320,7 +1305,7 @@ class TestArticulation(unittest.TestCase):
# update buffers # update buffers
articulation.update(sim.cfg.dt) articulation.update(sim.cfg.dt)
# reset dof state # reset joint state
joint_pos = articulation.data.default_joint_pos joint_pos = articulation.data.default_joint_pos
joint_pos[:, 3] = 0.0 joint_pos[:, 3] = 0.0
......
...@@ -12,6 +12,7 @@ simulation_app = AppLauncher(headless=True).app ...@@ -12,6 +12,7 @@ simulation_app = AppLauncher(headless=True).app
"""Rest everything follows.""" """Rest everything follows."""
import math
import unittest import unittest
import isaacsim.core.utils.prims as prim_utils import isaacsim.core.utils.prims as prim_utils
...@@ -71,7 +72,9 @@ class TestPhysicsSchema(unittest.TestCase): ...@@ -71,7 +72,9 @@ class TestPhysicsSchema(unittest.TestCase):
torsional_patch_radius=1.0, torsional_patch_radius=1.0,
) )
self.mass_cfg = schemas.MassPropertiesCfg(mass=1.0, density=100.0) self.mass_cfg = schemas.MassPropertiesCfg(mass=1.0, density=100.0)
self.joint_cfg = schemas.JointDrivePropertiesCfg(drive_type="acceleration") self.joint_cfg = schemas.JointDrivePropertiesCfg(
drive_type="acceleration", max_effort=80.0, max_velocity=10.0, stiffness=10.0, damping=0.1
)
def tearDown(self) -> None: def tearDown(self) -> None:
"""Stops simulator after each test.""" """Stops simulator after each test."""
...@@ -347,17 +350,44 @@ class TestPhysicsSchema(unittest.TestCase): ...@@ -347,17 +350,44 @@ class TestPhysicsSchema(unittest.TestCase):
# skip names we know are not present # skip names we know are not present
if attr_name == "func": if attr_name == "func":
continue continue
# manually check joint type # resolve the drive (linear or angular)
drive_model = "linear" if joint_prim.IsA(UsdPhysics.PrismaticJoint) else "angular"
# manually check joint type since it is a string type
if attr_name == "drive_type": if attr_name == "drive_type":
if joint_prim.IsA(UsdPhysics.PrismaticJoint): prim_attr_name = f"drive:{drive_model}:physics:type"
prim_attr_name = "drive:linear:physics:type"
elif joint_prim.IsA(UsdPhysics.RevoluteJoint):
prim_attr_name = "drive:angular:physics:type"
else:
raise ValueError(f"Unknown joint type for prim {joint_prim.GetPrimPath()}")
# check the value # check the value
self.assertEqual(attr_value, joint_prim.GetAttribute(prim_attr_name).Get()) self.assertEqual(attr_value, joint_prim.GetAttribute(prim_attr_name).Get())
continue continue
# non-string attributes
if attr_name == "max_velocity":
prim_attr_name = "physxJoint:maxJointVelocity"
elif attr_name == "max_effort":
prim_attr_name = f"drive:{drive_model}:physics:maxForce"
else:
prim_attr_name = f"drive:{drive_model}:physics:{to_camel_case(attr_name, to='cC')}"
# obtain value from USD API (for angular, these follow degrees unit)
prim_attr_value = joint_prim.GetAttribute(prim_attr_name).Get()
# for angular drives, we expect user to set in radians
# the values reported by USD are in degrees
if drive_model == "angular":
if attr_name == "max_velocity":
# deg / s --> rad / s
prim_attr_value = prim_attr_value * math.pi / 180.0
elif attr_name in ["stiffness", "damping"]:
# N-m/deg or N-m-s/deg --> N-m/rad or N-m-s/rad
prim_attr_value = prim_attr_value * 180.0 / math.pi
# validate the values
self.assertAlmostEqual(
prim_attr_value,
attr_value,
places=5,
msg=f"Failed setting for {prim_attr_name}",
)
elif verbose: elif verbose:
print(f"Skipping prim {joint_prim.GetPrimPath()} as it is not a joint drive api.") print(f"Skipping prim {joint_prim.GetPrimPath()} as it is not a joint drive api.")
......
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