Commit 9bb4c425 authored by David Hoeller's avatar David Hoeller Committed by Kelly Guo

Updates the URDF and MJCF importers for Isaac Sim 4.5 (#182)

Updates the URDF and MJCF importers for Isaac Sim 4.5.

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

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

---------
Co-authored-by: 's avatarKelly Guo <kellyg@nvidia.com>
Co-authored-by: 's avatarKelly Guo <kellyguo123@hotmail.com>
parent 51ffd32a
......@@ -37,23 +37,30 @@ For using the URDF importer in the GUI, please check the documentation at `URDF
is then passed to the :class:`~sim.converters.UrdfConverter` class.
The URDF importer has various configuration parameters that can be set to control the behavior of the importer.
The default values for the importer's configuration parameters are specified are in the :class:`~sim.converters.UrdfConverterCfg` class, and they are listed below. We made a few commonly modified settings to be available as command-line arguments when calling the ``convert_urdf.py``, and they are marked with ``*``` in the list. For a comprehensive list of the configuration parameters, please check the the documentation at `URDF importer`_.
The default values for the importer's configuration parameters are specified are in the :class:`~sim.converters.UrdfConverterCfg` class, and they are listed below. We made a few commonly modified settings to be available as command-line arguments when calling the ``convert_urdf.py``, and they are marked with ``*`` in the list. For a comprehensive list of the configuration parameters, please check the the documentation at `URDF importer`_.
* :attr:`~sim.converters.UrdfConverterCfg.fix_base*` - Whether to fix the base of the robot.
* :attr:`~sim.converters.UrdfConverterCfg.fix_base` * - Whether to fix the base of the robot.
This depends on whether you have a floating-base or fixed-base robot. The command-line flag is
``--fix-base`` where when set, the importer will fix the base of the robot, otherwise it will default to floating-base.
* :attr:`~sim.converters.UrdfConverterCfg.merge_fixed_joints*` - Whether to merge the fixed joints.
* :attr:`~sim.converters.UrdfConverterCfg.root_link_name` - The link on which the PhysX articulation root is placed.
* :attr:`~sim.converters.UrdfConverterCfg.merge_fixed_joints` * - Whether to merge the fixed joints.
Usually, this should be set to ``True`` to reduce the asset complexity. The command-line flag is
``--merge-joints`` where when set, the importer will merge the fixed joints, otherwise it will default to not merging the fixed joints.
* :attr:`~sim.converters.UrdfConverterCfg.default_drive_type` - The drive-type for the joints.
We recommend this to always be ``"none"``. This allows changing the drive configuration using the
actuator models.
* :attr:`~sim.converters.UrdfConverterCfg.default_drive_stiffness` - The drive stiffness for the joints.
We recommend this to always be ``0.0``. This allows changing the drive configuration using the
actuator models.
* :attr:`~sim.converters.UrdfConverterCfg.default_drive_damping` - The drive damping for the joints.
Similar to the stiffness, we recommend this to always be ``0.0``.
* :attr:`~sim.converters.UrdfConverterCfg.joint_drive` - The configuration for the joint drives on the robot.
* :attr:`~sim.converters.UrdfConverterCfg.JointDriveCfg.drive_type` - The drive type for the joints.
This can be either ``"acceleration"`` or ``"force"``. We recommend using ``"force"`` for most cases.
* :attr:`~sim.converters.UrdfConverterCfg.JointDriveCfg.target_type` - The target type for the joints.
This can be either ``"none"``, ``"position"``, or ``"velocity"``. We recommend using ``"position"`` for most cases.
Setting this to ``"none"`` will disable the drive and set the joint gains to 0.0.
* :attr:`~sim.converters.UrdfConverterCfg.JointDriveCfg.gains` - The drive stiffness and damping gains for the joint.
We support two ways to set the gains:
* :attr:`~sim.converters.UrdfConverterCfg.JointDriveCfg.PDGainsCfg` - To directly set the stiffness and damping.
* :attr:`~sim.converters.UrdfConverterCfg.JointDriveCfg.NaturalFrequencyGainsCfg` - To set the gains using the
desired natural frequency response of the system.
For more detailed information on the configuration parameters, please check the documentation for :class:`~sim.converters.UrdfConverterCfg`.
Example Usage
~~~~~~~~~~~~~
......
[package]
# Note: Semantic Versioning is used: https://semver.org/
version = "0.30.7"
version = "0.31.7"
# Description
title = "Isaac Lab framework for Robot Learning"
......
Changelog
---------
0.30.7 (2025-01-30)
0.31.7 (2025-01-30)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -12,7 +12,7 @@ Fixed
to the event being triggered at the wrong time after the reset.
0.30.6 (2025-01-17)
0.31.6 (2025-01-17)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -42,7 +42,7 @@ Fixed
the :class:`omni.isaac.lab.assets.RigidObjectCollection` class.
0.30.5 (2025-01-14)
0.31.5 (2025-01-14)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -51,17 +51,17 @@ Fixed
* Fixed the respawn of only wrong object samples in :func:`repeated_objects_terrain` of :mod:`omni.isaac.lab.terrains.trimesh` module. Previously, the function was respawning all objects in the scene instead of only the wrong object samples, which in worst case could lead to infinite respawn loop.
0.30.4 (2025-01-08)
0.31.4 (2025-01-08)
~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
* fixed docstring in articulation data :class:`omni.isaac.lab.assets.ArticulationData`.
* Fixed docstring in articulation data :class:`omni.isaac.lab.assets.ArticulationData`.
In body properties sections, the second dimension should be num_bodies but was documented as 1.
0.30.3 (2025-01-02)
0.31.3 (2025-01-02)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -70,7 +70,7 @@ Added
* Added body tracking as an origin type to :class:`omni.isaac.lab.envs.ViewerCfg` and :class:`omni.isaac.lab.envs.ui.ViewportCameraController`.
0.30.2 (2024-12-22)
0.31.2 (2024-12-22)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -79,7 +79,7 @@ Fixed
* Fixed populating default_joint_stiffness and default_joint_damping values for ImplicitActuator instances in :class:`omni.isaac.lab.assets.Articulation`
0.30.1 (2024-12-17)
0.31.1 (2024-12-17)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -93,7 +93,7 @@ Added
:class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class.
0.30.0 (2024-12-16)
0.31.0 (2024-12-16)
~~~~~~~~~~~~~~~~~~~
Changed
......@@ -102,7 +102,7 @@ Changed
* Previously, physx returns the rigid bodies and articulations velocities in the com of bodies rather than the link frame, while poses are in link frames. We now explicitly provide :attr:`body_link_state` and :attr:`body_com_state` APIs replacing the previous :attr:`body_state` API. Previous APIs are now marked as deprecated. Please update any code using the previous pose and velocity APIs to use the new ``*_link_*`` or ``*_com_*`` APIs in :attr:`omni.isaac_lab.assets.RigidBody`, :attr:`omni.isaac_lab.assets.RigidBodyCollection`, and :attr:`omni.isaac_lab.assets.Articulation`.
0.29.3 (2024-12-16)
0.30.3 (2024-12-16)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -111,7 +111,7 @@ Fixed
* Fixed ordering of logging and resamping in the command manager, where we were logging the metrics after resampling the commands. This leads to incorrect logging of metrics when inside the resample call, the metrics tensors get reset.
0.29.2 (2024-12-16)
0.30.2 (2024-12-16)
~~~~~~~~~~~~~~~~~~~
Fixed
......@@ -128,7 +128,7 @@ Added
* Added the implementation of :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class.
0.29.1 (2024-12-15)
0.30.1 (2024-12-15)
~~~~~~~~~~~~~~~~~~~
Changed
......@@ -137,7 +137,7 @@ Changed
* Added call to update articulation kinematics after reset to ensure states are updated for non-rendering sensors. Previously, some changes in reset such as modifying joint states would not be reflected in the rigid body states immediately after reset.
0.29.0 (2024-12-15)
0.30.0 (2024-12-15)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -151,7 +151,7 @@ Added
* Additions to :class:`BaseEnvWindow` and :class:`RLEnvWindow` to register ManagerLiveVisualizer UI interfaces for the chosen managers.
0.28.0 (2024-12-15)
0.29.0 (2024-12-15)
~~~~~~~~~~~~~~~~~~~
Added
......@@ -163,8 +163,8 @@ Added
* Added full buffer property to :class:`omni.isaac.lab.utils.buffers.circular_buffer.CircularBuffer`
0.27.36 (2024-12-15)
~~~~~~~~~~~~~~~~~~~~
0.28.3 (2024-12-15)
~~~~~~~~~~~~~~~~~~~
Added
^^^^^
......@@ -172,8 +172,8 @@ Added
* Added action clip to all :class:`omni.isaac.lab.envs.mdp.actions`.
0.27.35 (2024-12-14)
~~~~~~~~~~~~~~~~~~~~
0.28.2 (2024-12-14)
~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
......@@ -181,8 +181,8 @@ Changed
* Added check for error below threshold in state machines to ensure the state has been reached.
0.27.34 (2024-12-13)
~~~~~~~~~~~~~~~~~~~~
0.28.1 (2024-12-13)
~~~~~~~~~~~~~~~~~~~
Fixed
^^^^^
......@@ -190,12 +190,22 @@ Fixed
* Fixed the shape of ``quat_w`` in the ``apply_actions`` method of :attr:`~omni.isaac.lab.env.mdp.NonHolonomicAction` (previously (N,B,4), now (N,4) since the number of root bodies B is required to be 1). Previously ``apply_actions`` errored because ``euler_xyz_from_quat`` requires inputs of shape (N,4).
0.27.33 (2024-12-11)
~~~~~~~~~~~~~~~~~~~~
0.28.0 (2024-12-12)
~~~~~~~~~~~~~~~~~~~
Changed
^^^^^^^
* Adapted the :class:`~omni.isaac.lab.sim.converters.UrdfConverter` to use the latest URDF converter API from Isaac Sim 4.5. The
physics articulation root can now be set separately, and the joint drive gains can be set on a per joint basis.
0.27.33 (2024-12-11)
~~~~~~~~~~~~~~~~~~~~
Added
^^^^^
* Introduced an optional ``sensor_cfg`` parameter to the :meth:`~omni.isaac.lab.envs.mdp.rewards.base_height_l2` function, enabling the use of
:class:`~omni.isaac.lab.sensors.RayCaster` for height adjustments. For flat terrains, the function retains its previous behavior.
* Improved documentation to clarify the usage of the :meth:`~omni.isaac.lab.envs.mdp.rewards.base_height_l2` function in both flat and rough terrain settings.
......
......@@ -10,8 +10,6 @@ import os
import isaacsim
import omni.kit.commands
import omni.usd
from isaacsim.core.utils.extensions import enable_extension
from pxr import Usd
from .asset_converter_base import AssetConverterBase
from .mjcf_converter_cfg import MjcfConverterCfg
......@@ -57,47 +55,24 @@ class MjcfConverter(AssetConverterBase):
Args:
cfg: The configuration instance for MJCF to USD conversion.
"""
import_config = self._get_mjcf_import_config(cfg)
import_config = self._get_mjcf_import_config()
file_basename, _ = os.path.basename(cfg.asset_path).split(".")
omni.kit.commands.execute(
"MJCFCreateAsset",
mjcf_path=cfg.asset_path,
import_config=import_config,
dest_path=self.usd_path,
prim_path=f"/{file_basename}",
)
# fix the issue that material paths are not relative
if self.cfg.make_instanceable:
instanced_usd_path = os.path.join(self.usd_dir, self.usd_instanceable_meshes_path)
stage = Usd.Stage.Open(instanced_usd_path)
# resolve all paths relative to layer path
source_layer = stage.GetRootLayer()
omni.usd.resolve_paths(source_layer.identifier, source_layer.identifier)
stage.Save()
# fix the issue that material paths are not relative
# note: This issue seems to have popped up in Isaac Sim 2023.1.1
stage = Usd.Stage.Open(self.usd_path)
# resolve all paths relative to layer path
source_layer = stage.GetRootLayer()
omni.usd.resolve_paths(source_layer.identifier, source_layer.identifier)
stage.Save()
def _get_mjcf_import_config(self, cfg: MjcfConverterCfg) -> isaacsim.asset.importer.mjcf.ImportConfig:
def _get_mjcf_import_config(self) -> isaacsim.asset.importer.mjcf.ImportConfig:
"""Returns the import configuration for MJCF to USD conversion.
Args:
cfg: The configuration instance for MJCF to USD conversion.
Returns:
The constructed ``ImportConfig`` object containing the desired settings.
"""
# Enable MJCF Extensions
enable_extension("isaacsim.asset.importer.mjcf")
from isaacsim.asset.importer.mjcf import _mjcf as omni_mjcf
import_config = omni_mjcf.ImportConfig()
_, import_config = omni.kit.commands.execute("MJCFCreateImportConfig")
# set the unit scaling factor, 1.0 means meters, 100.0 means cm
# import_config.set_distance_scale(1.0)
......@@ -110,19 +85,19 @@ class MjcfConverter(AssetConverterBase):
# -- instancing settings
# meshes will be placed in a separate usd file
import_config.set_make_instanceable(cfg.make_instanceable)
import_config.set_make_instanceable(self.cfg.make_instanceable)
import_config.set_instanceable_usd_path(self.usd_instanceable_meshes_path)
# -- asset settings
# default density used for links, use 0 to auto-compute
import_config.set_density(cfg.link_density)
import_config.set_density(self.cfg.link_density)
# import inertia tensor from urdf, if it is not specified in urdf it will import as identity
import_config.set_import_inertia_tensor(cfg.import_inertia_tensor)
import_config.set_import_inertia_tensor(self.cfg.import_inertia_tensor)
# -- physics settings
# create fix joint for base link
import_config.set_fix_base(cfg.fix_base)
import_config.set_fix_base(self.cfg.fix_base)
# self collisions between links in the articulation
import_config.set_self_collision(cfg.self_collision)
import_config.set_self_collision(self.cfg.self_collision)
return import_config
......@@ -14,50 +14,114 @@ from omni.isaac.lab.utils import configclass
class UrdfConverterCfg(AssetConverterBaseCfg):
"""The configuration class for UrdfConverter."""
link_density = 0.0
"""Default density used for links. Defaults to 0.
@configclass
class JointDriveCfg:
This setting is only effective if ``"inertial"`` properties are missing in the URDF.
"""
@configclass
class PDGainsCfg:
"""Configuration for the PD gains of the drive."""
import_inertia_tensor: bool = True
"""Import the inertia tensor from urdf. Defaults to True.
stiffness: dict[str, float] | float = MISSING
"""The stiffness of the joint drive in Nm/rad or N/rad.
If the ``"inertial"`` tag is missing, then it is imported as an identity.
"""
If None, the stiffness is set to the value parsed from the URDF file.
If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value determines
the drive strength in joint velocity space.
"""
damping: dict[str, float] | float | None = None
"""The damping of the joint drive in Nm/(rad/s) or N/(rad/s). Defaults to None.
If None, the damping is set to the value parsed from the URDF file or 0.0 if no value is found in the URDF.
If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this attribute is set to
0.0 and :attr:`stiffness` serves as the drive's strength in joint velocity space.
"""
@configclass
class NaturalFrequencyGainsCfg:
r"""Configuration for the natural frequency gains of the drive.
Computes the joint drive stiffness and damping based on the desired natural frequency using the formula:
:math:`P = m \cdot f^2`, :math:`D = 2 \cdot r \cdot f \cdot m`
where :math:`f` is the natural frequency, :math:`r` is the damping ratio, and :math:`m` is the total
equivalent inertia at the joint. The damping ratio is such that:
* :math:`r = 1.0` is a critically damped system,
* :math:`r < 1.0` is underdamped,
* :math:`r > 1.0` is overdamped.
"""
natural_frequency: dict[str, float] | float = MISSING
"""The natural frequency of the joint drive.
If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value determines the
drive's natural frequency in joint velocity space.
"""
damping_ratio: dict[str, float] | float = 0.005
"""The damping ratio of the joint drive. Defaults to 0.005.
If :attr:`~UrdfConverterCfg.JointDriveCfg.target_type` is set to ``"velocity"``, this value is ignored and
only :attr:`natural_frequency` is used.
"""
drive_type: dict[str, Literal["acceleration", "force"]] | Literal["acceleration", "force"] = "force"
"""The drive type used for the joint. Defaults to ``"force"``.
convex_decompose_mesh = False
"""Decompose a convex mesh into smaller pieces for a closer fit. Defaults to False."""
* ``"acceleration"``: The joint drive normalizes the inertia before applying the joint effort so it's invariant
to inertia and mass changes (equivalent to ideal damped oscillator).
* ``"force"``: Applies effort through forces, so is subject to variations on the body inertia.
"""
target_type: dict[str, Literal["none", "position", "velocity"]] | Literal["none", "position", "velocity"] = (
"position"
)
"""The drive target type used for the joint. Defaults to ``"position"``.
If the target type is set to ``"none"``, the joint stiffness and damping are set to 0.0.
"""
gains: PDGainsCfg | NaturalFrequencyGainsCfg = PDGainsCfg()
"""The drive gains configuration."""
fix_base: bool = MISSING
"""Create a fix joint to the root/base link."""
merge_fixed_joints: bool = False
"""Consolidate links that are connected by fixed joints. Defaults to False."""
root_link_name: str | None = None
"""The name of the root link. Defaults to None.
self_collision: bool = False
"""Activate self-collisions between links of the articulation. Defaults to False."""
If None, the root link will be set by PhysX.
"""
default_drive_type: Literal["none", "position", "velocity"] = "none"
"""The drive type used for joints. Defaults to ``"none"``.
link_density: float = 0.0
"""Default density in ``kg/m^3`` for links whose ``"inertial"`` properties are missing in the URDF. Defaults to 0.0."""
The drive type dictates the loaded joint PD gains and USD attributes for joint control:
merge_fixed_joints: bool = True
"""Consolidate links that are connected by fixed joints. Defaults to True."""
* ``"none"``: The joint stiffness and damping are set to 0.0.
* ``"position"``: The joint stiff and damping are set based on the URDF file or provided configuration.
* ``"velocity"``: The joint stiff is set to zero and damping is based on the URDF file or provided configuration.
"""
convert_mimic_joints_to_normal_joints: bool = False
"""Convert mimic joints to normal joints. Defaults to False."""
override_joint_dynamics: bool = False
"""Override the joint dynamics parsed from the URDF file. Defaults to False."""
joint_drive: JointDriveCfg | None = JointDriveCfg()
"""The joint drive settings.
default_drive_stiffness: float = 0.0
"""The default stiffness of the joint drive. Defaults to 0.0."""
None can be used for URDFs without joints.
"""
collision_from_visuals = False
"""Create collision geometry from visual geometry."""
default_drive_damping: float = 0.0
"""The default damping of the joint drive. Defaults to 0.0.
collider_type: Literal["convex_hull", "convex_decomposition"] = "convex_hull"
"""The collision shape simplification. Defaults to ``"convex_hull"``.
Note:
If ``override_joint_dynamics`` is True, the values parsed from the URDF joint tag ``"<dynamics><damping>"`` are used.
Otherwise, it is overridden by the configured value.
* ``"convex_hull"``: The collision shape is simplified to a convex hull.
* ``"convex_decomposition"``: The collision shape is decomposed into smaller convex shapes for a closer fit.
"""
self_collision: bool = False
"""Activate self-collisions between links of the articulation. Defaults to False."""
replace_cylinders_with_capsules: bool = False
"""Replace cylinder shapes with capsule shapes. Defaults to False."""
......@@ -92,6 +92,9 @@ class MySceneCfg(InteractiveSceneCfg):
articulation_props=sim_utils.ArticulationRootPropertiesCfg(
enabled_self_collisions=True, solver_position_iteration_count=4, solver_velocity_iteration_count=0
),
joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None)
),
),
init_state=ArticulationCfg.InitialStateCfg(),
actuators={
......
......@@ -74,7 +74,11 @@ class TestSpawningFromFiles(unittest.TestCase):
extension_path = get_extension_path_from_name("isaacsim.asset.importer.urdf")
# Spawn franka from URDF
cfg = sim_utils.UrdfFileCfg(
asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf", fix_base=True
asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf",
fix_base=True,
joint_drive=sim_utils.UrdfConverterCfg.JointDriveCfg(
gains=sim_utils.UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=None, damping=None)
),
)
prim = cfg.func("/World/Franka", cfg)
# Check validity
......
......@@ -13,7 +13,6 @@ simulation_app = AppLauncher(config).app
"""Rest everything follows."""
import math
import numpy as np
import os
import unittest
......@@ -41,11 +40,16 @@ class TestUrdfConverter(unittest.TestCase):
self.config = UrdfConverterCfg(
asset_path=f"{extension_path}/data/urdf/robots/franka_description/robots/panda_arm_hand.urdf",
fix_base=True,
joint_drive=UrdfConverterCfg.JointDriveCfg(
gains=UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=400.0, damping=40.0)
),
)
# Simulation time-step
self.dt = 0.01
# Load kit helper
self.sim = SimulationContext(physics_dt=self.dt, rendering_dt=self.dt, backend="numpy")
self.sim = SimulationContext(
physics_dt=self.dt, rendering_dt=self.dt, stage_units_in_meters=1.0, backend="numpy"
)
def tearDown(self) -> None:
"""Stops simulator after each test."""
......@@ -109,10 +113,9 @@ class TestUrdfConverter(unittest.TestCase):
# change the config
self.config.force_usd_conversion = True
self.config.default_drive_type = "position"
self.config.default_drive_stiffness = 400.0
self.config.default_drive_damping = 40.0
self.config.override_joint_dynamics = True
self.config.joint_drive.target_type = "position"
self.config.joint_drive.gains.stiffness = 42.0
self.config.joint_drive.gains.damping = 4.2
self.config.usd_dir = output_dir
urdf_converter = UrdfConverter(self.config)
# check the drive type of the robot
......@@ -127,36 +130,14 @@ class TestUrdfConverter(unittest.TestCase):
# check drive values for the robot (read from physx)
drive_stiffness, drive_damping = robot.get_gains()
# -- for the arm (revolute joints)
# user provides the values in radians but simulator sets them as in degrees
expected_drive_stiffness = math.degrees(self.config.default_drive_stiffness)
expected_drive_damping = math.degrees(self.config.default_drive_damping)
np.testing.assert_array_equal(drive_stiffness[:, :7], expected_drive_stiffness)
np.testing.assert_array_equal(drive_damping[:, :7], expected_drive_damping)
# -- for the hand (prismatic joints)
# note: from isaac sim 2023.1, the test asset has mimic joints for the hand
# so the mimic joint doesn't have drive values
expected_drive_stiffness = self.config.default_drive_stiffness
expected_drive_damping = self.config.default_drive_damping
np.testing.assert_array_equal(drive_stiffness[:, 7], expected_drive_stiffness)
np.testing.assert_array_equal(drive_damping[:, 7], expected_drive_damping)
np.testing.assert_array_equal(drive_stiffness, self.config.joint_drive.gains.stiffness)
np.testing.assert_array_equal(drive_damping, self.config.joint_drive.gains.damping)
# check drive values for the robot (read from usd)
self.sim.stop()
drive_stiffness, drive_damping = robot.get_gains()
# -- for the arm (revolute joints)
# user provides the values in radians but simulator sets them as in degrees
expected_drive_stiffness = math.degrees(self.config.default_drive_stiffness)
expected_drive_damping = math.degrees(self.config.default_drive_damping)
np.testing.assert_array_equal(drive_stiffness[:, :7], expected_drive_stiffness)
np.testing.assert_array_equal(drive_damping[:, :7], expected_drive_damping)
# -- for the hand (prismatic joints)
# note: from isaac sim 2023.1, the test asset has mimic joints for the hand
# so the mimic joint doesn't have drive values
expected_drive_stiffness = self.config.default_drive_stiffness
expected_drive_damping = self.config.default_drive_damping
np.testing.assert_array_equal(drive_stiffness[:, 7], expected_drive_stiffness)
np.testing.assert_array_equal(drive_damping[:, 7], expected_drive_damping)
np.testing.assert_array_equal(drive_stiffness, self.config.joint_drive.gains.stiffness)
np.testing.assert_array_equal(drive_damping, self.config.joint_drive.gains.damping)
if __name__ == "__main__":
......
......@@ -86,6 +86,9 @@ def main():
fix_base=args_cli.fix_base,
merge_fixed_joints=args_cli.merge_joints,
force_usd_conversion=True,
joint_drive=UrdfConverterCfg.JointDriveCfg(
gains=UrdfConverterCfg.JointDriveCfg.PDGainsCfg(stiffness=100.0, damping=1.0)
),
)
# Print info
......
......@@ -11,8 +11,6 @@ TESTS_TO_SKIP = [
"test_env_var_launch.py", # app.close issue
"test_kwarg_launch.py", # app.close issue
"test_differential_ik.py", # Failing
"test_urdf_converter.py", # need to update wrapper for URDF importer
"test_mjcf_converter.py", # need to update wrapper for MJCF importer
# lab_tasks
"test_data_collector.py", # Failing
"test_record_video.py", # Failing
......
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