Fixes the inverse kinematics example for Franka robot (#319)

# Description

This MR makes sure the IK example works properly again. It adds the IK
action space to the lift environment. Additionally, it fixes the state
machine for the lift environment.

## Type of change

- Bug fix (non-breaking change which fixes an issue)
- This change requires a documentation update

## Checklist

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

docs/source/setup/sample.rst

@@ -73,7 +73,7 @@ allows efficient execution for large number of environments using CUDA kernels.
 
 .. code:: bash
 
-   ./orbit.sh -p source/standalone/environments/state_machine/play_lift.py --num_envs 32
+   ./orbit.sh -p source/standalone/environments/state_machine/lift_cube_sm.py --num_envs 32
 
 
 Teleoperation
@@ -88,7 +88,7 @@ To play inverse kinematics (IK) control with a keyboard device:
 
 .. code:: bash
 
-   ./orbit.sh -p source/standalone/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Franka-v0 --num_envs 1 --cpu --device keyboard
+   ./orbit.sh -p source/standalone/environments/teleoperation/teleop_se3_agent.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --device keyboard
 
 The script prints the teleoperation events configured. For keyboard,
 these are as follows:
@@ -116,14 +116,14 @@ data in
 format.
 
 1. Collect demonstrations with teleoperation for the environment
-   ``Isaac-Lift-Franka-v0``:
+   ``Isaac-Lift-Cube-Franka-IK-Rel-v0``:
 
    .. code:: bash
 
       # step a: collect data with keyboard
-      ./orbit.sh -p source/standalone/workflows/robomimic/collect_demonstrations.py --task Isaac-Lift-Franka-v0 --num_envs 1 --num_demos 10 --device keyboard
+      ./orbit.sh -p source/standalone/workflows/robomimic/collect_demonstrations.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --num_envs 1 --num_demos 10 --device keyboard
       # step b: inspect the collected dataset
-      ./orbit.sh -p source/standalone/workflows/robomimic/tools/inspect_demonstrations.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5
+      ./orbit.sh -p source/standalone/workflows/robomimic/tools/inspect_demonstrations.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5
 
 2. Split the dataset into train and validation set:
 
@@ -132,20 +132,20 @@ format.
       # install python module (for robomimic)
       ./orbit.sh -e robomimic
       # split data
-      ./orbit.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5 --ratio 0.2
+      ./orbit.sh -p source/standalone//workflows/robomimic/tools/split_train_val.py logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5 --ratio 0.2
 
-3. Train a BC agent for ``Isaac-Lift-Franka-v0`` with
+3. Train a BC agent for ``Isaac-Lift-Cube-Franka-IK-Rel-v0`` with
    `Robomimic <https://robomimic.github.io/>`__:
 
    .. code:: bash
 
-      ./orbit.sh -p source/standalone/workflows/robomimic/train.py --task Isaac-Lift-Franka-v0 --algo bc --dataset logs/robomimic/Isaac-Lift-Franka-v0/hdf_dataset.hdf5
+      ./orbit.sh -p source/standalone/workflows/robomimic/train.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --algo bc --dataset logs/robomimic/Isaac-Lift-Cube-Franka-IK-Rel-v0/hdf_dataset.hdf5
 
 4. Play the learned model to visualize results:
 
    .. code:: bash
 
-      ./orbit.sh -p source/standalone//workflows/robomimic/play.py --task Isaac-Lift-Franka-v0 --checkpoint /PATH/TO/model.pth
+      ./orbit.sh -p source/standalone//workflows/robomimic/play.py --task Isaac-Lift-Cube-Franka-IK-Rel-v0 --checkpoint /PATH/TO/model.pth
 
 Reinforcement Learning
 ~~~~~~~~~~~~~~~~~~~~~~

docs/source/tutorials/05_controllers/ik_controller.rst

-Using a task-space controller
-=============================
-
-In the previous tutorials, we learned how to create a scene and control a robotic arm using a
-joint-space controller. In this tutorial, we will learn how to use a task-space controller to
-control the robot. More specifically, we will use the :class:`DifferentialInverseKinematics` class
-to track a desired end-effector pose command.
-
-The Code
-~~~~~~~~
-
-The tutorial corresponds to the ``play_ik_control.py`` script in the ``orbit/source/standalone/demo`` directory.
-
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :emphasize-lines: 44-47,130-138,147,201-217
-   :linenos:
-
-The Code Explained
-~~~~~~~~~~~~~~~~~~
-
-While using any task-space controller, it is important to ensure that the provided
-quantities are in the correct frames. When parallelizing environment instances using the cloner,
-each environment itself can be thought of having its own local frame. However, from the way
-the physics engine is implemented in Isaac Sim, all environments exist on the same stage and
-thus there is a unique global frame for the entire simulation. In summary, there are the three
-main frames that are used in Orbit:
-
-- The simulation world frame (denoted as ``w``), which is the frame of the entire simulation.
-- The local environment frame (denoted as ``e``), which is the frame of the local environment.
-- The robot's base frame (denoted as ``b``), which is the frame of the robot's base link.
-
-In the current scenario, where the robot is mounted on the table, the base frame of the robot coincides with
-the local environment frame. However, this is not always the case. For example, in a scenario where the robot
-is a floating-base system. The location of the environment frames are obtained from the
-:attr:`envs_positions` value returned by the cloner.
-
-
-Creating an IK controller
--------------------------
-
-Computing the inverse kinematics (IK) of a robot is a common task in robotics.
-The :class:`DifferentialInverseKinematics` class computes the desired joint positions
-for a robot to reach a desired end-effector pose. The included implementation performs
-the computation in a batched format and is optimized for speed.
-
-Since in many robots the end-effector is not a rigid body, the simulator does not provide
-the pose and Jacobian of the end-effector directly. Instead, the obtained Jacobian
-is that of the parent body and not the end-effector. Thus, the IK controller takes in
-as input the end-effector offset from the parent frame. This offset is typically specified
-in the robot's configuration instance and thus is obtained from there.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 130-138
-   :linenos:
-   :lineno-start: 130
-
-
-Computing robot command
------------------------
-
-The IK controller separates the operation of setting the desired command and
-computing the desired joint positions. This is done to allow for the user to
-run the IK controller at a different frequency than the robot's control frequency.
-
-The :attr:`set_command` method takes in the desired end-effector pose as a single
-batched array. The first three columns correspond to the desired position and the
-last four columns correspond to the desired quaternion orientation in ``(w, x, y, z)``
-order.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 201-202
-   :linenos:
-   :lineno-start: 201
-
-We can then compute the desired joint positions using the :attr:`compute` method.
-The method takes in the current end-effector position, orientation, Jacobian, and
-current joint positions. We read the Jacobian matrix from the robot's data, which uses
-its value computed from the physics engine.
-
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 203-209
-   :linenos:
-   :lineno-start: 203
-
-While the IK controller returns the desired joint positions, we need to convert
-them to the robot's action space. This is done by subtracting joint positions
-offsets from the desired joint positions. The joint offsets are obtained from the
-robot's data which is a constant value obtained from the robot's configuration.
-For more details, we suggest reading the :doc:`/source/api/orbit.actuators.group` tutorial.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 210-215
-   :linenos:
-   :lineno-start: 210
-
-These actions can then be applied on the robot, as done in the previous tutorials.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 216-219
-   :linenos:
-   :lineno-start: 216
-
-Using markers for displaying frames
------------------------------------
-
-We will use the :class:`StaticMarker` class to display the current and desired end-effector poses.
-The marker class takes as input the associated prim name, the number of markers to display, the
-USD file corresponding to the marker, and the scale of the marker. By default, it uses a frame marker
-to display the pose.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 81-83
-   :linenos:
-   :lineno-start: 81
-
-We can then set the pose of the marker using the :attr:`set_world_poses` method.
-It is important to ensure that the set poses are in the simulation world frame and not the
-local environment frame.
-
-.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/ik_control.py
-   :language: python
-   :lines: 223-229
-   :linenos:
-   :lineno-start: 223
-
-
-The Code Execution
-~~~~~~~~~~~~~~~~~~
-
-Now that we have gone through the code, let's run the script and see the result:
-
-.. code-block:: bash
-
-   ./orbit.sh -p source/standalone/tutorials/05_controllers/ik_control.py --robot franka_panda --num_envs 128
-
-The script will start a simulation with 128 robots. The robots will be controlled using a task-space controller.
-The current and desired end-effector poses should be displayed using frame markers. When the robot reaches the
-desired pose, the command should cycle through to the next pose specified in the script.
-To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or
-press ``Ctrl+C`` in the terminal.

docs/source/tutorials/05_controllers/index.rst

@@ -8,4 +8,4 @@ tutorials show you how to use motion generators to control the robots at the tas
     :maxdepth: 1
     :titlesonly:
 
-    ik_controller
+    run_diff_ik

docs/source/tutorials/05_controllers/run_diff_ik.rst

+Using a task-space controller
+=============================
+
+.. currentmodule:: omni.isaac.orbit
+
+In the previous tutorials, we have joint-space controllers to control the robot. However, in many
+cases, it is more intuitive to control the robot using a task-space controller. For example, if we
+want to teleoperate the robot, it is easier to specify the desired end-effector pose rather than
+the desired joint positions.
+
+In this tutorial, we will learn how to use a task-space controller to control the robot.
+We will use the :class:`controllers.DifferentialIKController` class to track a desired
+end-effector pose command.
+
+
+The Code
+~~~~~~~~
+
+The tutorial corresponds to the ``run_diff_ik.py`` script in the ``orbit/source/standalone/demo`` directory.
+
+
+.. dropdown:: Code for run_diff_ik.py
+   :icon: code
+
+   .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+      :language: python
+      :emphasize-lines: 99-101, 122-137, 156-158, 162-172
+      :linenos:
+
+
+The Code Explained
+~~~~~~~~~~~~~~~~~~
+
+While using any task-space controller, it is important to ensure that the provided
+quantities are in the correct frames. When parallelizing environment instances, they are
+all existing in the same unique simulation world frame. However, typically, we want each
+environment itself to have its own local frame. This is accessible through the
+:attr:`scene.InteractiveScene.env_origins` attribute.
+
+In our APIs, we use the following notation for frames:
+
+- The simulation world frame (denoted as ``w``), which is the frame of the entire simulation.
+- The local environment frame (denoted as ``e``), which is the frame of the local environment.
+- The robot's base frame (denoted as ``b``), which is the frame of the robot's base link.
+
+Since the asset instances are not "aware" of the local environment frame, they return
+their states in the simulation world frame. Thus, we need to convert the obtained
+quantities to the local environment frame. This is done by subtracting the local environment
+origin from the obtained quantities.
+
+
+Creating an IK controller
+-------------------------
+
+The :class:`~controllers.DifferentialIKController` class computes the desired joint
+positions for a robot to reach a desired end-effector pose. The included implementation
+performs the computation in a batched format and uses PyTorch operations. It supports
+different types of inverse kinematics solvers, including the damped least-squares method
+and the pseudo-inverse method. These solvers can be specified using the
+:attr:`~controllers.DifferentialIKControllerCfg.ik_method` argument.
+Additionally, the controller can handle commands as both relative and absolute poses.
+
+In this tutorial, we will use the damped least-squares method to compute the desired
+joint positions. Additionally, since we want to track desired end-effector poses, we
+will use the absolute pose command mode.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+   :language: python
+   :lines: 99-101
+   :linenos:
+   :lineno-start: 99
+
+Obtaining the robot's joint and body indices
+--------------------------------------------
+
+The IK controller implementation is a computation-only class. Thus, it expects the
+user to provide the necessary information about the robot. This includes the robot's
+joint positions, current end-effector pose, and the Jacobian matrix.
+
+While the attribute :attr:`assets.ArticulationData.joint_pos` provides the joint positions,
+we only want the joint positions of the robot's arm, and not the gripper. Similarly, while
+the attribute :attr:`assets.ArticulationData.body_state_w` provides the state of all the
+robot's bodies, we only want the state of the robot's end-effector. Thus, we need to
+index into these arrays to obtain the desired quantities.
+
+For this, the articulation class provides the methods :meth:`~assets.Articulation.find_joints`
+and :meth:`~assets.Articulation.find_bodies`. These methods take in the names of the joints
+and bodies and return their corresponding indices.
+
+While you may directly use these methods to obtain the indices, we recommend using the
+:attr:`~managers.SceneEntityCfg` class to resolve the indices. This class is used in various
+places in the APIs to extract certain information from a scene entity. Internally, it
+calls the above methods to obtain the indices. However, it also performs some additional
+checks to ensure that the provided names are valid. Thus, it is a safer option to use
+this class.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+   :language: python
+   :lines: 122-137
+   :linenos:
+   :lineno-start: 122
+
+
+Computing robot command
+-----------------------
+
+The IK controller separates the operation of setting the desired command and
+computing the desired joint positions. This is done to allow for the user to
+run the IK controller at a different frequency than the robot's control frequency.
+
+The :meth:`~controllers.DifferentialIKController.set_command` method takes in
+the desired end-effector pose as a single batched array. The pose is specified in
+the robot's base frame.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+   :language: python
+   :lines: 156-158
+   :linenos:
+   :lineno-start: 156
+
+We can then compute the desired joint positions using the
+:meth:`~controllers.DifferentialIKController.compute` method.
+The method takes in the current end-effector pose (in base frame), Jacobian, and
+current joint positions. We read the Jacobian matrix from the robot's data, which uses
+its value computed from the physics engine.
+
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+   :language: python
+   :lines: 162-172
+   :linenos:
+   :lineno-start: 162
+
+The computed joint position targets can then be applied on the robot, as done in the
+previous tutorials.
+
+.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_diff_ik.py
+   :language: python
+   :lines: 174-176
+   :linenos:
+   :lineno-start: 174
+
+
+The Code Execution
+~~~~~~~~~~~~~~~~~~
+
+Now that we have gone through the code, let's run the script and see the result:
+
+.. code-block:: bash
+
+   ./orbit.sh -p source/standalone/tutorials/05_controllers/run_diff_ik.py --robot franka_panda --num_envs 128
+
+The script will start a simulation with 128 robots. The robots will be controlled using the IK controller.
+The current and desired end-effector poses should be displayed using frame markers. When the robot reaches
+the desired pose, the command should cycle through to the next pose specified in the script.
+
+To stop the simulation, you can either close the window, or press the ``STOP`` button in the UI, or
+press ``Ctrl+C`` in the terminal.

source/extensions/omni.isaac.orbit/config/extension.toml

 [package]
 
 # Note: Semantic Versioning is used: https://semver.org/
-version = "0.10.7"
+version = "0.10.8"
 
 # Description
 title = "ORBIT framework for Robot Learning"

source/extensions/omni.isaac.orbit/docs/CHANGELOG.rst

 Changelog
 ---------
 
+0.10.8 (2023-12-20)
+~~~~~~~~~~~~~~~~~~~
+
+Fixed
+^^^^^
+
+* Fixed the :class:`omni.isaac.orbit.envs.mdp.actions.DifferentialInverseKinematicsAction` class
+  to account for the offset pose of the end-effector.
+
+
 0.10.7 (2023-12-19)
 ~~~~~~~~~~~~~~~~~~~
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/assets/config/franka.py

@@ -7,7 +7,8 @@
 
 The following configurations are available:
 
-* :obj:`FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG`: Franka Emika Panda robot with Panda hand
+* :obj:`FRANKA_PANDA_CFG`: Franka Emika Panda robot with Panda hand
+* :obj:`FRANKA_PANDA_HIGH_PD_CFG`: Franka Emika Panda robot with Panda hand with stiffer PD control
 
 Reference: https://github.com/frankaemika/franka_ros
 """
@@ -22,7 +23,7 @@ from ..articulation import ArticulationCfg
 # Configuration
 ##
 
-FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
+FRANKA_PANDA_CFG = ArticulationCfg(
     spawn=sim_utils.UsdFileCfg(
         usd_path=f"{ISAAC_ORBIT_NUCLEUS_DIR}/Robots/FrankaEmika/panda_instanceable.usd",
         activate_contact_sensors=False,
@@ -51,14 +52,14 @@ FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
         "panda_shoulder": ImplicitActuatorCfg(
             joint_names_expr=["panda_joint[1-4]"],
             effort_limit=87.0,
-            velocity_limit=100.0,
+            velocity_limit=2.175,
             stiffness=80.0,
             damping=4.0,
         ),
         "panda_forearm": ImplicitActuatorCfg(
             joint_names_expr=["panda_joint[5-7]"],
-            effort_limit=87.0,
-            velocity_limit=100.0,
+            effort_limit=12.0,
+            velocity_limit=2.61,
             stiffness=80.0,
             damping=4.0,
         ),
@@ -72,4 +73,16 @@ FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG = ArticulationCfg(
     },
     soft_joint_pos_limit_factor=1.0,
 )
-"""Configuration of Franka Emika Panda robot with Panda hand."""
+"""Configuration of Franka Emika Panda robot."""
+
+
+FRANKA_PANDA_HIGH_PD_CFG = FRANKA_PANDA_CFG.copy()
+FRANKA_PANDA_HIGH_PD_CFG.spawn.rigid_props.disable_gravity = True
+FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].stiffness = 400.0
+FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_shoulder"].damping = 80.0
+FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].stiffness = 400.0
+FRANKA_PANDA_HIGH_PD_CFG.actuators["panda_forearm"].damping = 80.0
+"""Configuration of Franka Emika Panda robot with stiffer PD control.
+
+This configuration is useful for task-space control using differential IK.
+"""

source/extensions/omni.isaac.orbit/omni/isaac/orbit/controllers/differential_ik_cfg.py

@@ -49,7 +49,7 @@ class DifferentialIKControllerCfg:
     - Jacobian transpose ("trans"):
         - "k_val": Scaling of computed delta-joint positions (default: 1.0).
     - Damped Moore-Penrose pseudo-inverse ("dls"):
-        - "lambda_val": Damping coefficient (default: 0.1).
+        - "lambda_val": Damping coefficient (default: 0.01).
     """
 
     def __post_init__(self):
@@ -63,7 +63,7 @@ class DifferentialIKControllerCfg:
             "pinv": {"k_val": 1.0},
             "svd": {"k_val": 1.0, "min_singular_value": 1e-5},
             "trans": {"k_val": 1.0},
-            "dls": {"lambda_val": 0.1},
+            "dls": {"lambda_val": 0.01},
         }
         # update parameters for IK-method if not provided
         ik_params = default_ik_params[self.ik_method].copy()

source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/actions/actions_cfg.py

@@ -157,12 +157,29 @@ class DifferentialInverseKinematicsActionCfg(ActionTermCfg):
     See :class:`DifferentialInverseKinematicsAction` for more details.
     """
 
+    @configclass
+    class OffsetCfg:
+        """The offset pose from parent frame to child frame.
+
+        On many robots, end-effector frames are fictitious frames that do not have a corresponding
+        rigid body. In such cases, it is easier to define this transform w.r.t. their parent rigid body.
+        For instance, for the Franka Emika arm, the end-effector is defined at an offset to the the
+        "panda_hand" frame.
+        """
+
+        pos: tuple[float, float, float] = (0.0, 0.0, 0.0)
+        """Translation w.r.t. the parent frame. Defaults to (0.0, 0.0, 0.0)."""
+        rot: tuple[float, float, float, float] = (1.0, 0.0, 0.0, 0.0)
+        """Quaternion rotation ``(w, x, y, z)`` w.r.t. the parent frame. Defaults to (1.0, 0.0, 0.0, 0.0)."""
+
     class_type: type[ActionTerm] = task_space_actions.DifferentialInverseKinematicsAction
 
     joint_names: list[str] = MISSING
     """List of joint names or regex expressions that the action will be mapped to."""
     body_name: str = MISSING
     """Name of the body or frame for which IK is performed."""
+    body_offset: OffsetCfg | None = None
+    """Offset of target frame w.r.t. to the body frame. Defaults to None, in which case no offset is applied."""
     scale: float | tuple[float, ...] = 1.0
     """Scale factor for the action. Defaults to 1.0."""
     controller: DifferentialIKControllerCfg = MISSING

source/extensions/omni.isaac.orbit/omni/isaac/orbit/envs/mdp/actions/task_space_actions.py

@@ -10,10 +10,10 @@ from typing import TYPE_CHECKING
 
 import carb
 
+import omni.isaac.orbit.utils.math as math_utils
 from omni.isaac.orbit.assets.articulation import Articulation
 from omni.isaac.orbit.controllers.differential_ik import DifferentialIKController
 from omni.isaac.orbit.managers.action_manager import ActionTerm
-from omni.isaac.orbit.utils.math import subtract_frame_transforms
 
 if TYPE_CHECKING:
     from omni.isaac.orbit.envs import BaseEnv
@@ -79,7 +79,9 @@ class DifferentialInverseKinematicsAction(ActionTerm):
             self._joint_ids = slice(None)
 
         # create the differential IK controller
-        self._controller = DifferentialIKController(cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device)
+        self._ik_controller = DifferentialIKController(
+            cfg=self.cfg.controller, num_envs=self.num_envs, device=self.device
+        )
 
         # create tensors for raw and processed actions
         self._raw_actions = torch.zeros(self.num_envs, self.action_dim, device=self.device)
@@ -89,13 +91,20 @@ class DifferentialInverseKinematicsAction(ActionTerm):
         self._scale = torch.zeros((self.num_envs, self.action_dim), device=self.device)
         self._scale[:] = torch.tensor(self.cfg.scale, device=self.device)
 
+        # convert the fixed offsets to torch tensors of batched shape
+        if self.cfg.body_offset is not None:
+            self._offset_pos = torch.tensor(self.cfg.body_offset.pos, device=self.device).repeat(self.num_envs, 1)
+            self._offset_rot = torch.tensor(self.cfg.body_offset.rot, device=self.device).repeat(self.num_envs, 1)
+        else:
+            self._offset_pos, self._offset_rot = None, None
+
     """
     Properties.
     """
 
     @property
     def action_dim(self) -> int:
-        return self._controller.action_dim
+        return self._ik_controller.action_dim
 
     @property
     def raw_actions(self) -> torch.Tensor:
@@ -114,17 +123,20 @@ class DifferentialInverseKinematicsAction(ActionTerm):
         self._raw_actions[:] = actions
         self._processed_actions[:] = self.raw_actions * self._scale
         # obtain quantities from simulation
-        ee_pos_curr, ee_quat_curr = self._compute_body_pose()
+        ee_pos_curr, ee_quat_curr = self._compute_frame_pose()
         # set command into controller
         self._ik_controller.set_command(self._processed_actions, ee_pos_curr, ee_quat_curr)
 
     def apply_actions(self):
         # obtain quantities from simulation
-        jacobian = self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._joint_ids]
-        ee_pos_curr, ee_quat_curr = self._compute_body_pose()
+        ee_pos_curr, ee_quat_curr = self._compute_frame_pose()
         joint_pos = self._asset.data.joint_pos[:, self._joint_ids]
         # compute the delta in joint-space
-        joint_pos_des = self._ik_controller.compute(ee_pos_curr, ee_quat_curr, jacobian, joint_pos)
+        if ee_quat_curr.norm() != 0:
+            jacobian = self._compute_frame_jacobian()
+            joint_pos_des = self._ik_controller.compute(ee_pos_curr, ee_quat_curr, jacobian, joint_pos)
+        else:
+            joint_pos_des = joint_pos.clone()
         # set the joint position command
         self._asset.set_joint_position_target(joint_pos_des, self._joint_ids)
 
@@ -132,8 +144,8 @@ class DifferentialInverseKinematicsAction(ActionTerm):
     Helper functions.
     """
 
-    def _compute_body_pose(self) -> tuple[torch.Tensor, torch.Tensor]:
-        """Computes the pose of the body in the root frame.
+    def _compute_frame_pose(self) -> tuple[torch.Tensor, torch.Tensor]:
+        """Computes the pose of the target frame in the root frame.
 
         Returns:
             A tuple of the body's position and orientation in the root frame.
@@ -142,4 +154,35 @@ class DifferentialInverseKinematicsAction(ActionTerm):
         ee_pose_w = self._asset.data.body_state_w[:, self._body_idx, :7]
         root_pose_w = self._asset.data.root_state_w[:, :7]
         # compute the pose of the body in the root frame
-        return subtract_frame_transforms(root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7])
+        ee_pose_b, ee_quat_b = math_utils.subtract_frame_transforms(
+            root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]
+        )
+        # account for the offset
+        if self.cfg.body_offset is not None:
+            ee_pose_b, ee_quat_b = math_utils.combine_frame_transforms(
+                ee_pose_b, ee_quat_b, self._offset_pos, self._offset_rot
+            )
+
+        return ee_pose_b, ee_quat_b
+
+    def _compute_frame_jacobian(self):
+        """Computes the geometric Jacobian of the target frame in the root frame.
+
+        This function accounts for the target frame offset and applies the necessary transformations to obtain
+        the right Jacobian from the parent body Jacobian.
+        """
+        # read the parent jacobian
+        jacobian = self._asset.root_physx_view.get_jacobians()[:, self._jacobi_body_idx, :, self._joint_ids]
+        # account for the offset
+        if self.cfg.body_offset is not None:
+            # Modify the jacobian to account for the offset
+            # -- translational part
+            # v_link = v_ee + w_ee x r_link_ee = v_J_ee * q + w_J_ee * q x r_link_ee
+            #        = (v_J_ee + w_J_ee x r_link_ee ) * q
+            #        = (v_J_ee - r_link_ee_[x] @ w_J_ee) * q
+            jacobian[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), jacobian[:, 3:, :])
+            # -- rotational part
+            # w_link = R_link_ee @ w_ee
+            jacobian[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), jacobian[:, 3:, :])
+
+        return jacobian

source/extensions/omni.isaac.orbit/omni/isaac/orbit/managers/scene_entity_cfg.py

@@ -35,13 +35,13 @@ class SceneEntityCfg:
     The names can be either joint names or a regular expression matching the joint names.
 
     These are converted to joint indices on initialization of the manager and passed to the term
-    function as a list of joint indices under :attr:`dof_ids`.
+    function as a list of joint indices under :attr:`joint_ids`.
     """
 
     joint_ids: list[int] | None = None
     """The indices of the joints from the asset required by the term. Defaults to None.
 
-    If ``joint_names`` is specified, this is filled in automatically on initialization of the
+    If :attr:`joint_names` is specified, this is filled in automatically on initialization of the
     manager.
     """
 
@@ -57,7 +57,7 @@ class SceneEntityCfg:
     body_ids: list[int] | None = None
     """The indices of the bodies from the asset required by the term. Defaults to None.
 
-    If ``body_names`` is specified, this is filled in automatically on initialization of the
+    If :attr:`body_names` is specified, this is filled in automatically on initialization of the
     manager.
     """
 

source/extensions/omni.isaac.orbit/omni/isaac/orbit/utils/math.py

@@ -659,6 +659,37 @@ def quat_error_magnitude(q1: torch.Tensor, q2: torch.Tensor) -> torch.Tensor:
     return torch.norm(axis_angle_from_quat(quat_diff), dim=1)
 
 
+@torch.jit.script
+def skew_symmetric_matrix(vec: torch.Tensor) -> torch.Tensor:
+    """Computes the skew-symmetric matrix of a vector.
+
+    Args:
+        vec: The input vector. Shape is (3,) or (N, 3).
+
+    Returns:
+        The skew-symmetric matrix. Shape is (1, 3, 3) or (N, 3, 3).
+
+    Raises:
+        ValueError: If input tensor is not of shape (..., 3).
+    """
+    # check input is correct
+    if vec.shape[-1] != 3:
+        raise ValueError(f"Expected input vector shape mismatch: {vec.shape} != (..., 3).")
+    # unsqueeze the last dimension
+    if vec.ndim == 1:
+        vec = vec.unsqueeze(0)
+    # create a skew-symmetric matrix
+    skew_sym_mat = torch.zeros(vec.shape[0], 3, 3, device=vec.device, dtype=vec.dtype)
+    skew_sym_mat[:, 0, 1] = -vec[:, 2]
+    skew_sym_mat[:, 0, 2] = vec[:, 1]
+    skew_sym_mat[:, 1, 2] = -vec[:, 0]
+    skew_sym_mat[:, 1, 0] = vec[:, 2]
+    skew_sym_mat[:, 2, 0] = -vec[:, 1]
+    skew_sym_mat[:, 2, 1] = vec[:, 0]
+
+    return skew_sym_mat
+
+
 """
 Transformations
 """

source/extensions/omni.isaac.orbit/test/assets/test_articulation.py

@@ -30,7 +30,7 @@ import omni.isaac.orbit.sim as sim_utils
 import omni.isaac.orbit.utils.string as string_utils
 from omni.isaac.orbit.actuators import ImplicitActuatorCfg
 from omni.isaac.orbit.assets import Articulation, ArticulationCfg
-from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
+from omni.isaac.orbit.assets.config import ANYMAL_C_CFG, FRANKA_PANDA_CFG
 from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
 
 
@@ -120,7 +120,7 @@ class TestArticulation(unittest.TestCase):
     def test_initialization_fixed_base(self):
         """Test initialization for fixed base."""
         # Create articulation
-        robot = Articulation(cfg=FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/Robot"))
+        robot = Articulation(cfg=FRANKA_PANDA_CFG.replace(prim_path="/World/Robot"))
 
         # Check that boundedness of articulation is correct
         self.assertEqual(ctypes.c_long.from_address(id(robot)).value, 1)

source/extensions/omni.isaac.orbit/test/controllers/test_differential_ik.py

@@ -25,7 +25,7 @@ from omni.isaac.cloner import GridCloner
 
 import omni.isaac.orbit.sim as sim_utils
 from omni.isaac.orbit.assets import Articulation
-from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG, UR10_CFG
+from omni.isaac.orbit.assets.config import FRANKA_PANDA_HIGH_PD_CFG, UR10_CFG
 from omni.isaac.orbit.controllers import DifferentialIKController, DifferentialIKControllerCfg
 from omni.isaac.orbit.utils.math import compute_pose_error, subtract_frame_transforms
 
@@ -42,6 +42,8 @@ class TestDifferentialIKController(unittest.TestCase):
         # Load kit helper
         sim_cfg = sim_utils.SimulationCfg(dt=0.01)
         self.sim = sim_utils.SimulationContext(sim_cfg)
+        # TODO: Remove this once we have a better way to handle this.
+        self.sim._app_control_on_stop_handle = None
 
         # Create a ground plane
         cfg = sim_utils.GroundPlaneCfg()
@@ -83,8 +85,7 @@ class TestDifferentialIKController(unittest.TestCase):
     def test_franka_ik_pose_abs(self):
         """Test IK controller for Franka arm with Franka hand."""
         # Create robot instance
-        robot_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/envs/env_.*/Robot")
-        robot_cfg.spawn.rigid_props.disable_gravity = True
+        robot_cfg = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="/World/envs/env_.*/Robot")
         robot = Articulation(cfg=robot_cfg)
 
         # Create IK controller

source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/__init__.py

@@ -6,16 +6,21 @@
 
 import gymnasium as gym
 
-from . import agents, env_cfg
+from . import agents, ik_abs_env_cfg, ik_rel_env_cfg, joint_pos_env_cfg
 
 ##
 # Register Gym environments.
 ##
+
+##
+# Joint Position Control
+##
+
 gym.register(
     id="Isaac-Lift-Cube-Franka-v0",
     entry_point="omni.isaac.orbit.envs:RLTaskEnv",
     kwargs={
-        "env_cfg_entry_point": env_cfg.FrankaCubeLiftEnvCfg,
+        "env_cfg_entry_point": joint_pos_env_cfg.FrankaCubeLiftEnvCfg,
         "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
     },
     disable_env_checker=True,
@@ -25,7 +30,55 @@ gym.register(
     id="Isaac-Lift-Cube-Franka-Play-v0",
     entry_point="omni.isaac.orbit.envs:RLTaskEnv",
     kwargs={
-        "env_cfg_entry_point": env_cfg.FrankaCubeLiftEnvCfg_PLAY,
+        "env_cfg_entry_point": joint_pos_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
+        "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
+    },
+    disable_env_checker=True,
+)
+
+##
+# Inverse Kinematics - Absolute Pose Control
+##
+
+gym.register(
+    id="Isaac-Lift-Cube-Franka-IK-Abs-v0",
+    entry_point="omni.isaac.orbit.envs:RLTaskEnv",
+    kwargs={
+        "env_cfg_entry_point": ik_abs_env_cfg.FrankaCubeLiftEnvCfg,
+        "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
+    },
+    disable_env_checker=True,
+)
+
+gym.register(
+    id="Isaac-Lift-Cube-Franka-IK-Abs-Play-v0",
+    entry_point="omni.isaac.orbit.envs:RLTaskEnv",
+    kwargs={
+        "env_cfg_entry_point": ik_abs_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
+        "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
+    },
+    disable_env_checker=True,
+)
+
+##
+# Inverse Kinematics - Relative Pose Control
+##
+
+gym.register(
+    id="Isaac-Lift-Cube-Franka-IK-Rel-v0",
+    entry_point="omni.isaac.orbit.envs:RLTaskEnv",
+    kwargs={
+        "env_cfg_entry_point": ik_rel_env_cfg.FrankaCubeLiftEnvCfg,
+        "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
+    },
+    disable_env_checker=True,
+)
+
+gym.register(
+    id="Isaac-Lift-Cube-Franka-IK-Rel-Play-v0",
+    entry_point="omni.isaac.orbit.envs:RLTaskEnv",
+    kwargs={
+        "env_cfg_entry_point": ik_rel_env_cfg.FrankaCubeLiftEnvCfg_PLAY,
         "rsl_rl_cfg_entry_point": agents.rsl_rl_cfg.LiftCubePPORunnerCfg,
     },
     disable_env_checker=True,

source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/ik_abs_env_cfg.py

+# Copyright (c) 2022-2023, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from omni.isaac.orbit.controllers.differential_ik_cfg import DifferentialIKControllerCfg
+from omni.isaac.orbit.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
+from omni.isaac.orbit.utils import configclass
+
+from . import joint_pos_env_cfg
+
+##
+# Pre-defined configs
+##
+# isort: off
+from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_HIGH_PD_CFG
+
+
+@configclass
+class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg):
+    def __post_init__(self):
+        # post init of parent
+        super().__post_init__()
+
+        # Set Franka as robot
+        # We switch here to a stiffer PD controller for IK tracking to be better.
+        self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+
+        # Set actions for the specific robot type (franka)
+        self.actions.body_joint_pos = DifferentialInverseKinematicsActionCfg(
+            asset_name="robot",
+            joint_names=["panda_joint.*"],
+            body_name="panda_hand",
+            controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=False, ik_method="dls"),
+            body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
+        )
+
+
+@configclass
+class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg):
+    def __post_init__(self):
+        # post init of parent
+        super().__post_init__()
+        # make a smaller scene for play
+        self.scene.num_envs = 50
+        self.scene.env_spacing = 2.5
+        # disable randomization for play
+        self.observations.policy.enable_corruption = False

source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/ik_rel_env_cfg.py

+# Copyright (c) 2022-2023, The ORBIT Project Developers.
+# All rights reserved.
+#
+# SPDX-License-Identifier: BSD-3-Clause
+
+from omni.isaac.orbit.controllers.differential_ik_cfg import DifferentialIKControllerCfg
+from omni.isaac.orbit.envs.mdp.actions.actions_cfg import DifferentialInverseKinematicsActionCfg
+from omni.isaac.orbit.utils import configclass
+
+from . import joint_pos_env_cfg
+
+##
+# Pre-defined configs
+##
+# isort: off
+from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_HIGH_PD_CFG
+
+
+@configclass
+class FrankaCubeLiftEnvCfg(joint_pos_env_cfg.FrankaCubeLiftEnvCfg):
+    def __post_init__(self):
+        # post init of parent
+        super().__post_init__()
+
+        # Set Franka as robot
+        # We switch here to a stiffer PD controller for IK tracking to be better.
+        self.scene.robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+
+        # Set actions for the specific robot type (franka)
+        self.actions.body_joint_pos = DifferentialInverseKinematicsActionCfg(
+            asset_name="robot",
+            joint_names=["panda_joint.*"],
+            body_name="panda_hand",
+            controller=DifferentialIKControllerCfg(command_type="pose", use_relative_mode=True, ik_method="dls"),
+            scale=0.5,
+            body_offset=DifferentialInverseKinematicsActionCfg.OffsetCfg(pos=[0.0, 0.0, 0.107]),
+        )
+
+
+@configclass
+class FrankaCubeLiftEnvCfg_PLAY(FrankaCubeLiftEnvCfg):
+    def __post_init__(self):
+        # post init of parent
+        super().__post_init__()
+        # make a smaller scene for play
+        self.scene.num_envs = 50
+        self.scene.env_spacing = 2.5
+        # disable randomization for play
+        self.observations.policy.enable_corruption = False

source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/env_cfg.py → source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/lift/config/franka/joint_pos_env_cfg.py

@@ -18,7 +18,7 @@ from omni.isaac.orbit_tasks.manipulation.lift.lift_env_cfg import LiftEnvCfg
 # Pre-defined configs
 ##
 # isort: off
-from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
+from omni.isaac.orbit.assets.config.franka import FRANKA_PANDA_CFG
 from omni.isaac.orbit.markers.config import FRAME_MARKER_CFG
 
 
@@ -29,7 +29,7 @@ class FrankaCubeLiftEnvCfg(LiftEnvCfg):
         super().__post_init__()
 
         # Set Franka as robot
-        self.scene.robot = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+        self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
 
         # Set actions for the specific robot type (franka)
         self.actions.body_joint_pos = mdp.JointPositionActionCfg(
@@ -38,7 +38,7 @@ class FrankaCubeLiftEnvCfg(LiftEnvCfg):
         self.actions.finger_joint_pos = mdp.BinaryJointPositionActionCfg(
             asset_name="robot",
             joint_names=["panda_finger.*"],
-            open_command_expr={"panda_finger_.*": 0.3},
+            open_command_expr={"panda_finger_.*": 0.04},
             close_command_expr={"panda_finger_.*": 0.0},
         )
         # Set the body name for the end effector

source/extensions/omni.isaac.orbit_tasks/omni/isaac/orbit_tasks/manipulation/reach/config/franka/env_cfg.py

@@ -16,7 +16,7 @@ from omni.isaac.orbit_tasks.manipulation.reach.reach_env_cfg import ReachEnvCfg
 # Pre-defined configs
 ##
 # isort: off
-from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG
+from omni.isaac.orbit.assets.config import FRANKA_PANDA_CFG
 
 
 ##
@@ -31,7 +31,7 @@ class FrankaReachEnvCfg(ReachEnvCfg):
         super().__post_init__()
 
         # switch robot to franka
-        self.scene.robot = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
+        self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot")
         # override rewards
         self.rewards.end_effector_position_tracking.params["asset_cfg"].body_names = ["panda_hand"]
         self.rewards.end_effector_orientation_tracking.params["asset_cfg"].body_names = ["panda_hand"]

source/standalone/demos/arms.py

@@ -41,7 +41,7 @@ import omni.isaac.core.utils.prims as prim_utils
 
 import omni.isaac.orbit.sim as sim_utils
 from omni.isaac.orbit.assets import Articulation
-from omni.isaac.orbit.assets.config import FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG, UR10_CFG
+from omni.isaac.orbit.assets.config import FRANKA_PANDA_CFG, UR10_CFG
 from omni.isaac.orbit.utils.assets import ISAAC_NUCLEUS_DIR
 
 
@@ -64,7 +64,7 @@ def design_scene() -> tuple[dict, list[list[float]]]:
     cfg = sim_utils.UsdFileCfg(usd_path=f"{ISAAC_NUCLEUS_DIR}/Props/Mounts/SeattleLabTable/table_instanceable.usd")
     cfg.func("/World/Origin1/Table", cfg, translation=(0.55, 0.0, 1.05))
     # -- Robot
-    franka_arm_cfg = FRANKA_PANDA_ARM_WITH_PANDA_HAND_CFG.replace(prim_path="/World/Origin1/Robot")
+    franka_arm_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/Origin1/Robot")
     franka_arm_cfg.init_state.pos = (0.0, 0.0, 1.05)
     robot_franka = Articulation(cfg=franka_arm_cfg)
 

source/standalone/environments/teleoperation/teleop_se3_agent.py

@@ -66,9 +66,8 @@ def main():
     # parse configuration
     env_cfg = parse_env_cfg(args_cli.task, use_gpu=not args_cli.cpu, num_envs=args_cli.num_envs)
     # modify configuration
-    env_cfg.control.control_type = "inverse_kinematics"
-    env_cfg.control.inverse_kinematics.command_type = "pose_rel"
-    env_cfg.terminations.episode_timeout = False
+    env_cfg.terminations.time_out = None
+
     # create environment
     env = gym.make(args_cli.task, cfg=env_cfg)
     # check environment name (for reach , we don't allow the gripper)
@@ -107,12 +106,13 @@ def main():
         with torch.inference_mode():
             # get keyboard command
             delta_pose, gripper_command = teleop_interface.advance()
+            delta_pose = delta_pose.astype("float32")
             # convert to torch
-            delta_pose = torch.tensor(delta_pose, dtype=torch.float, device=env.device).repeat(env.num_envs, 1)
+            delta_pose = torch.tensor(delta_pose, device=env.unwrapped.device).repeat(env.unwrapped.num_envs, 1)
             # pre-process actions
             actions = pre_process_actions(delta_pose, gripper_command)
             # apply actions
-            _, _, _, _ = env.step(actions)
+            env.step(actions)
 
     # close the simulator
     env.close()

source/standalone/workflows/robomimic/collect_demonstrations.py

@@ -135,7 +135,8 @@ def main():
             # -- actions
             collector_interface.add("actions", actions)
             # perform action on environment
-            obs_dict, rewards, dones, info = env.step(actions)
+            obs_dict, rewards, terminated, truncated, info = env.step(actions)
+            dones = terminated | truncated
             # check that simulation is stopped or not
             if env.unwrapped.sim.is_stopped():
                 break