diff --git a/docs/source/_static/tutorials/tutorial_operational_space_controller.jpg b/docs/source/_static/tutorials/tutorial_operational_space_controller.jpg new file mode 100644 index 0000000000..e356eee4ff Binary files /dev/null and b/docs/source/_static/tutorials/tutorial_operational_space_controller.jpg differ diff --git a/docs/source/api/lab/omni.isaac.lab.controllers.rst b/docs/source/api/lab/omni.isaac.lab.controllers.rst index d56efdd05a..67136f0a3a 100644 --- a/docs/source/api/lab/omni.isaac.lab.controllers.rst +++ b/docs/source/api/lab/omni.isaac.lab.controllers.rst @@ -9,6 +9,8 @@ DifferentialIKController DifferentialIKControllerCfg + OperationalSpaceController + OperationalSpaceControllerCfg Differential Inverse Kinematics ------------------------------- @@ -23,3 +25,17 @@ Differential Inverse Kinematics :inherited-members: :show-inheritance: :exclude-members: __init__, class_type + +Operational Space controllers +----------------------------- + +.. autoclass:: OperationalSpaceController + :members: + :inherited-members: + :show-inheritance: + +.. autoclass:: OperationalSpaceControllerCfg + :members: + :inherited-members: + :show-inheritance: + :exclude-members: __init__, class_type diff --git a/docs/source/overview/environments.rst b/docs/source/overview/environments.rst index 5d08c22b6e..95ace6c61a 100644 --- a/docs/source/overview/environments.rst +++ b/docs/source/overview/environments.rst @@ -91,7 +91,7 @@ Manipulation Environments based on fixed-arm manipulation tasks. For many of these tasks, we include configurations with different arm action spaces. For example, -for the reach environment: +for the lift-cube environment: * |lift-cube-link|: Franka arm with joint position control * |lift-cube-ik-abs-link|: Franka arm with absolute IK control @@ -421,6 +421,10 @@ Comprehensive List of Environments - - Manager Based - + * - Isaac-Reach-Franka-OSC-v0 + - Isaac-Reach-Franka-OSC-Play-v0 + - Manager Based + - **rsl_rl** (PPO) * - Isaac-Reach-Franka-v0 - Isaac-Reach-Franka-Play-v0 - Manager Based diff --git a/docs/source/tutorials/05_controllers/run_osc.rst b/docs/source/tutorials/05_controllers/run_osc.rst new file mode 100644 index 0000000000..661056b1a8 --- /dev/null +++ b/docs/source/tutorials/05_controllers/run_osc.rst @@ -0,0 +1,178 @@ +Using an operational space controller +===================================== + +.. currentmodule:: omni.isaac.lab + +Sometimes, controlling the end-effector pose of the robot using a differential IK controller is not sufficient. +For example, we might want to enforce a very specific pose tracking error dynamics in the task space, actuate the robot +with joint effort/torque commands, or apply a contact force at a specific direction while controlling the motion of +the other directions (e.g., washing the surface of the table with a cloth). In such tasks, we can use an +operational space controller (OSC). + +.. rubric:: References for the operational space control: + +1. O Khatib. A unified approach for motion and force control of robot manipulators: + The operational space formulation. IEEE Journal of Robotics and Automation, 3(1):43–53, 1987. URL http://dx.doi.org/10.1109/JRA.1987.1087068. + +2. Robot Dynamics Lecture Notes by Marco Hutter (ETH Zurich). URL https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + +In this tutorial, we will learn how to use an OSC to control the robot. +We will use the :class:`controllers.OperationalSpaceController` class to apply a constant force perpendicular to a +tilted wall surface while tracking a desired end-effector pose in all the other directions. + +The Code +~~~~~~~~ + +The tutorial corresponds to the ``run_osc.py`` script in the +``source/standalone/tutorials/05_controllers`` directory. + + +.. dropdown:: Code for run_osc.py + :icon: code + + .. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :linenos: + + +Creating an Operational Space Controller +---------------------------------------- + +The :class:`~controllers.OperationalSpaceController` class computes the joint +efforts/torques for a robot to do simultaneous motion and force control in task space. + +The reference frame of this task space could be an arbitrary coordinate frame in Euclidean space. By default, +it is the robot's base frame. However, in certain cases, it could be easier to define target coordinates w.r.t. a +different frame. In such cases, the pose of this task reference frame, w.r.t. to the robot's base frame, should be +provided in the ``set_command`` method's ``current_task_frame_pose_b`` argument. For example, in this tutorial, it +makes sense to define the target commands w.r.t. a frame that is parallel to the wall surface, as the force control +direction would be then only nonzero in the z-axis of this frame. The target pose, which is set to have the same +orientation as the wall surface, is such a candidate and is used as the task frame in this tutorial. Therefore, all +the arguments to the :class:`~controllers.OperationalSpaceControllerCfg` should be set with this task reference frame +in mind. + +For the motion control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, +``target_types: "pose_abs"``) or relative the the end-effector's current pose (i.e., ``target_types: "pose_rel"``). +For the force control, the task space targets could be given as absolute (i.e., defined w.r.t. the robot base, +``target_types: "force_abs"``). If it is desired to apply pose and force control simultaneously, the ``target_types`` +should be a list such as ``["pose_abs", "wrench_abs"]`` or ``["pose_rel", "wrench_abs"]``. + +The axes that the motion and force control will be applied can be specified using the ``motion_control_axes_task`` and +``force_control_axes_task`` arguments, respectively. These lists should consist of 0/1 for all six axes (position and +rotation) and be complementary to each other (e.g., for the x-axis, if the ``motion_control_axes_task`` is ``0``, the +``force_control_axes_task`` should be ``1``). + +For the motion control axes, desired stiffness, and damping ratio values can be specified using the +``motion_control_stiffness`` and ``motion_damping_ratio_task`` arguments, which can be a scalar (same value for all +axes) or a list of six scalars, one value corresponding to each axis. If desired, the stiffness and damping ratio +values could be a command parameter (e.g., to learn the values using RL or change them on the go). For this, +``impedance_mode`` should be either ``"variable_kp"`` to include the stiffness values within the command or +``"variable"`` to include both the stiffness and damping ratio values. In these cases, ``motion_stiffness_limits_task`` +and ``motion_damping_limits_task`` should be set as well, which puts bounds on the stiffness and damping ratio values. + +For contact force control, it is possible to apply an open-loop force control by not setting the +``contact_wrench_stiffness_task``, or apply a closed-loop force control (with the feed-forward term) by setting +the desired stiffness values using the ``contact_wrench_stiffness_task`` argument, which can be a scalar or a list +of six scalars. Please note that, currently, only the linear part of the contact wrench (hence the first three +elements of the ``contact_wrench_stiffness_task``) is considered in the closed-loop control, as the rotational part +cannot be measured with the contact sensors. + +For the motion control, ``inertial_dynamics_decoupling`` should be set to ``True`` to use the robot's inertia matrix +to decouple the desired accelerations in the task space. This is important for the motion control to be accurate, +especially for rapid movements. This inertial decoupling accounts for the coupling between all the six motion axes. +If desired, the inertial coupling between the translational and rotational axes could be ignored by setting the +``partial_inertial_dynamics_decoupling`` to ``True``. + +If it is desired to include the gravity compensation in the operational space command, the ``gravity_compensation`` +should be set to ``True``. + +The included OSC implementation performs the computation in a batched format and uses PyTorch operations. + +In this tutorial, we will use ``"pose_abs"`` for controlling the motion in all axes except the z-axis and +``"wrench_abs"`` for controlling the force in the z-axis. Moreover, we will include the full inertia decoupling in +the motion control and not include the gravity compensation, as the gravity is disabled from the robot configuration. +Finally, we set the impedance mode to ``"variable_kp"`` to dynamically change the stiffness values +(``motion_damping_ratio_task`` is set to ``1``: the kd values adapt according to kp values to maintain a critically +damped response). + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # Create the OSC + :end-at: osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device) + +Updating the states of the robot +-------------------------------------------- + +The OSC implementation is a computation-only class. Thus, it expects the user to provide the necessary information +about the robot. This includes the robot's Jacobian matrix, mass/inertia matrix, end-effector pose, velocity, and +contact force, all in the root frame. Moreover, the user should provide gravity compensation vector, if desired. + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # Update robot states + :end-before: return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + + +Computing robot command +----------------------- + +The OSC separates the operation of setting the desired command and computing the desired joint positions. +To set the desired command, the user should provide command vector, which includes the target commands +(i.e., in the order they appear in the ``target_types`` argument of the OSC configuration), +and the desired stiffness and damping ratio values if the impedance_mode is set to ``"variable_kp"`` or ``"variable"``. +They should be all in the same coordinate frame as the task frame (e.g., indicated with ``_task`` subscript) and +concatanated together. + +In this tutorial, the desired wrench is already defined w.r.t. the task frame, and the desired pose is transformed +to the task frame as the following: + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # Convert the target commands to the task frame + :end-at: return command, task_frame_pose_b + +The OSC command is set with the command vector in the task frame, the end-effector pose in the base frame, and the +task (reference) frame pose in the base frame as the following. This information is needed, as the internal +computations are done in the base frame. + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # set the osc command + :end-at: osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b) + +The joint effort/torque values are computed using the provided robot states and the desired command as the following: + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # compute the joint commands + :end-at: ) + + +The computed joint effort/torque targets can then be applied on the robot. + +.. literalinclude:: ../../../../source/standalone/tutorials/05_controllers/run_osc.py + :language: python + :start-at: # apply actions + :end-at: robot.write_data_to_sim() + + +The Code Execution +~~~~~~~~~~~~~~~~~~ + +You can now run the script and see the result: + +.. code-block:: bash + + ./isaaclab.sh -p source/standalone/tutorials/05_controllers/run_osc.py --num_envs 128 + +The script will start a simulation with 128 robots. The robots will be controlled using the OSC. +The current and desired end-effector poses should be displayed using frame markers in addition to the red tilted wall. +You should see that the robot reaches the desired pose while applying a constant force perpendicular to the wall +surface. + +.. figure:: ../../_static/tutorials/tutorial_operational_space_controller.jpg + :align: center + :figwidth: 100% + :alt: result of run_osc.py + +To stop the simulation, you can either close the window or press ``Ctrl+C`` in the terminal. diff --git a/docs/source/tutorials/index.rst b/docs/source/tutorials/index.rst index d3d582a5c4..3e6a09a1e6 100644 --- a/docs/source/tutorials/index.rst +++ b/docs/source/tutorials/index.rst @@ -101,3 +101,4 @@ tutorials show you how to use motion generators to control the robots at the tas :titlesonly: 05_controllers/run_diff_ik + 05_controllers/run_osc diff --git a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst index 61206dcc8b..0dfdb83c2e 100644 --- a/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab/docs/CHANGELOG.rst @@ -1,7 +1,7 @@ Changelog --------- -0.29.2 (2024-12-15) +0.29.3 (2024-12-16) ~~~~~~~~~~~~~~~~~~~ Fixed @@ -10,6 +10,23 @@ 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) +~~~~~~~~~~~~~~~~~~~ + +Fixed +^^^^^ + +* Fixed errors within the calculations of :class:`omni.isaac.lab.controllers.OperationalSpaceController`. + +Added +^^^^^ + +* Added :class:`omni.isaac.lab.controllers.OperationalSpaceController` to API documentation. +* Added test cases for :class:`omni.isaac.lab.controllers.OperationalSpaceController`. +* Added a tutorial for :class:`omni.isaac.lab.controllers.OperationalSpaceController`. +* Added the implementation of :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction` class. + + 0.29.1 (2024-12-15) ~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/__init__.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/__init__.py index e97b0ac0ab..5c509e2067 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/__init__.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/__init__.py @@ -13,3 +13,5 @@ from .differential_ik import DifferentialIKController from .differential_ik_cfg import DifferentialIKControllerCfg +from .operational_space import OperationalSpaceController +from .operational_space_cfg import OperationalSpaceControllerCfg diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py index 5b57b14c5a..9f278248e2 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space.py @@ -3,92 +3,40 @@ # # SPDX-License-Identifier: BSD-3-Clause -import torch -from collections.abc import Sequence -from dataclasses import MISSING - -from omni.isaac.lab.utils import configclass -from omni.isaac.lab.utils.math import apply_delta_pose, compute_pose_error - - -@configclass -class OperationSpaceControllerCfg: - """Configuration for operation-space controller.""" - - command_types: Sequence[str] = MISSING - """Type of command. - - It has two sub-strings joined by underscore: - - type of command mode: "position", "pose", "force" - - type of command resolving: "abs" (absolute), "rel" (relative) - """ - - impedance_mode: str = MISSING - """Type of gains for motion control: "fixed", "variable", "variable_kp".""" - - uncouple_motion_wrench: bool = False - """Whether to decouple the wrench computation from task-space pose (motion) error.""" - - motion_control_axes: Sequence[int] = (1, 1, 1, 1, 1, 1) - """Motion direction to control. Mark as 0/1 for each axis.""" - force_control_axes: Sequence[int] = (0, 0, 0, 0, 0, 0) - """Force direction to control. Mark as 0/1 for each axis.""" - - inertial_compensation: bool = False - """Whether to perform inertial compensation for motion control (inverse dynamics).""" - - gravity_compensation: bool = False - """Whether to perform gravity compensation.""" - - stiffness: float | Sequence[float] = MISSING - """The positional gain for determining wrenches based on task-space pose error.""" - - damping_ratio: float | Sequence[float] | None = None - """The damping ratio is used in-conjunction with positional gain to compute wrenches - based on task-space velocity error. - - The following math operation is performed for computing velocity gains: - :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. - """ - - stiffness_limits: tuple[float, float] = (0, 300) - """Minimum and maximum values for positional gains. - - Note: Used only when :obj:`impedance_mode` is "variable" or "variable_kp". - """ - - damping_ratio_limits: tuple[float, float] = (0, 100) - """Minimum and maximum values for damping ratios used to compute velocity gains. +from __future__ import annotations - Note: Used only when :obj:`impedance_mode` is "variable". - """ +import torch +from typing import TYPE_CHECKING - force_stiffness: float | Sequence[float] = None - """The positional gain for determining wrenches for closed-loop force control. +from omni.isaac.lab.utils.math import ( + apply_delta_pose, + combine_frame_transforms, + compute_pose_error, + matrix_from_quat, + subtract_frame_transforms, +) - If obj:`None`, then open-loop control of desired forces is performed. - """ +if TYPE_CHECKING: + from .operational_space_cfg import OperationalSpaceControllerCfg - position_command_scale: tuple[float, float, float] = (1.0, 1.0, 1.0) - """Scaling of the position command received. Used only in relative mode.""" - rotation_command_scale: tuple[float, float, float] = (1.0, 1.0, 1.0) - """Scaling of the rotation command received. Used only in relative mode.""" - -class OperationSpaceController: - """Operation-space controller. +class OperationalSpaceController: + """Operational-space controller. Reference: - [1] https://ethz.ch/content/dam/ethz/special-interest/mavt/robotics-n-intelligent-systems/rsl-dam/documents/RobotDynamics2017/RD_HS2017script.pdf + + 1. `A unified approach for motion and force control of robot manipulators: The operational space formulation `_ + by Oussama Khatib (Stanford University) + 2. `Robot Dynamics Lecture Notes `_ + by Marco Hutter (ETH Zurich) """ - def __init__(self, cfg: OperationSpaceControllerCfg, num_robots: int, num_dof: int, device: str): - """Initialize operation-space controller. + def __init__(self, cfg: OperationalSpaceControllerCfg, num_envs: int, device: str): + """Initialize operational-space controller. Args: - cfg: The configuration for operation-space controller. - num_robots: The number of robots to control. - num_dof: The number of degrees of freedom of the robot. + cfg: The configuration for operational-space controller. + num_envs: The number of environments. device: The device to use for computations. Raises: @@ -96,79 +44,105 @@ def __init__(self, cfg: OperationSpaceControllerCfg, num_robots: int, num_dof: i """ # store inputs self.cfg = cfg - self.num_robots = num_robots - self.num_dof = num_dof + self.num_envs = num_envs self._device = device # resolve tasks-pace target dimensions self.target_list = list() - for command_type in self.cfg.command_types: - if "position" in command_type: - self.target_list.append(3) - elif command_type == "pose_rel": + for command_type in self.cfg.target_types: + if command_type == "pose_rel": self.target_list.append(6) elif command_type == "pose_abs": self.target_list.append(7) - elif command_type == "force_abs": + elif command_type == "wrench_abs": self.target_list.append(6) else: raise ValueError(f"Invalid control command: {command_type}.") self.target_dim = sum(self.target_list) # create buffers - # -- selection matrices - self._selection_matrix_motion = torch.diag( - torch.tensor(self.cfg.motion_control_axes, dtype=torch.float, device=self._device) + # -- selection matrices, which might be defined in the task reference frame different from the root frame + self._selection_matrix_motion_task = torch.diag_embed( + torch.tensor(self.cfg.motion_control_axes_task, dtype=torch.float, device=self._device) + .unsqueeze(0) + .repeat(self.num_envs, 1) ) - self._selection_matrix_force = torch.diag( - torch.tensor(self.cfg.force_control_axes, dtype=torch.float, device=self._device) + self._selection_matrix_force_task = torch.diag_embed( + torch.tensor(self.cfg.contact_wrench_control_axes_task, dtype=torch.float, device=self._device) + .unsqueeze(0) + .repeat(self.num_envs, 1) ) + # -- selection matrices in root frame + self._selection_matrix_motion_b = torch.zeros_like(self._selection_matrix_motion_task) + self._selection_matrix_force_b = torch.zeros_like(self._selection_matrix_force_task) # -- commands - self._task_space_target = torch.zeros(self.num_robots, self.target_dim, device=self._device) - # -- scaling of command - self._position_command_scale = torch.diag(torch.tensor(self.cfg.position_command_scale, device=self._device)) - self._rotation_command_scale = torch.diag(torch.tensor(self.cfg.rotation_command_scale, device=self._device)) + self._task_space_target_task = torch.zeros(self.num_envs, self.target_dim, device=self._device) + # -- buffers for motion/force control + self.desired_ee_pose_task = None + self.desired_ee_pose_b = None + self.desired_ee_wrench_task = None + self.desired_ee_wrench_b = None # -- motion control gains - self._p_gains = torch.zeros(self.num_robots, 6, device=self._device) - self._p_gains[:] = torch.tensor(self.cfg.stiffness, device=self._device) - self._d_gains = 2 * torch.sqrt(self._p_gains) * torch.tensor(self.cfg.damping_ratio, device=self._device) + self._motion_p_gains_task = torch.diag_embed( + torch.ones(self.num_envs, 6, device=self._device) + * torch.tensor(self.cfg.motion_stiffness_task, dtype=torch.float, device=self._device) + ) + # -- -- zero out the axes that are not motion controlled, as keeping them non-zero will cause other axes + # -- -- to act due to coupling + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task = torch.diag_embed( + 2 + * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() + * torch.as_tensor(self.cfg.motion_damping_ratio_task, dtype=torch.float, device=self._device).reshape(1, -1) + ) + # -- -- motion control gains in root frame + self._motion_p_gains_b = torch.zeros_like(self._motion_p_gains_task) + self._motion_d_gains_b = torch.zeros_like(self._motion_d_gains_task) # -- force control gains - if self.cfg.force_stiffness is not None: - self._p_wrench_gains = torch.zeros(self.num_robots, 6, device=self._device) - self._p_wrench_gains[:] = torch.tensor(self.cfg.force_stiffness, device=self._device) + if self.cfg.contact_wrench_stiffness_task is not None: + self._contact_wrench_p_gains_task = torch.diag_embed( + torch.ones(self.num_envs, 6, device=self._device) + * torch.tensor(self.cfg.contact_wrench_stiffness_task, dtype=torch.float, device=self._device) + ) + self._contact_wrench_p_gains_task[:] = ( + self._selection_matrix_force_task @ self._contact_wrench_p_gains_task[:] + ) + # -- -- force control gains in root frame + self._contact_wrench_p_gains_b = torch.zeros_like(self._contact_wrench_p_gains_task) else: - self._p_wrench_gains = None + self._contact_wrench_p_gains_task = None + self._contact_wrench_p_gains_b = None # -- position gain limits - self._p_gains_limits = torch.zeros(self.num_robots, 6, device=self._device) - self._p_gains_limits[..., 0], self._p_gains_limits[..., 1] = ( - self.cfg.stiffness_limits[0], - self.cfg.stiffness_limits[1], + self._motion_p_gains_limits = torch.zeros(self.num_envs, 6, 2, device=self._device) + self._motion_p_gains_limits[..., 0], self._motion_p_gains_limits[..., 1] = ( + self.cfg.motion_stiffness_limits_task[0], + self.cfg.motion_stiffness_limits_task[1], ) # -- damping ratio limits - self._damping_ratio_limits = torch.zeros_like(self._p_gains_limits) - self._damping_ratio_limits[..., 0], self._damping_ratio_limits[..., 1] = ( - self.cfg.damping_ratio_limits[0], - self.cfg.damping_ratio_limits[1], + self._motion_damping_ratio_limits = torch.zeros_like(self._motion_p_gains_limits) + self._motion_damping_ratio_limits[..., 0], self._motion_damping_ratio_limits[..., 1] = ( + self.cfg.motion_damping_ratio_limits_task[0], + self.cfg.motion_damping_ratio_limits_task[1], ) - # -- storing outputs - self._desired_torques = torch.zeros(self.num_robots, self.num_dof, self.num_dof, device=self._device) + # -- end-effector contact wrench + self._ee_contact_wrench_b = torch.zeros(self.num_envs, 6, device=self._device) """ Properties. """ @property - def num_actions(self) -> int: + def action_dim(self) -> int: """Dimension of the action space of controller.""" # impedance mode if self.cfg.impedance_mode == "fixed": - # task-space pose + # task-space targets return self.target_dim elif self.cfg.impedance_mode == "variable_kp": - # task-space pose + stiffness + # task-space targets + stiffness return self.target_dim + 6 elif self.cfg.impedance_mode == "variable": - # task-space pose + stiffness + damping + # task-space targets + stiffness + damping return self.target_dim + 6 + 6 else: raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") @@ -177,184 +151,301 @@ def num_actions(self) -> int: Operations. """ - def initialize(self): - """Initialize the internals.""" - pass - - def reset_idx(self, robot_ids: torch.Tensor = None): + def reset(self): """Reset the internals.""" - pass + self.desired_ee_pose_b = None + self.desired_ee_pose_task = None + self.desired_ee_wrench_b = None + self.desired_ee_wrench_task = None - def set_command(self, command: torch.Tensor): - """Set target end-effector pose or force command. + def set_command( + self, + command: torch.Tensor, + current_ee_pose_b: torch.Tensor | None = None, + current_task_frame_pose_b: torch.Tensor | None = None, + ): + """Set the task-space targets and impedance parameters. Args: - command: The target end-effector pose or force command. + command (torch.Tensor): A concatenated tensor of shape (``num_envs``, ``action_dim``) containing task-space + targets (i.e., pose/wrench) and impedance parameters. + current_ee_pose_b (torch.Tensor, optional): Current end-effector pose, in root frame, of shape + (``num_envs``, 7), containing position and quaternion ``(w, x, y, z)``. Required for relative + commands. Defaults to None. + current_task_frame_pose_b: Current pose of the task frame, in root frame, in which the targets and the + (motion/wrench) control axes are defined. It is a tensor of shape (``num_envs``, 7), + containing position and the quaternion ``(w, x, y, z)``. Defaults to None. + + Format: + Task-space targets, ordered according to 'command_types': + + Absolute pose: shape (``num_envs``, 7), containing position and quaternion ``(w, x, y, z)``. + Relative pose: shape (``num_envs``, 6), containing delta position and rotation in axis-angle form. + Absolute wrench: shape (``num_envs``, 6), containing force and torque. + + Impedance parameters: stiffness for ``variable_kp``, or stiffness, followed by damping ratio for + ``variable``: + + Stiffness: shape (``num_envs``, 6) + Damping ratio: shape (``num_envs``, 6) + + Raises: + ValueError: When the command dimensions are invalid. + ValueError: When an invalid impedance mode is provided. + ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command. + ValueError: When an invalid control command is provided. """ - # check input size - if command.shape != (self.num_robots, self.num_actions): + # Check the input dimensions + if command.shape != (self.num_envs, self.action_dim): raise ValueError( - f"Invalid command shape '{command.shape}'. Expected: '{(self.num_robots, self.num_actions)}'." + f"Invalid command shape '{command.shape}'. Expected: '{(self.num_envs, self.action_dim)}'." ) - # impedance mode + + # Resolve the impedance parameters if self.cfg.impedance_mode == "fixed": - # joint positions - self._task_space_target[:] = command + # task space targets (i.e., pose/wrench) + self._task_space_target_task[:] = command elif self.cfg.impedance_mode == "variable_kp": # split input command - task_space_command, stiffness = torch.tensor_split(command, (self.target_dim, 6), dim=-1) + task_space_command, stiffness = torch.split(command, [self.target_dim, 6], dim=-1) # format command - stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) - # joint positions + stiffness - self._task_space_target[:] = task_space_command.squeeze(dim=-1) - self._p_gains[:] = stiffness - self._d_gains[:] = 2 * torch.sqrt(self._p_gains) # critically damped + stiffness = stiffness.clip_( + min=self._motion_p_gains_limits[..., 0], max=self._motion_p_gains_limits[..., 1] + ) + # task space targets + stiffness + self._task_space_target_task[:] = task_space_command.squeeze(dim=-1) + self._motion_p_gains_task[:] = torch.diag_embed(stiffness) + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task = torch.diag_embed( + 2 + * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() + * torch.as_tensor(self.cfg.motion_damping_ratio_task, dtype=torch.float, device=self._device).reshape( + 1, -1 + ) + ) elif self.cfg.impedance_mode == "variable": # split input command - task_space_command, stiffness, damping_ratio = torch.tensor_split(command, 3, dim=-1) + task_space_command, stiffness, damping_ratio = torch.split(command, [self.target_dim, 6, 6], dim=-1) # format command - stiffness = stiffness.clip_(min=self._p_gains_limits[0], max=self._p_gains_limits[1]) - damping_ratio = damping_ratio.clip_(min=self._damping_ratio_limits[0], max=self._damping_ratio_limits[1]) - # joint positions + stiffness + damping - self._task_space_target[:] = task_space_command - self._p_gains[:] = stiffness - self._d_gains[:] = 2 * torch.sqrt(self._p_gains) * damping_ratio + stiffness = stiffness.clip_( + min=self._motion_p_gains_limits[..., 0], max=self._motion_p_gains_limits[..., 1] + ) + damping_ratio = damping_ratio.clip_( + min=self._motion_damping_ratio_limits[..., 0], max=self._motion_damping_ratio_limits[..., 1] + ) + # task space targets + stiffness + damping + self._task_space_target_task[:] = task_space_command + self._motion_p_gains_task[:] = torch.diag_embed(stiffness) + self._motion_p_gains_task[:] = self._selection_matrix_motion_task @ self._motion_p_gains_task[:] + self._motion_d_gains_task[:] = torch.diag_embed( + 2 * torch.diagonal(self._motion_p_gains_task, dim1=-2, dim2=-1).sqrt() * damping_ratio + ) else: raise ValueError(f"Invalid impedance mode: {self.cfg.impedance_mode}.") + if current_task_frame_pose_b is None: + current_task_frame_pose_b = torch.tensor( + [[0.0, 0.0, 0.0, 1.0, 0.0, 0.0, 0.0]] * self.num_envs, device=self._device + ) + + # Resolve the target commands + target_groups = torch.split(self._task_space_target_task, self.target_list, dim=1) + for command_type, target in zip(self.cfg.target_types, target_groups): + if command_type == "pose_rel": + # check input is provided + if current_ee_pose_b is None: + raise ValueError("Current pose is required for 'pose_rel' command.") + # Transform the current pose from base/root frame to task frame + current_ee_pos_task, current_ee_rot_task = subtract_frame_transforms( + current_task_frame_pose_b[:, :3], + current_task_frame_pose_b[:, 3:], + current_ee_pose_b[:, :3], + current_ee_pose_b[:, 3:], + ) + # compute targets in task frame + desired_ee_pos_task, desired_ee_rot_task = apply_delta_pose( + current_ee_pos_task, current_ee_rot_task, target + ) + self.desired_ee_pose_task = torch.cat([desired_ee_pos_task, desired_ee_rot_task], dim=-1) + elif command_type == "pose_abs": + # compute targets + self.desired_ee_pose_task = target.clone() + elif command_type == "wrench_abs": + # compute targets + self.desired_ee_wrench_task = target.clone() + else: + raise ValueError(f"Invalid control command: {command_type}.") + + # Rotation of task frame wrt root frame, converts a coordinate from task frame to root frame. + R_task_b = matrix_from_quat(current_task_frame_pose_b[:, 3:]) + # Rotation of root frame wrt task frame, converts a coordinate from root frame to task frame. + R_b_task = R_task_b.mT + + # Transform motion control stiffness gains from task frame to root frame + self._motion_p_gains_b[:, 0:3, 0:3] = R_task_b @ self._motion_p_gains_task[:, 0:3, 0:3] @ R_b_task + self._motion_p_gains_b[:, 3:6, 3:6] = R_task_b @ self._motion_p_gains_task[:, 3:6, 3:6] @ R_b_task + + # Transform motion control damping gains from task frame to root frame + self._motion_d_gains_b[:, 0:3, 0:3] = R_task_b @ self._motion_d_gains_task[:, 0:3, 0:3] @ R_b_task + self._motion_d_gains_b[:, 3:6, 3:6] = R_task_b @ self._motion_d_gains_task[:, 3:6, 3:6] @ R_b_task + + # Transform contact wrench gains from task frame to root frame (if applicable) + if self._contact_wrench_p_gains_task is not None and self._contact_wrench_p_gains_b is not None: + self._contact_wrench_p_gains_b[:, 0:3, 0:3] = ( + R_task_b @ self._contact_wrench_p_gains_task[:, 0:3, 0:3] @ R_b_task + ) + self._contact_wrench_p_gains_b[:, 3:6, 3:6] = ( + R_task_b @ self._contact_wrench_p_gains_task[:, 3:6, 3:6] @ R_b_task + ) + + # Transform selection matrices from target frame to base frame + self._selection_matrix_motion_b[:, 0:3, 0:3] = ( + R_task_b @ self._selection_matrix_motion_task[:, 0:3, 0:3] @ R_b_task + ) + self._selection_matrix_motion_b[:, 3:6, 3:6] = ( + R_task_b @ self._selection_matrix_motion_task[:, 3:6, 3:6] @ R_b_task + ) + self._selection_matrix_force_b[:, 0:3, 0:3] = ( + R_task_b @ self._selection_matrix_force_task[:, 0:3, 0:3] @ R_b_task + ) + self._selection_matrix_force_b[:, 3:6, 3:6] = ( + R_task_b @ self._selection_matrix_force_task[:, 3:6, 3:6] @ R_b_task + ) + + # Transform desired pose from task frame to root frame + if self.desired_ee_pose_task is not None: + self.desired_ee_pose_b = torch.zeros_like(self.desired_ee_pose_task) + self.desired_ee_pose_b[:, :3], self.desired_ee_pose_b[:, 3:] = combine_frame_transforms( + current_task_frame_pose_b[:, :3], + current_task_frame_pose_b[:, 3:], + self.desired_ee_pose_task[:, :3], + self.desired_ee_pose_task[:, 3:], + ) + + # Transform desired wrenches to root frame + if self.desired_ee_wrench_task is not None: + self.desired_ee_wrench_b = torch.zeros_like(self.desired_ee_wrench_task) + self.desired_ee_wrench_b[:, :3] = (R_task_b @ self.desired_ee_wrench_task[:, :3].unsqueeze(-1)).squeeze(-1) + self.desired_ee_wrench_b[:, 3:] = (R_task_b @ self.desired_ee_wrench_task[:, 3:].unsqueeze(-1)).squeeze( + -1 + ) + torch.cross(current_task_frame_pose_b[:, :3], self.desired_ee_wrench_b[:, :3], dim=-1) + def compute( self, - jacobian: torch.Tensor, - ee_pose: torch.Tensor | None = None, - ee_vel: torch.Tensor | None = None, - ee_force: torch.Tensor | None = None, + jacobian_b: torch.Tensor, + current_ee_pose_b: torch.Tensor | None = None, + current_ee_vel_b: torch.Tensor | None = None, + current_ee_force_b: torch.Tensor | None = None, mass_matrix: torch.Tensor | None = None, gravity: torch.Tensor | None = None, ) -> torch.Tensor: """Performs inference with the controller. Args: - jacobian: The Jacobian matrix of the end-effector. - ee_pose: The current end-effector pose. It is a tensor of shape - (num_robots, 7), which contains the position and quaternion (w, x, y, z). Defaults to None. - ee_vel: The current end-effector velocity. It is a tensor of shape - (num_robots, 6), which contains the linear and angular velocities. Defaults to None. - ee_force: The current external force on the end-effector. - It is a tensor of shape (num_robots, 3), which contains the linear force. Defaults to None. - mass_matrix: The joint-space inertial matrix. Defaults to None. - gravity: The joint-space gravity vector. Defaults to None. + jacobian_b: The Jacobian matrix of the end-effector in root frame. It is a tensor of shape + (``num_envs``, 6, ``num_DoF``). + current_ee_pose_b: The current end-effector pose in root frame. It is a tensor of shape + (``num_envs``, 7), which contains the position and quaternion ``(w, x, y, z)``. Defaults to ``None``. + current_ee_vel_b: The current end-effector velocity in root frame. It is a tensor of shape + (``num_envs``, 6), which contains the linear and angular velocities. Defaults to None. + current_ee_force_b: The current external force on the end-effector in root frame. + It is a tensor of shape (``num_envs``, 3), which contains the linear force. Defaults to ``None``. + mass_matrix: The joint-space mass/inertia matrix. It is a tensor of shape (``num_envs``, ``num_DoF``, + ``num_DoF``). Defaults to ``None``. + gravity: The joint-space gravity vector. It is a tensor of shape (``num_envs``, ``num_DoF``). Defaults + to ``None``. Raises: - ValueError: When the end-effector pose is not provided for the 'position_rel' command. - ValueError: When the end-effector pose is not provided for the 'position_abs' command. - ValueError: When the end-effector pose is not provided for the 'pose_rel' command. - ValueError: When an invalid command type is provided. - ValueError: When motion-control is enabled but the end-effector pose or velocity is not provided. - ValueError: When force-control is enabled but the end-effector force is not provided. - ValueError: When inertial compensation is enabled but the mass matrix is not provided. + ValueError: When motion-control is enabled but the current end-effector pose or velocity is not provided. + ValueError: When inertial dynamics decoupling is enabled but the mass matrix is not provided. + ValueError: When the current end-effector pose is not provided for the ``pose_rel`` command. + ValueError: When closed-loop force control is enabled but the current end-effector force is not provided. ValueError: When gravity compensation is enabled but the gravity vector is not provided. Returns: - The target joint torques commands. + Tensor: The joint efforts computed by the controller. It is a tensor of shape (``num_envs``, ``num_DoF``). """ - # buffers for motion/force control - desired_ee_pos = None - desired_ee_rot = None - desired_ee_force = None - # resolve the commands - target_groups = torch.split(self._task_space_target, self.target_list) - for command_type, target in zip(self.cfg.command_types, target_groups): - if command_type == "position_rel": - # check input is provided - if ee_pose is None: - raise ValueError("End-effector pose is required for 'position_rel' command.") - # scale command - target @= self._position_command_scale - # compute targets - desired_ee_pos = ee_pose[:, :3] + target - desired_ee_rot = ee_pose[:, 3:] - elif command_type == "position_abs": - # check input is provided - if ee_pose is None: - raise ValueError("End-effector pose is required for 'position_abs' command.") - # compute targets - desired_ee_pos = target - desired_ee_rot = ee_pose[:, 3:] - elif command_type == "pose_rel": - # check input is provided - if ee_pose is None: - raise ValueError("End-effector pose is required for 'pose_rel' command.") - # scale command - target[:, 0:3] @= self._position_command_scale - target[:, 3:6] @= self._rotation_command_scale - # compute targets - desired_ee_pos, desired_ee_rot = apply_delta_pose(ee_pose[:, :3], ee_pose[:, 3:], target) - elif command_type == "pose_abs": - # compute targets - desired_ee_pos = target[:, 0:3] - desired_ee_rot = target[:, 3:7] - elif command_type == "force_abs": - # compute targets - desired_ee_force = target - else: - raise ValueError(f"Invalid control command: {self.cfg.command_type}.") - # reset desired joint torques - self._desired_torques[:] = 0 - # compute for motion-control - if desired_ee_pos is not None: + # deduce number of DoF + num_DoF = jacobian_b.shape[2] + # create joint effort vector + joint_efforts = torch.zeros(self.num_envs, num_DoF, device=self._device) + + # compute joint efforts for motion-control + if self.desired_ee_pose_b is not None: # check input is provided - if ee_pose is None or ee_vel is None: - raise ValueError("End-effector pose and velocity are required for motion control.") + if current_ee_pose_b is None or current_ee_vel_b is None: + raise ValueError("Current end-effector pose and velocity are required for motion control.") # -- end-effector tracking error - pose_error = compute_pose_error( - ee_pose[:, :3], ee_pose[:, 3:], desired_ee_pos, desired_ee_rot, rot_error_type="axis_angle" + pose_error_b = torch.cat( + compute_pose_error( + current_ee_pose_b[:, :3], + current_ee_pose_b[:, 3:], + self.desired_ee_pose_b[:, :3], + self.desired_ee_pose_b[:, 3:], + rot_error_type="axis_angle", + ), + dim=-1, ) - velocity_error = -ee_vel # zero target velocity - # -- desired end-effector acceleration (spring damped system) - des_ee_acc = self._p_gains * pose_error + self._d_gains * velocity_error - # -- inertial compensation - if self.cfg.inertial_compensation: + velocity_error_b = -current_ee_vel_b # zero target velocity. The target is assumed to be stationary. + # -- desired end-effector acceleration (spring-damper system) + des_ee_acc_b = self._motion_p_gains_b @ pose_error_b.unsqueeze( + -1 + ) + self._motion_d_gains_b @ velocity_error_b.unsqueeze(-1) + # -- Inertial dynamics decoupling + if self.cfg.inertial_dynamics_decoupling: # check input is provided if mass_matrix is None: - raise ValueError("Mass matrix is required for inertial compensation.") - # compute task-space dynamics quantities - # wrench = (J M^(-1) J^T)^(-1) * \ddot(x_des) + raise ValueError("Mass matrix is required for inertial decoupling.") + # Compute operational space mass matrix mass_matrix_inv = torch.inverse(mass_matrix) - if self.cfg.uncouple_motion_wrench: - # decoupled-mass matrices - lambda_pos = torch.inverse(jacobian[:, 0:3] @ mass_matrix_inv * jacobian[:, 0:3].T) - lambda_ori = torch.inverse(jacobian[:, 3:6] @ mass_matrix_inv * jacobian[:, 3:6].T) - # desired end-effector wrench (from pseudo-dynamics) - decoupled_force = lambda_pos @ des_ee_acc[:, 0:3] - decoupled_torque = lambda_ori @ des_ee_acc[:, 3:6] - des_motion_wrench = torch.cat(decoupled_force, decoupled_torque) + if self.cfg.partial_inertial_dynamics_decoupling: + # Create a zero tensor representing the mass matrix, to fill in the non-zero elements later + os_mass_matrix = torch.zeros(self.num_envs, 6, 6, device=self._device) + # Fill in the translational and rotational parts of the inertia separately, ignoring their coupling + os_mass_matrix[:, 0:3, 0:3] = torch.inverse( + jacobian_b[:, 0:3] @ mass_matrix_inv @ jacobian_b[:, 0:3].mT + ) + os_mass_matrix[:, 3:6, 3:6] = torch.inverse( + jacobian_b[:, 3:6] @ mass_matrix_inv @ jacobian_b[:, 3:6].mT + ) else: - # coupled dynamics - lambda_full = torch.inverse(jacobian @ mass_matrix_inv * jacobian.T) - # desired end-effector wrench (from pseudo-dynamics) - des_motion_wrench = lambda_full @ des_ee_acc + # Calculate the operational space mass matrix fully accounting for the couplings + os_mass_matrix = torch.inverse(jacobian_b @ mass_matrix_inv @ jacobian_b.mT) + # (Generalized) operational space command forces + # F = (J M^(-1) J^T)^(-1) * \ddot(x_des) = M_task * \ddot(x_des) + os_command_forces_b = os_mass_matrix @ des_ee_acc_b else: - # task-space impedance control - # wrench = \ddot(x_des) - des_motion_wrench = des_ee_acc - # -- joint-space wrench - self._desired_torques += jacobian.T @ self._selection_matrix_motion @ des_motion_wrench - - # compute for force control - if desired_ee_force is not None: - # -- task-space wrench - if self.cfg.stiffness is not None: + # Task-space impedance control: command forces = \ddot(x_des). + # Please note that the definition of task-space impedance control varies in literature. + # This implementation ignores the inertial term. For inertial decoupling, + # use inertial_dynamics_decoupling=True. + os_command_forces_b = des_ee_acc_b + # -- joint-space commands + joint_efforts += (jacobian_b.mT @ self._selection_matrix_motion_b @ os_command_forces_b).squeeze(-1) + + # compute joint efforts for contact wrench/force control + if self.desired_ee_wrench_b is not None: + # -- task-space contact wrench + if self.cfg.contact_wrench_stiffness_task is not None: # check input is provided - if ee_force is None: - raise ValueError("End-effector force is required for closed-loop force control.") - # closed-loop control - des_force_wrench = desired_ee_force + self._p_wrench_gains * (desired_ee_force - ee_force) + if current_ee_force_b is None: + raise ValueError("Current end-effector force is required for closed-loop force control.") + # We can only measure the force component at the contact, so only apply the feedback for only the force + # component, keep the control of moment components open loop + self._ee_contact_wrench_b[:, 0:3] = current_ee_force_b + self._ee_contact_wrench_b[:, 3:6] = self.desired_ee_wrench_b[:, 3:6] + # closed-loop control with feedforward term + os_contact_wrench_command_b = self.desired_ee_wrench_b.unsqueeze( + -1 + ) + self._contact_wrench_p_gains_b @ (self.desired_ee_wrench_b - self._ee_contact_wrench_b).unsqueeze( + -1 + ) else: # open-loop control - des_force_wrench = desired_ee_force - # -- joint-space wrench - self._desired_torques += jacobian.T @ self._selection_matrix_force @ des_force_wrench + os_contact_wrench_command_b = self.desired_ee_wrench_b.unsqueeze(-1) + # -- joint-space commands + joint_efforts += (jacobian_b.mT @ self._selection_matrix_force_b @ os_contact_wrench_command_b).squeeze(-1) # add gravity compensation (bias correction) if self.cfg.gravity_compensation: @@ -362,6 +453,6 @@ def compute( if gravity is None: raise ValueError("Gravity vector is required for gravity compensation.") # add gravity compensation - self._desired_torques += gravity + joint_efforts += gravity - return self._desired_torques + return joint_efforts diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py new file mode 100644 index 0000000000..8befaee6aa --- /dev/null +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/controllers/operational_space_cfg.py @@ -0,0 +1,77 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from collections.abc import Sequence +from dataclasses import MISSING + +from omni.isaac.lab.utils import configclass + +from .operational_space import OperationalSpaceController + + +@configclass +class OperationalSpaceControllerCfg: + """Configuration for operational-space controller.""" + + class_type: type = OperationalSpaceController + """The associated controller class.""" + + target_types: Sequence[str] = MISSING + """Type of task-space targets. + + It has two sub-strings joined by underscore: + - type of task-space target: ``"pose"``, ``"wrench"`` + - reference for the task-space targets: ``"abs"`` (absolute), ``"rel"`` (relative, only for pose) + """ + + motion_control_axes_task: Sequence[int] = (1, 1, 1, 1, 1, 1) + """Motion direction to control in task reference frame. Mark as ``0/1`` for each axis.""" + + contact_wrench_control_axes_task: Sequence[int] = (0, 0, 0, 0, 0, 0) + """Contact wrench direction to control in task reference frame. Mark as 0/1 for each axis.""" + + inertial_dynamics_decoupling: bool = False + """Whether to perform inertial dynamics decoupling for motion control (inverse dynamics).""" + + partial_inertial_dynamics_decoupling: bool = False + """Whether to ignore the inertial coupling between the translational & rotational motions.""" + + gravity_compensation: bool = False + """Whether to perform gravity compensation.""" + + impedance_mode: str = "fixed" + """Type of gains for motion control: ``"fixed"``, ``"variable"``, ``"variable_kp"``.""" + + motion_stiffness_task: float | Sequence[float] = (100.0, 100.0, 100.0, 100.0, 100.0, 100.0) + """The positional gain for determining operational space command forces based on task-space pose error.""" + + motion_damping_ratio_task: float | Sequence[float] = (1.0, 1.0, 1.0, 1.0, 1.0, 1.0) + """The damping ratio is used in-conjunction with positional gain to compute operational space command forces + based on task-space velocity error. + + The following math operation is performed for computing velocity gains: + :math:`d_gains = 2 * sqrt(p_gains) * damping_ratio`. + """ + + motion_stiffness_limits_task: tuple[float, float] = (0, 1000) + """Minimum and maximum values for positional gains. + + Note: Used only when :obj:`impedance_mode` is ``"variable"`` or ``"variable_kp"``. + """ + + motion_damping_ratio_limits_task: tuple[float, float] = (0, 100) + """Minimum and maximum values for damping ratios used to compute velocity gains. + + Note: Used only when :obj:`impedance_mode` is ``"variable"``. + """ + + contact_wrench_stiffness_task: float | Sequence[float] | None = None + """The proportional gain for determining operational space command forces for closed-loop contact force control. + + If ``None``, then open-loop control of desired contact wrench is performed. + + Note: since only the linear forces could be measured at the moment, + only the first three elements are used for the feedback loop. + """ diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py index 6d07ceb855..09ffd48522 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/actions_cfg.py @@ -5,7 +5,7 @@ from dataclasses import MISSING -from omni.isaac.lab.controllers import DifferentialIKControllerCfg +from omni.isaac.lab.controllers import DifferentialIKControllerCfg, OperationalSpaceControllerCfg from omni.isaac.lab.managers.action_manager import ActionTerm, ActionTermCfg from omni.isaac.lab.utils import configclass @@ -248,3 +248,58 @@ class OffsetCfg: """Scale factor for the action. Defaults to 1.0.""" controller: DifferentialIKControllerCfg = MISSING """The configuration for the differential IK controller.""" + + +@configclass +class OperationalSpaceControllerActionCfg(ActionTermCfg): + """Configuration for operational space controller action term. + + See :class:`OperationalSpaceControllerAction` 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.OperationalSpaceControllerAction + + 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 motion/force control 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.""" + + task_frame_rel_path: str = None + """The path of a ``RigidObject``, relative to the sub-environment, representing task frame. Defaults to None.""" + + controller_cfg: OperationalSpaceControllerCfg = MISSING + """The configuration for the operational space controller.""" + + position_scale: float = 1.0 + """Scale factor for the position targets. Defaults to 1.0.""" + + orientation_scale: float = 1.0 + """Scale factor for the orientation (quad for ``pose_abs`` or axis-angle for ``pose_rel``). Defaults to 1.0.""" + + wrench_scale: float = 1.0 + """Scale factor for the wrench targets. Defaults to 1.0.""" + + stiffness_scale: float = 1.0 + """Scale factor for the stiffness commands. Defaults to 1.0.""" + + damping_ratio_scale: float = 1.0 + """Scale factor for the damping ratio commands. Defaults to 1.0.""" diff --git a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py index 55fd5126b8..5bd38957be 100644 --- a/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py +++ b/source/extensions/omni.isaac.lab/omni/isaac/lab/envs/mdp/actions/task_space_actions.py @@ -10,12 +10,16 @@ from typing import TYPE_CHECKING import omni.log +from pxr import UsdPhysics import omni.isaac.lab.utils.math as math_utils import omni.isaac.lab.utils.string as string_utils from omni.isaac.lab.assets.articulation import Articulation from omni.isaac.lab.controllers.differential_ik import DifferentialIKController +from omni.isaac.lab.controllers.operational_space import OperationalSpaceController from omni.isaac.lab.managers.action_manager import ActionTerm +from omni.isaac.lab.sensors import ContactSensor, ContactSensorCfg, FrameTransformer, FrameTransformerCfg +from omni.isaac.lab.sim.utils import find_matching_prims if TYPE_CHECKING: from omni.isaac.lab.envs import ManagerBasedEnv @@ -223,3 +227,423 @@ def _compute_frame_jacobian(self): jacobian[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), jacobian[:, 3:, :]) return jacobian + + +class OperationalSpaceControllerAction(ActionTerm): + r"""Operational space controller action term. + + This action term performs pre-processing of the raw actions for operational space control. + + """ + + cfg: actions_cfg.OperationalSpaceControllerActionCfg + """The configuration of the action term.""" + _asset: Articulation + """The articulation asset on which the action term is applied.""" + _contact_sensor: ContactSensor = None + """The contact sensor for the end-effector body.""" + _task_frame_transformer: FrameTransformer = None + """The frame transformer for the task frame.""" + + def __init__(self, cfg: actions_cfg.OperationalSpaceControllerActionCfg, env: ManagerBasedEnv): + # initialize the action term + super().__init__(cfg, env) + + self._sim_dt = env.sim.get_physics_dt() + + # resolve the joints over which the action term is applied + self._joint_ids, self._joint_names = self._asset.find_joints(self.cfg.joint_names) + self._num_DoF = len(self._joint_ids) + # parse the ee body index + body_ids, body_names = self._asset.find_bodies(self.cfg.body_name) + if len(body_ids) != 1: + raise ValueError( + f"Expected one match for the ee body name: {self.cfg.body_name}. Found {len(body_ids)}: {body_names}." + ) + # save only the first ee body index + self._ee_body_idx = body_ids[0] + self._ee_body_name = body_names[0] + # check if articulation is fixed-base + # if fixed-base then the jacobian for the base is not computed + # this means that number of bodies is one less than the articulation's number of bodies + if self._asset.is_fixed_base: + self._jacobi_ee_body_idx = self._ee_body_idx - 1 + self._jacobi_joint_idx = self._joint_ids + else: + self._jacobi_ee_body_idx = self._ee_body_idx + self._jacobi_joint_idx = [i + 6 for i in self._joint_ids] + + # log info for debugging + omni.log.info( + f"Resolved joint names for the action term {self.__class__.__name__}:" + f" {self._joint_names} [{self._joint_ids}]" + ) + omni.log.info( + f"Resolved ee body name for the action term {self.__class__.__name__}:" + f" {self._ee_body_name} [{self._ee_body_idx}]" + ) + # Avoid indexing across all joints for efficiency + if self._num_DoF == self._asset.num_joints: + self._joint_ids = slice(None) + + # 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 + + # create contact sensor if any of the command is wrench_abs, and if stiffness is provided + if ( + "wrench_abs" in self.cfg.controller_cfg.target_types + and self.cfg.controller_cfg.contact_wrench_stiffness_task is not None + ): + self._contact_sensor_cfg = ContactSensorCfg(prim_path=self._asset.cfg.prim_path + "/" + self._ee_body_name) + self._contact_sensor = ContactSensor(self._contact_sensor_cfg) + if not self._contact_sensor.is_initialized: + self._contact_sensor._initialize_impl() + self._contact_sensor._is_initialized = True + + # Initialize the task frame transformer if a relative path for the RigidObject, representing the task frame, + # is provided. + if self.cfg.task_frame_rel_path is not None: + # The source RigidObject can be any child of the articulation asset (we will not use it), + # hence, we will use the first RigidObject child. + root_rigidbody_path = self._first_RigidObject_child_path() + task_frame_transformer_path = "/World/envs/env_.*/" + self.cfg.task_frame_rel_path + task_frame_transformer_cfg = FrameTransformerCfg( + prim_path=root_rigidbody_path, + target_frames=[ + FrameTransformerCfg.FrameCfg( + name="task_frame", + prim_path=task_frame_transformer_path, + ), + ], + ) + self._task_frame_transformer = FrameTransformer(task_frame_transformer_cfg) + if not self._task_frame_transformer.is_initialized: + self._task_frame_transformer._initialize_impl() + self._task_frame_transformer._is_initialized = True + # create tensor for task frame pose wrt the root frame + self._task_frame_pose_b = torch.zeros(self.num_envs, 7, device=self.device) + else: + # create an empty reference for task frame pose + self._task_frame_pose_b = None + + # create the operational space controller + self._osc = OperationalSpaceController(cfg=self.cfg.controller_cfg, 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) + self._processed_actions = torch.zeros_like(self.raw_actions) + + # create tensors for the dynamic-related quantities + self._jacobian_b = torch.zeros(self.num_envs, 6, self._num_DoF, device=self.device) + self._mass_matrix = torch.zeros(self.num_envs, self._num_DoF, self._num_DoF, device=self.device) + self._gravity = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # create tensors for the ee states + self._ee_pose_w = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b = torch.zeros(self.num_envs, 7, device=self.device) + self._ee_pose_b_no_offset = torch.zeros(self.num_envs, 7, device=self.device) # The original ee without offset + self._ee_vel_w = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_vel_b = torch.zeros(self.num_envs, 6, device=self.device) + self._ee_force_w = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + self._ee_force_b = torch.zeros(self.num_envs, 3, device=self.device) # Only the forces are used for now + + # create the joint effort tensor + self._joint_efforts = torch.zeros(self.num_envs, self._num_DoF, device=self.device) + + # save the scale as tensors + self._position_scale = torch.tensor(self.cfg.position_scale, device=self.device) + self._orientation_scale = torch.tensor(self.cfg.orientation_scale, device=self.device) + self._wrench_scale = torch.tensor(self.cfg.wrench_scale, device=self.device) + self._stiffness_scale = torch.tensor(self.cfg.stiffness_scale, device=self.device) + self._damping_ratio_scale = torch.tensor(self.cfg.damping_ratio_scale, device=self.device) + + # indexes for the various command elements (e.g., pose_rel, stifness, etc.) within the command tensor + self._pose_abs_idx = None + self._pose_rel_idx = None + self._wrench_abs_idx = None + self._stiffness_idx = None + self._damping_ratio_idx = None + self._resolve_command_indexes() + + """ + Properties. + """ + + @property + def action_dim(self) -> int: + """Dimension of the action space of operational space control.""" + return self._osc.action_dim + + @property + def raw_actions(self) -> torch.Tensor: + """Raw actions for operational space control.""" + return self._raw_actions + + @property + def processed_actions(self) -> torch.Tensor: + """Processed actions for operational space control.""" + return self._processed_actions + + @property + def jacobian_w(self) -> torch.Tensor: + return self._asset.root_physx_view.get_jacobians()[:, self._jacobi_ee_body_idx, :, self._jacobi_joint_idx] + + @property + def jacobian_b(self) -> torch.Tensor: + jacobian = self.jacobian_w + base_rot = self._asset.data.root_quat_w + base_rot_matrix = math_utils.matrix_from_quat(math_utils.quat_inv(base_rot)) + jacobian[:, :3, :] = torch.bmm(base_rot_matrix, jacobian[:, :3, :]) + jacobian[:, 3:, :] = torch.bmm(base_rot_matrix, jacobian[:, 3:, :]) + return jacobian + + """ + Operations. + """ + + def process_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions and sets them as commands for for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + + # Update ee pose, which would be used by relative targets (i.e., pose_rel) + self._compute_ee_pose() + + # Update task frame pose w.r.t. the root frame. + self._compute_task_frame_pose() + + # Pre-process the raw actions for operational space control. + self._preprocess_actions(actions) + + # set command into controller + self._osc.set_command( + command=self._processed_actions, + current_ee_pose_b=self._ee_pose_b, + current_task_frame_pose_b=self._task_frame_pose_b, + ) + + def apply_actions(self): + """Computes the joint efforts for operational space control and applies them to the articulation.""" + + # Update the relevant states and dynamical quantities + self._compute_dynamic_quantities() + self._compute_ee_jacobian() + self._compute_ee_pose() + self._compute_ee_velocity() + self._compute_ee_force() + # Calculate the joint efforts + self._joint_efforts[:] = self._osc.compute( + jacobian_b=self._jacobian_b, + current_ee_pose_b=self._ee_pose_b, + current_ee_vel_b=self._ee_vel_b, + current_ee_force_b=self._ee_force_b, + mass_matrix=self._mass_matrix, + gravity=self._gravity, + ) + self._asset.set_joint_effort_target(self._joint_efforts, joint_ids=self._joint_ids) + + def reset(self, env_ids: Sequence[int] | None = None) -> None: + """Resets the raw actions and the sensors if available. + + Args: + env_ids (Sequence[int] | None): The environment indices to reset. If ``None``, all environments are reset. + """ + self._raw_actions[env_ids] = 0.0 + if self._contact_sensor is not None: + self._contact_sensor.reset(env_ids) + if self._task_frame_transformer is not None: + self._task_frame_transformer.reset(env_ids) + + """ + Helper functions. + + """ + + def _first_RigidObject_child_path(self): + """Finds the first ``RigidObject`` child under the articulation asset. + + Raises: + ValueError: If no child ``RigidObject`` is found under the articulation asset. + + Returns: + str: The path to the first ``RigidObject`` child under the articulation asset. + """ + child_prims = find_matching_prims(self._asset.cfg.prim_path + "/.*") + rigid_child_prim = None + # Loop through the list and stop at the first RigidObject found + for prim in child_prims: + if prim.HasAPI(UsdPhysics.RigidBodyAPI): + rigid_child_prim = prim + break + if rigid_child_prim is None: + raise ValueError("No child rigid body found under the expression: '{self._asset.cfg.prim_path}'/.") + rigid_child_prim_path = rigid_child_prim.GetPath().pathString + # Remove the specific env index from the path string + rigid_child_prim_path = self._asset.cfg.prim_path + "/" + rigid_child_prim_path.split("/")[-1] + return rigid_child_prim_path + + def _resolve_command_indexes(self): + """Resolves the indexes for the various command elements within the command tensor. + + Raises: + ValueError: If any command index is left unresolved. + """ + # First iterate over the target types to find the indexes of the different command elements + cmd_idx = 0 + for target_type in self.cfg.controller_cfg.target_types: + if target_type == "pose_abs": + self._pose_abs_idx = cmd_idx + cmd_idx += 7 + elif target_type == "pose_rel": + self._pose_rel_idx = cmd_idx + cmd_idx += 6 + elif target_type == "wrench_abs": + self._wrench_abs_idx = cmd_idx + cmd_idx += 6 + else: + raise ValueError("Undefined target_type for OSC within OperationalSpaceControllerAction.") + # Then iterate over the impedance parameters depending on the impedance mode + if ( + self.cfg.controller_cfg.impedance_mode == "variable_kp" + or self.cfg.controller_cfg.impedance_mode == "variable" + ): + self._stiffness_idx = cmd_idx + cmd_idx += 6 + if self.cfg.controller_cfg.impedance_mode == "variable": + self._damping_ratio_idx = cmd_idx + cmd_idx += 6 + + # Check if any command is left unresolved + if self.action_dim != cmd_idx: + raise ValueError("Not all command indexes have been resolved.") + + def _compute_dynamic_quantities(self): + """Computes the dynamic quantities for operational space control.""" + + self._mass_matrix[:] = self._asset.root_physx_view.get_mass_matrices()[:, self._joint_ids, :][ + :, :, self._joint_ids + ] + self._gravity[:] = self._asset.root_physx_view.get_generalized_gravity_forces()[:, self._joint_ids] + + def _compute_ee_jacobian(self): + """Computes the geometric Jacobian of the ee body frame in 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. + """ + # Get the Jacobian in root frame + self._jacobian_b[:] = self.jacobian_b + + # 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 + self._jacobian_b[:, 0:3, :] += torch.bmm(-math_utils.skew_symmetric_matrix(self._offset_pos), self._jacobian_b[:, 3:, :]) # type: ignore + # -- rotational part + # w_link = R_link_ee @ w_ee + self._jacobian_b[:, 3:, :] = torch.bmm(math_utils.matrix_from_quat(self._offset_rot), self._jacobian_b[:, 3:, :]) # type: ignore + + def _compute_ee_pose(self): + """Computes the pose of the ee frame in root frame.""" + # Obtain quantities from simulation + self._ee_pose_w[:] = self._asset.data.body_state_w[:, self._ee_body_idx, :7] + # Compute the pose of the ee body in the root frame + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, self._asset.data.root_quat_w, self._ee_pose_w[:, 0:3], self._ee_pose_w[:, 3:7] + ) + # Account for the offset + if self.cfg.body_offset is not None: + self._ee_pose_b[:, 0:3], self._ee_pose_b[:, 3:7] = math_utils.combine_frame_transforms( + self._ee_pose_b_no_offset[:, 0:3], self._ee_pose_b_no_offset[:, 3:7], self._offset_pos, self._offset_rot + ) + else: + self._ee_pose_b[:] = self._ee_pose_b_no_offset + + def _compute_ee_velocity(self): + """Computes the velocity of the ee frame in root frame.""" + # Extract end-effector velocity in the world frame + self._ee_vel_w[:] = self._asset.data.body_vel_w[:, self._ee_body_idx, :] + # Compute the relative velocity in the world frame + relative_vel_w = self._ee_vel_w - self._asset.data.root_vel_w + + # Convert ee velocities from world to root frame + self._ee_vel_b[:, 0:3] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 0:3]) + self._ee_vel_b[:, 3:6] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, relative_vel_w[:, 3:6]) + + # Account for the offset + if self.cfg.body_offset is not None: + # Compute offset vector in root frame + r_offset_b = math_utils.quat_rotate(self._ee_pose_b_no_offset[:, 3:7], self._offset_pos) + # Adjust the linear velocity to account for the offset + self._ee_vel_b[:, :3] += torch.cross(self._ee_vel_b[:, 3:], r_offset_b, dim=-1) + # Angular velocity is not affected by the offset + + def _compute_ee_force(self): + """Computes the contact forces on the ee frame in root frame.""" + # Obtain contact forces only if the contact sensor is available + if self._contact_sensor is not None: + self._contact_sensor.update(self._sim_dt) + self._ee_force_w[:] = self._contact_sensor.data.net_forces_w[:, 0, :] # type: ignore + # Rotate forces and torques into root frame + self._ee_force_b[:] = math_utils.quat_rotate_inverse(self._asset.data.root_quat_w, self._ee_force_w) + + def _compute_task_frame_pose(self): + """Computes the pose of the task frame in root frame.""" + # Update task frame pose if task frame rigidbody is provided + if self._task_frame_transformer is not None and self._task_frame_pose_b is not None: + self._task_frame_transformer.update(self._sim_dt) + # Calculate the pose of the task frame in the root frame + self._task_frame_pose_b[:, :3], self._task_frame_pose_b[:, 3:] = math_utils.subtract_frame_transforms( + self._asset.data.root_pos_w, + self._asset.data.root_quat_w, + self._task_frame_transformer.data.target_pos_w[:, 0, :], + self._task_frame_transformer.data.target_quat_w[:, 0, :], + ) + + def _preprocess_actions(self, actions: torch.Tensor): + """Pre-processes the raw actions for operational space control. + + Args: + actions (torch.Tensor): The raw actions for operational space control. It is a tensor of + shape (``num_envs``, ``action_dim``). + """ + # Store the raw actions. Please note that the actions contain task space targets + # (in the order of the target_types), and possibly the impedance parameters depending on impedance_mode. + self._raw_actions[:] = actions + # Initialize the processed actions with raw actions. + self._processed_actions[:] = self._raw_actions + # Go through the command types one by one, and apply the pre-processing if needed. + if self._pose_abs_idx is not None: + self._processed_actions[:, self._pose_abs_idx : self._pose_abs_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_abs_idx + 3 : self._pose_abs_idx + 7] *= self._orientation_scale + if self._pose_rel_idx is not None: + self._processed_actions[:, self._pose_rel_idx : self._pose_rel_idx + 3] *= self._position_scale + self._processed_actions[:, self._pose_rel_idx + 3 : self._pose_rel_idx + 6] *= self._orientation_scale + if self._wrench_abs_idx is not None: + self._processed_actions[:, self._wrench_abs_idx : self._wrench_abs_idx + 6] *= self._wrench_scale + if self._stiffness_idx is not None: + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] *= self._stiffness_scale + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6] = torch.clamp( + self._processed_actions[:, self._stiffness_idx : self._stiffness_idx + 6], + min=self.cfg.controller_cfg.motion_stiffness_limits_task[0], + max=self.cfg.controller_cfg.motion_stiffness_limits_task[1], + ) + if self._damping_ratio_idx is not None: + self._processed_actions[ + :, self._damping_ratio_idx : self._damping_ratio_idx + 6 + ] *= self._damping_ratio_scale + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6] = torch.clamp( + self._processed_actions[:, self._damping_ratio_idx : self._damping_ratio_idx + 6], + min=self.cfg.controller_cfg.motion_damping_ratio_limits_task[0], + max=self.cfg.controller_cfg.motion_damping_ratio_limits_task[1], + ) diff --git a/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py new file mode 100644 index 0000000000..151e42c155 --- /dev/null +++ b/source/extensions/omni.isaac.lab/test/controllers/test_operational_space.py @@ -0,0 +1,935 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +"""Launch Isaac Sim Simulator first.""" + +from omni.isaac.lab.app import AppLauncher, run_tests + +# launch omniverse app +simulation_app = AppLauncher(headless=True).app + +"""Rest everything follows.""" + +import torch +import unittest + +import omni.isaac.core.utils.prims as prim_utils +import omni.isaac.core.utils.stage as stage_utils +from omni.isaac.cloner import GridCloner + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import Articulation +from omni.isaac.lab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg +from omni.isaac.lab.markers import VisualizationMarkers +from omni.isaac.lab.markers.config import FRAME_MARKER_CFG +from omni.isaac.lab.sensors import ContactSensor, ContactSensorCfg +from omni.isaac.lab.utils.math import ( + apply_delta_pose, + combine_frame_transforms, + compute_pose_error, + matrix_from_quat, + quat_inv, + quat_rotate_inverse, + subtract_frame_transforms, +) + +## +# Pre-defined configs +## +from omni.isaac.lab_assets import FRANKA_PANDA_CFG # isort:skip + + +class TestOperationalSpaceController(unittest.TestCase): + """Test fixture for checking that Operational Space controller tracks commands properly.""" + + def setUp(self): + """Create a blank new stage for each test.""" + # Wait for spawning + stage_utils.create_new_stage() + # Constants + self.num_envs = 16 + # 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() + cfg.func("/World/GroundPlane", cfg) + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + self.ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + self.goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + + light_cfg = sim_utils.DistantLightCfg(intensity=5.0, exposure=10.0) + light_cfg.func( + "/Light", + light_cfg, + translation=[0, 0, 1], + ) + + # Create interface to clone the scene + cloner = GridCloner(spacing=2.0) + cloner.define_base_env("/World/envs") + self.env_prim_paths = cloner.generate_paths("/World/envs/env", self.num_envs) + # create source prim + prim_utils.define_prim(self.env_prim_paths[0], "Xform") + # clone the env xform + self.env_origins = cloner.clone( + source_prim_path=self.env_prim_paths[0], + prim_paths=self.env_prim_paths, + replicate_physics=True, + ) + + self.robot_cfg = FRANKA_PANDA_CFG.replace(prim_path="/World/envs/env_.*/Robot") + self.robot_cfg.actuators["panda_shoulder"].stiffness = 0.0 + self.robot_cfg.actuators["panda_shoulder"].damping = 0.0 + self.robot_cfg.actuators["panda_forearm"].stiffness = 0.0 + self.robot_cfg.actuators["panda_forearm"].damping = 0.0 + self.robot_cfg.spawn.rigid_props.disable_gravity = True + + # Define the ContactSensor + self.contact_forces = None + + # Define the target sets + ee_goal_abs_pos_set_b = torch.tensor( + [ + [0.5, 0.5, 0.7], + [0.5, -0.4, 0.6], + [0.5, 0, 0.5], + ], + device=self.sim.device, + ) + ee_goal_abs_quad_set_b = torch.tensor( + [ + [0.707, 0.0, 0.707, 0.0], + [0.707, 0.707, 0.0, 0.0], + [0.0, 1.0, 0.0, 0.0], + ], + device=self.sim.device, + ) + ee_goal_rel_pos_set = torch.tensor( + [ + [0.2, 0.0, 0.0], + [0.2, 0.2, 0.0], + [0.2, 0.2, -0.2], + ], + device=self.sim.device, + ) + ee_goal_rel_axisangle_set = torch.tensor( + [ + [0.0, torch.pi / 2, 0.0], # for [0.707, 0, 0.707, 0] + [torch.pi / 2, 0.0, 0.0], # for [0.707, 0.707, 0, 0] + [torch.pi, 0.0, 0.0], # for [0.0, 1.0, 0, 0] + ], + device=self.sim.device, + ) + ee_goal_abs_wrench_set_b = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, -1.0, 0.0], + [0.0, 10.0, 0.0, 0.0, 0.0, 0.0], + [10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + device=self.sim.device, + ) + kp_set = torch.tensor( + [ + [200.0, 200.0, 200.0, 200.0, 200.0, 200.0], + [240.0, 240.0, 240.0, 240.0, 240.0, 240.0], + [160.0, 160.0, 160.0, 160.0, 160.0, 160.0], + ], + device=self.sim.device, + ) + d_ratio_set = torch.tensor( + [ + [1.0, 1.0, 1.0, 1.0, 1.0, 1.0], + [1.1, 1.1, 1.1, 1.1, 1.1, 1.1], + [0.9, 0.9, 0.9, 0.9, 0.9, 0.9], + ], + device=self.sim.device, + ) + ee_goal_hybrid_set_b = torch.tensor( + [ + [0.6, 0.2, 0.5, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.6, -0.29, 0.6, 0.0, 0.707, 0.0, 0.707, 10.0, 0.0, 0.0, 0.0, 0.0, 0.0], + [0.6, 0.1, 0.8, 0.0, 0.5774, 0.0, 0.8165, 4.0, 0.0, 0.0, 0.0, 0.0, 0.0], + ], + device=self.sim.device, + ) + ee_goal_pose_set_tilted_b = torch.tensor( + [ + [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], + ], + device=self.sim.device, + ) + ee_goal_wrench_set_tilted_task = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + ], + device=self.sim.device, + ) + + # Define goals for the arm [xyz] + self.target_abs_pos_set_b = ee_goal_abs_pos_set_b.clone() + # Define goals for the arm [xyz + quat_wxyz] + self.target_abs_pose_set_b = torch.cat([ee_goal_abs_pos_set_b, ee_goal_abs_quad_set_b], dim=-1) + # Define goals for the arm [xyz] + self.target_rel_pos_set = ee_goal_rel_pos_set.clone() + # Define goals for the arm [xyz + axis-angle] + self.target_rel_pose_set_b = torch.cat([ee_goal_rel_pos_set, ee_goal_rel_axisangle_set], dim=-1) + # Define goals for the arm [force_xyz + torque_xyz] + self.target_abs_wrench_set = ee_goal_abs_wrench_set_b.clone() + # Define goals for the arm [xyz + quat_wxyz] and variable kp [kp_xyz + kp_rot_xyz] + self.target_abs_pose_variable_kp_set = torch.cat([self.target_abs_pose_set_b, kp_set], dim=-1) + # Define goals for the arm [xyz + quat_wxyz] and the variable imp. [kp_xyz + kp_rot_xyz + d_xyz + d_rot_xyz] + self.target_abs_pose_variable_set = torch.cat([self.target_abs_pose_set_b, kp_set, d_ratio_set], dim=-1) + # Define goals for the arm pose [xyz + quat_wxyz] and wrench [force_xyz + torque_xyz] + self.target_hybrid_set_b = ee_goal_hybrid_set_b.clone() + # Define goals for the arm pose, and wrench, and kp + self.target_hybrid_variable_kp_set = torch.cat([self.target_hybrid_set_b, kp_set], dim=-1) + # Define goals for the arm pose [xyz + quat_wxyz] in root and and wrench [force_xyz + torque_xyz] in task frame + self.target_hybrid_set_tilted = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task], dim=-1) + + # Reference frame for targets + self.frame = "root" + + def tearDown(self): + """Stops simulator after each test.""" + # stop simulation + self.sim.stop() + # self.sim.clear() # FIXME: This hangs the test for some reason when LIVESTREAM is not enabled. + self.sim.clear_all_callbacks() + self.sim.clear_instance() + # Make contact_forces None after relevant tests otherwise other tests give warning + self.contact_forces = None + + """ + Test fixtures. + """ + + def test_franka_pose_abs_without_inertial_decoupling(self): + """Test absolute pose control with fixed impedance and without inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=[400.0, 400.0, 400.0, 100.0, 100.0, 100.0], + motion_damping_ratio_task=[5.0, 5.0, 5.0, 0.001, 0.001, 0.001], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_abs_with_partial_inertial_decoupling(self): + """Test absolute pose control with fixed impedance and partial inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=1000.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_abs_fixed_impedance_with_gravity_compensation(self): + """Test absolute pose control with fixed impedance, gravity compensation, and inertial dynamics decoupling.""" + self.robot_cfg.spawn.rigid_props.disable_gravity = False + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=True, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_abs(self): + """Test absolute pose control with fixed impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_pose_rel(self): + """Test relative pose control with fixed impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_rel"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_rel_pose_set_b) + + def test_franka_pose_abs_variable_impedance(self): + """Test absolute pose control with variable impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_variable_set) + + def test_franka_wrench_abs_open_loop(self): + """Test open loop absolute force control.""" + robot = Articulation(cfg=self.robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(0.7, 0.7, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(0.2, 0.0, 0.93), + orientation=(0.9848, 0.0, -0.1736, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle2", + obstacle_spawn_cfg, + translation=(0.2, 0.35, 0.7), + orientation=(0.707, 0.707, 0.0, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle3", + obstacle_spawn_cfg, + translation=(0.55, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=50, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["wrench_abs"], + motion_control_axes_task=[0, 0, 0, 0, 0, 0], + contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_wrench_set) + + def test_franka_wrench_abs_closed_loop(self): + """Test closed loop absolute force control.""" + robot = Articulation(cfg=self.robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(0.7, 0.7, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(0.2, 0.0, 0.93), + orientation=(0.9848, 0.0, -0.1736, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle2", + obstacle_spawn_cfg, + translation=(0.2, 0.35, 0.7), + orientation=(0.707, 0.707, 0.0, 0.0), + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle3", + obstacle_spawn_cfg, + translation=(0.55, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["wrench_abs"], + contact_wrench_stiffness_task=[ + 0.2, + 0.2, + 0.2, + 0.0, + 0.0, + 0.0, + ], # Zero torque feedback as we cannot contact torque + motion_control_axes_task=[0, 0, 0, 0, 0, 0], + contact_wrench_control_axes_task=[1, 1, 1, 1, 1, 1], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_wrench_set) + + def test_franka_hybrid_decoupled_motion(self): + """Test hybrid control with fixed impedance and partial inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(1.0, 1.0, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(self.target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=True, + gravity_compensation=False, + motion_stiffness_task=300.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], + motion_control_axes_task=[0, 1, 1, 1, 1, 1], + contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_b) + + def test_franka_hybrid_variable_kp_impedance(self): + """Test hybrid control with variable kp impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(1.0, 1.0, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(self.target_hybrid_set_b[0, 0] + 0.05, 0.0, 0.7), + orientation=(0.707, 0.0, 0.707, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_damping_ratio_task=0.8, + contact_wrench_stiffness_task=[0.1, 0.0, 0.0, 0.0, 0.0, 0.0], + motion_control_axes_task=[0, 1, 1, 1, 1, 1], + contact_wrench_control_axes_task=[1, 0, 0, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller( + robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_variable_kp_set + ) + + def test_franka_taskframe_pose_abs(self): + """Test absolute pose control in task frame with fixed impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + self.frame = "task" + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_abs_pose_set_b) + + def test_franka_taskframe_pose_rel(self): + """Test relative pose control in task frame with fixed impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + self.frame = "task" + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_rel"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=500.0, + motion_damping_ratio_task=1.0, + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_hand", ["panda_joint.*"], self.target_rel_pose_set_b) + + def test_franka_taskframe_hybrid(self): + """Test hybrid control in task frame with fixed impedance and inertial dynamics decoupling.""" + robot = Articulation(cfg=self.robot_cfg) + self.frame = "task" + + obstacle_spawn_cfg = sim_utils.CuboidCfg( + size=(2.0, 1.5, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ) + obstacle_spawn_cfg.func( + "/World/envs/env_.*/obstacle1", + obstacle_spawn_cfg, + translation=(self.target_hybrid_set_tilted[0, 0] + 0.085, 0.0, 0.3), + orientation=(0.9238795325, 0.0, -0.3826834324, 0.0), + ) + contact_forces_cfg = ContactSensorCfg( + prim_path="/World/envs/env_.*/obstacle.*", + update_period=0.0, + history_length=2, + debug_vis=False, + force_threshold=0.1, + ) + self.contact_forces = ContactSensor(contact_forces_cfg) + + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="fixed", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=400.0, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], + motion_control_axes_task=[1, 1, 0, 1, 1, 1], + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=self.num_envs, device=self.sim.device) + + self._run_op_space_controller(robot, osc, "panda_leftfinger", ["panda_joint.*"], self.target_hybrid_set_tilted) + + """ + Helper functions + """ + + def _run_op_space_controller( + self, + robot: Articulation, + osc: OperationalSpaceController, + ee_frame_name: str, + arm_joint_names: list[str], + target_set: torch.tensor, + ): + """Run the operational space controller with the given parameters. + + Args: + robot (Articulation): The robot to control. + osc (OperationalSpaceController): The operational space controller. + ee_frame_name (str): The name of the end-effector frame. + arm_joint_names (list[str]): The names of the arm joints. + target_set (torch.tensor): The target set to track. + """ + # Initialize the masks for evaluating target convergence according to selection matrices + self.pos_mask = torch.tensor(osc.cfg.motion_control_axes_task[:3], device=self.sim.device).view(1, 3) + self.rot_mask = torch.tensor(osc.cfg.motion_control_axes_task[3:], device=self.sim.device).view(1, 3) + self.wrench_mask = torch.tensor(osc.cfg.contact_wrench_control_axes_task, device=self.sim.device).view(1, 6) + self.force_mask = self.wrench_mask[:, 0:3] # Take only the force components as we can measure only these + + # Define simulation stepping + sim_dt = self.sim.get_physics_dt() + # Play the simulator + self.sim.reset() + + # Obtain the frame index of the end-effector + ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] + # Obtain joint indices + arm_joint_ids = robot.find_joints(arm_joint_names)[0] + + # Update existing buffers + # Note: We need to update buffers before the first step for the controller. + robot.update(dt=sim_dt) + + # get the updated states + jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = self._update_states( + robot, ee_frame_idx, arm_joint_ids + ) + + # Track the given target command + current_goal_idx = 0 # Current goal index for the arm + command = torch.zeros( + self.num_envs, osc.action_dim, device=self.sim.device + ) # Generic target command, which can be pose, position, force, etc. + ee_target_pose_b = torch.zeros(self.num_envs, 7, device=self.sim.device) # Target pose in the body frame + ee_target_pose_w = torch.zeros( + self.num_envs, 7, device=self.sim.device + ) # Target pose in the world frame (for marker) + + # Set joint efforts to zero + zero_joint_efforts = torch.zeros(self.num_envs, robot.num_joints, device=self.sim.device) + joint_efforts = torch.zeros(self.num_envs, len(arm_joint_ids), device=self.sim.device) + + # Now we are ready! + for count in range(1501): + # reset every 500 steps + if count % 500 == 0: + # check that we converged to the goal + if count > 0: + self._check_convergence(osc, ee_pose_b, ee_target_pose_b, ee_force_b, command) + # reset joint state to default + default_joint_pos = robot.data.default_joint_pos.clone() + default_joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel) + robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step + robot.write_data_to_sim() + robot.reset() + # reset contact sensor + if self.contact_forces is not None: + self.contact_forces.reset() + # reset target pose + robot.update(sim_dt) + _, _, _, ee_pose_b, _, _, _, _ = self._update_states( + robot, ee_frame_idx, arm_joint_ids + ) # at reset, the jacobians are not updated to the latest state + command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = self._update_target( + osc, root_pose_w, ee_pose_b, target_set, current_goal_idx + ) + # set the osc command + osc.reset() + command, task_frame_pose_b = self._convert_to_task_frame( + osc, command=command, ee_target_pose_b=ee_target_pose_b + ) + osc.set_command( + command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b + ) + else: + # get the updated states + jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = ( + self._update_states(robot, ee_frame_idx, arm_joint_ids) + ) + # compute the joint commands + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + current_ee_force_b=ee_force_b, + mass_matrix=mass_matrix, + gravity=gravity, + ) + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + + # update marker positions + self.ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + self.goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7]) + + # perform step + self.sim.step(render=False) + # update buffers + robot.update(sim_dt) + + def _update_states( + self, + robot: Articulation, + ee_frame_idx: int, + arm_joint_ids: list[int], + ): + """Update the states of the robot and obtain the relevant quantities for the operational space controller. + + Args: + robot (Articulation): The robot to control. + ee_frame_idx (int): The index of the end-effector frame. + arm_joint_ids (list[int]): The indices of the arm joints. + + Returns: + jacobian_b (torch.tensor): The Jacobian in the root frame. + mass_matrix (torch.tensor): The mass matrix. + gravity (torch.tensor): The gravity vector. + ee_pose_b (torch.tensor): The end-effector pose in the root frame. + ee_vel_b (torch.tensor): The end-effector velocity in the root frame. + root_pose_w (torch.tensor): The root pose in the world frame. + ee_pose_w (torch.tensor): The end-effector pose in the world frame. + ee_force_b (torch.tensor): The end-effector force in the root frame. + """ + # obtain dynamics related quantities from simulation + ee_jacobi_idx = ee_frame_idx - 1 + jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] + mass_matrix = robot.root_physx_view.get_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] + gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] + # Convert the Jacobian from world to root frame + jacobian_b = jacobian_w.clone() + root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_state_w[:, 3:7])) + jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) + jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) + + # Compute current pose of the end-effector + root_pose_w = robot.data.root_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] + ee_pos_b, ee_quat_b = 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 = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + + # Compute the current velocity of the end-effector + ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame + root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame + relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame + ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) + ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) + + # Calculate the contact force + ee_force_w = torch.zeros(self.num_envs, 3, device=self.sim.device) + if self.contact_forces is not None: # Only modify if it exist + sim_dt = self.sim.get_physics_dt() + self.contact_forces.update(sim_dt) # update contact sensor + # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and + # taking the max of three surfaces as only one should be the contact of interest + ee_force_w, _ = torch.max(torch.mean(self.contact_forces.data.net_forces_w_history, dim=1), dim=1) + + # This is a simplification, only for the sake of testing. + ee_force_b = ee_force_w + + return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + + def _update_target( + self, + osc: OperationalSpaceController, + root_pose_w: torch.tensor, + ee_pose_b: torch.tensor, + target_set: torch.tensor, + current_goal_idx: int, + ): + """Update the target for the operational space controller. + + Args: + osc (OperationalSpaceController): The operational space controller. + root_pose_w (torch.tensor): The root pose in the world frame. + ee_pose_b (torch.tensor): The end-effector pose in the body frame. + target_set (torch.tensor): The target set to track. + current_goal_idx (int): The current goal index. + + Returns: + command (torch.tensor): The target command. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + ee_target_pose_w (torch.tensor): The end-effector target pose in the world frame. + next_goal_idx (int): The next goal index. + + Raises: + ValueError: If the target type is undefined. + """ + # update the ee desired command + command = torch.zeros(self.num_envs, osc.action_dim, device=self.sim.device) + command[:] = target_set[current_goal_idx] + + # update the ee desired pose + ee_target_pose_b = torch.zeros(self.num_envs, 7, device=self.sim.device) + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + ee_target_pose_b[:] = command[:, :7] + elif target_type == "pose_rel": + ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] = apply_delta_pose( + ee_pose_b[:, :3], ee_pose_b[:, 3:], command[:, :7] + ) + elif target_type == "wrench_abs": + pass # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b + else: + raise ValueError("Undefined target_type within _update_target().") + + # update the target desired pose in world frame (for marker) + ee_target_pos_w, ee_target_quat_w = combine_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1) + + next_goal_idx = (current_goal_idx + 1) % len(target_set) + + return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx + + def _convert_to_task_frame( + self, osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor + ): + """Convert the target command to the task frame if required. + + Args: + osc (OperationalSpaceController): The operational space controller. + command (torch.tensor): The target command to convert. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + + Returns: + command (torch.tensor): The converted target command. + task_frame_pose_b (torch.tensor): The task frame pose in the body frame. + + Raises: + ValueError: If the frame is invalid. + """ + command = command.clone() + task_frame_pose_b = None + if self.frame == "root": + # No need to transform anything if they are already in root frame + pass + elif self.frame == "task": + # Convert target commands from base to the task frame + command = command.clone() + task_frame_pose_b = ee_target_pose_b.clone() + + cmd_idx = 0 + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + command[:, :3], command[:, 3:7] = subtract_frame_transforms( + task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7] + ) + cmd_idx += 7 + elif target_type == "pose_rel": + # Compute rotation matrices + R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) # Task frame to base frame + R_b_task = R_task_b.mT # Base frame to task frame + # Transform the delta position and orientation from base to task frame + command[:, :3] = (R_b_task @ command[:, :3].unsqueeze(-1)).squeeze(-1) + command[:, 3:7] = (R_b_task @ command[:, 3:7].unsqueeze(-1)).squeeze(-1) + cmd_idx += 6 + elif target_type == "wrench_abs": + # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is + # easier), so not transforming + cmd_idx += 6 + else: + raise ValueError("Undefined target_type within _convert_to_task_frame().") + else: + # Raise error for invalid frame + raise ValueError("Invalid frame selection for target setting inside the test_operational_space.") + + return command, task_frame_pose_b + + def _check_convergence( + self, + osc: OperationalSpaceController, + ee_pose_b: torch.tensor, + ee_target_pose_b: torch.tensor, + ee_force_b: torch.tensor, + ee_target_b: torch.tensor, + ): + """Check the convergence to the target. + + Args: + osc (OperationalSpaceController): The operational space controller. + ee_pose_b (torch.tensor): The end-effector pose in the body frame. + ee_target_pose_b (torch.tensor): The end-effector target pose in the body frame. + ee_force_b (torch.tensor): The end-effector force in the body frame. + ee_target_b (torch.tensor): The end-effector target in the body frame. + + Raises: + AssertionError: If the convergence is not achieved. + ValueError: If the target type is undefined. + """ + cmd_idx = 0 + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + pos_error, rot_error = compute_pose_error( + ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + pos_error_norm = torch.norm(pos_error * self.pos_mask, dim=-1) + rot_error_norm = torch.norm(rot_error * self.rot_mask, dim=-1) + # desired error (zer) + des_error = torch.zeros_like(pos_error_norm) + # check convergence + torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) + torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) + cmd_idx += 7 + elif target_type == "pose_rel": + pos_error, rot_error = compute_pose_error( + ee_pose_b[:, 0:3], ee_pose_b[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + pos_error_norm = torch.norm(pos_error * self.pos_mask, dim=-1) + rot_error_norm = torch.norm(rot_error * self.rot_mask, dim=-1) + # desired error (zer) + des_error = torch.zeros_like(pos_error_norm) + # check convergence + torch.testing.assert_close(pos_error_norm, des_error, rtol=0.0, atol=0.1) + torch.testing.assert_close(rot_error_norm, des_error, rtol=0.0, atol=0.1) + cmd_idx += 6 + elif target_type == "wrench_abs": + force_target_b = ee_target_b[:, cmd_idx : cmd_idx + 3].clone() + # Convert to base frame if the target was defined in task frame + if self.frame == "task": + task_frame_pose_b = ee_target_pose_b.clone() + R_task_b = matrix_from_quat(task_frame_pose_b[:, 3:]) + force_target_b[:] = (R_task_b @ force_target_b[:].unsqueeze(-1)).squeeze(-1) + force_error = ee_force_b - force_target_b + force_error_norm = torch.norm( + force_error * self.force_mask, dim=-1 + ) # ignore torque part as we cannot measure it + des_error = torch.zeros_like(force_error_norm) + # check convergence: big threshold here as the force control is not precise when the robot moves + torch.testing.assert_close(force_error_norm, des_error, rtol=0.0, atol=1.0) + cmd_idx += 6 + else: + raise ValueError("Undefined target_type within _check_convergence().") + + +if __name__ == "__main__": + run_tests() diff --git a/source/extensions/omni.isaac.lab_tasks/config/extension.toml b/source/extensions/omni.isaac.lab_tasks/config/extension.toml index ba976faf92..accf06c5c5 100644 --- a/source/extensions/omni.isaac.lab_tasks/config/extension.toml +++ b/source/extensions/omni.isaac.lab_tasks/config/extension.toml @@ -1,7 +1,7 @@ [package] # Note: Semantic Versioning is used: https://semver.org/ -version = "0.10.14" +version = "0.10.15" # Description title = "Isaac Lab Environments" diff --git a/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst b/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst index 068db9b64a..2e10dc6dbd 100644 --- a/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst +++ b/source/extensions/omni.isaac.lab_tasks/docs/CHANGELOG.rst @@ -1,6 +1,16 @@ Changelog --------- +0.10.15 (2024-12-16) +~~~~~~~~~~~~~~~~~~~~ + +Added +^^^^^ +* Added ``Isaac-Reach-Franka-OSC-v0`` and ``Isaac-Reach-Franka-OSC-Play-v0`` + variations of the manager based reach environment that uses + :class:`omni.isaac.lab.envs.mdp.actions.OperationalSpaceControllerAction`. + + 0.10.14 (2024-12-03) ~~~~~~~~~~~~~~~~~~~~ diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/__init__.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/__init__.py index f66d0ba910..07fc54526f 100644 --- a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/__init__.py +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/__init__.py @@ -65,3 +65,27 @@ }, disable_env_checker=True, ) + +## +# Operational Space Control +## + +gym.register( + id="Isaac-Reach-Franka-OSC-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaReachEnvCfg", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + }, +) + +gym.register( + id="Isaac-Reach-Franka-OSC-Play-v0", + entry_point="omni.isaac.lab.envs:ManagerBasedRLEnv", + disable_env_checker=True, + kwargs={ + "env_cfg_entry_point": f"{__name__}.osc_env_cfg:FrankaReachEnvCfg_PLAY", + "rsl_rl_cfg_entry_point": f"{agents.__name__}.rsl_rl_ppo_cfg:FrankaReachPPORunnerCfg", + }, +) diff --git a/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py new file mode 100644 index 0000000000..410e33c467 --- /dev/null +++ b/source/extensions/omni.isaac.lab_tasks/omni/isaac/lab_tasks/manager_based/manipulation/reach/config/franka/osc_env_cfg.py @@ -0,0 +1,71 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +from omni.isaac.lab.controllers.operational_space_cfg import OperationalSpaceControllerCfg +from omni.isaac.lab.envs.mdp.actions.actions_cfg import OperationalSpaceControllerActionCfg +from omni.isaac.lab.utils import configclass + +from . import joint_pos_env_cfg + +## +# Pre-defined configs +## +from omni.isaac.lab_assets.franka import FRANKA_PANDA_CFG # isort: skip + + +@configclass +class FrankaReachEnvCfg(joint_pos_env_cfg.FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + + # Set Franka as robot + # We remove stiffness and damping for the shoulder and forearm joints for effort control + self.scene.robot = FRANKA_PANDA_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + self.scene.robot.actuators["panda_shoulder"].stiffness = 0.0 + self.scene.robot.actuators["panda_shoulder"].damping = 0.0 + self.scene.robot.actuators["panda_forearm"].stiffness = 0.0 + self.scene.robot.actuators["panda_forearm"].damping = 0.0 + self.scene.robot.spawn.rigid_props.disable_gravity = True + + # If closed-loop contact force control is desired, contact sensors should be enabled for the robot + # self.scene.robot.spawn.activate_contact_sensors = True + + self.actions.arm_action = OperationalSpaceControllerActionCfg( + asset_name="robot", + joint_names=["panda_joint.*"], + body_name="panda_hand", + # If a task frame different from articulation root/base is desired, a RigidObject, e.g., "task_frame", + # can be added to the scene and its relative path could provided as task_frame_rel_path + # task_frame_rel_path="task_frame", + controller_cfg=OperationalSpaceControllerCfg( + target_types=["pose_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_stiffness_task=100.0, + motion_damping_ratio_task=1.0, + motion_stiffness_limits_task=(50.0, 200.0), + ), + position_scale=1.0, + orientation_scale=1.0, + stiffness_scale=100.0, + ) + # Removing these observations as they are not needed for OSC and we want keep the observation space small + self.observations.policy.joint_pos = None + self.observations.policy.joint_vel = None + + +@configclass +class FrankaReachEnvCfg_PLAY(FrankaReachEnvCfg): + def __post_init__(self): + # post init of parent + super().__post_init__() + # make a smaller scene for play + self.scene.num_envs = 16 + self.scene.env_spacing = 2.5 + # disable randomization for play + self.observations.policy.enable_corruption = False diff --git a/source/standalone/tutorials/05_controllers/run_osc.py b/source/standalone/tutorials/05_controllers/run_osc.py new file mode 100644 index 0000000000..8970d364a2 --- /dev/null +++ b/source/standalone/tutorials/05_controllers/run_osc.py @@ -0,0 +1,440 @@ +# Copyright (c) 2022-2024, The Isaac Lab Project Developers. +# All rights reserved. +# +# SPDX-License-Identifier: BSD-3-Clause + +""" +This script demonstrates how to use the operational space controller (OSC) with the simulator. + +The OSC controller can be configured in different modes. It uses the dynamical quantities such as Jacobians and +mass matricescomputed by PhysX. + +.. code-block:: bash + + # Usage + ./isaaclab.sh -p source/standalone/tutorials/05_controllers/run_osc.py + +""" + +"""Launch Isaac Sim Simulator first.""" + +import argparse + +from omni.isaac.lab.app import AppLauncher + +# add argparse arguments +parser = argparse.ArgumentParser(description="Tutorial on using the operational space controller.") +parser.add_argument("--num_envs", type=int, default=128, help="Number of environments to spawn.") +# append AppLauncher cli args +AppLauncher.add_app_launcher_args(parser) +# parse the arguments +args_cli = parser.parse_args() + +# launch omniverse app +app_launcher = AppLauncher(args_cli) +simulation_app = app_launcher.app + +"""Rest everything follows.""" + +import torch + +import omni.isaac.lab.sim as sim_utils +from omni.isaac.lab.assets import Articulation, AssetBaseCfg +from omni.isaac.lab.controllers import OperationalSpaceController, OperationalSpaceControllerCfg +from omni.isaac.lab.markers import VisualizationMarkers +from omni.isaac.lab.markers.config import FRAME_MARKER_CFG +from omni.isaac.lab.scene import InteractiveScene, InteractiveSceneCfg +from omni.isaac.lab.sensors import ContactSensorCfg +from omni.isaac.lab.utils import configclass +from omni.isaac.lab.utils.math import ( + combine_frame_transforms, + matrix_from_quat, + quat_inv, + quat_rotate_inverse, + subtract_frame_transforms, +) + +## +# Pre-defined configs +## +from omni.isaac.lab_assets import FRANKA_PANDA_HIGH_PD_CFG # isort:skip + + +@configclass +class SceneCfg(InteractiveSceneCfg): + """Configuration for a simple scene with a tilted wall.""" + + # ground plane + ground = AssetBaseCfg( + prim_path="/World/defaultGroundPlane", + spawn=sim_utils.GroundPlaneCfg(), + ) + + # lights + dome_light = AssetBaseCfg( + prim_path="/World/Light", spawn=sim_utils.DomeLightCfg(intensity=3000.0, color=(0.75, 0.75, 0.75)) + ) + + # Tilted wall + tilted_wall = AssetBaseCfg( + prim_path="{ENV_REGEX_NS}/TiltedWall", + spawn=sim_utils.CuboidCfg( + size=(2.0, 1.5, 0.01), + collision_props=sim_utils.CollisionPropertiesCfg(), + visual_material=sim_utils.PreviewSurfaceCfg(diffuse_color=(1.0, 0.0, 0.0), opacity=0.1), + rigid_props=sim_utils.RigidBodyPropertiesCfg(kinematic_enabled=True), + activate_contact_sensors=True, + ), + init_state=AssetBaseCfg.InitialStateCfg( + pos=(0.6 + 0.085, 0.0, 0.3), rot=(0.9238795325, 0.0, -0.3826834324, 0.0) + ), + ) + + contact_forces = ContactSensorCfg( + prim_path="/World/envs/env_.*/TiltedWall", + update_period=0.0, + history_length=2, + debug_vis=False, + ) + + robot = FRANKA_PANDA_HIGH_PD_CFG.replace(prim_path="{ENV_REGEX_NS}/Robot") + robot.actuators["panda_shoulder"].stiffness = 0.0 + robot.actuators["panda_shoulder"].damping = 0.0 + robot.actuators["panda_forearm"].stiffness = 0.0 + robot.actuators["panda_forearm"].damping = 0.0 + robot.spawn.rigid_props.disable_gravity = True + + +def run_simulator(sim: sim_utils.SimulationContext, scene: InteractiveScene): + """Runs the simulation loop. + + Args: + sim: (SimulationContext) Simulation context. + scene: (InteractiveScene) Interactive scene. + """ + + # Extract scene entities for readability. + robot = scene["robot"] + contact_forces = scene["contact_forces"] + + # Obtain indices for the end-effector and arm joints + ee_frame_name = "panda_leftfinger" + arm_joint_names = ["panda_joint.*"] + ee_frame_idx = robot.find_bodies(ee_frame_name)[0][0] + arm_joint_ids = robot.find_joints(arm_joint_names)[0] + + # Create the OSC + osc_cfg = OperationalSpaceControllerCfg( + target_types=["pose_abs", "wrench_abs"], + impedance_mode="variable_kp", + inertial_dynamics_decoupling=True, + partial_inertial_dynamics_decoupling=False, + gravity_compensation=False, + motion_damping_ratio_task=1.0, + contact_wrench_stiffness_task=[0.0, 0.0, 0.1, 0.0, 0.0, 0.0], + motion_control_axes_task=[1, 1, 0, 1, 1, 1], + contact_wrench_control_axes_task=[0, 0, 1, 0, 0, 0], + ) + osc = OperationalSpaceController(osc_cfg, num_envs=scene.num_envs, device=sim.device) + + # Markers + frame_marker_cfg = FRAME_MARKER_CFG.copy() + frame_marker_cfg.markers["frame"].scale = (0.1, 0.1, 0.1) + ee_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_current")) + goal_marker = VisualizationMarkers(frame_marker_cfg.replace(prim_path="/Visuals/ee_goal")) + + # Define targets for the arm + ee_goal_pose_set_tilted_b = torch.tensor( + [ + [0.6, 0.15, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.6, -0.3, 0.3, 0.0, 0.92387953, 0.0, 0.38268343], + [0.8, 0.0, 0.5, 0.0, 0.92387953, 0.0, 0.38268343], + ], + device=sim.device, + ) + ee_goal_wrench_set_tilted_task = torch.tensor( + [ + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + [0.0, 0.0, 10.0, 0.0, 0.0, 0.0], + ], + device=sim.device, + ) + kp_set_task = torch.tensor( + [ + [360.0, 360.0, 360.0, 360.0, 360.0, 360.0], + [420.0, 420.0, 420.0, 420.0, 420.0, 420.0], + [320.0, 320.0, 320.0, 320.0, 320.0, 320.0], + ], + device=sim.device, + ) + ee_target_set = torch.cat([ee_goal_pose_set_tilted_b, ee_goal_wrench_set_tilted_task, kp_set_task], dim=-1) + + # Define simulation stepping + sim_dt = sim.get_physics_dt() + + # Update existing buffers + # Note: We need to update buffers before the first step for the controller. + robot.update(dt=sim_dt) + + # get the updated states + jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = update_states( + sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces + ) + + # Track the given target command + current_goal_idx = 0 # Current goal index for the arm + command = torch.zeros( + scene.num_envs, osc.action_dim, device=sim.device + ) # Generic target command, which can be pose, position, force, etc. + ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device) # Target pose in the body frame + ee_target_pose_w = torch.zeros(scene.num_envs, 7, device=sim.device) # Target pose in the world frame (for marker) + + # Set joint efforts to zero + zero_joint_efforts = torch.zeros(scene.num_envs, robot.num_joints, device=sim.device) + joint_efforts = torch.zeros(scene.num_envs, len(arm_joint_ids), device=sim.device) + + count = 0 + # Simulation loop + while simulation_app.is_running(): + # reset every 500 steps + if count % 500 == 0: + # reset joint state to default + default_joint_pos = robot.data.default_joint_pos.clone() + default_joint_vel = robot.data.default_joint_vel.clone() + robot.write_joint_state_to_sim(default_joint_pos, default_joint_vel) + robot.set_joint_effort_target(zero_joint_efforts) # Set zero torques in the initial step + robot.write_data_to_sim() + robot.reset() + # reset contact sensor + contact_forces.reset() + # reset target pose + robot.update(sim_dt) + _, _, _, ee_pose_b, _, _, _, _ = update_states( + sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces + ) # at reset, the jacobians are not updated to the latest state + command, ee_target_pose_b, ee_target_pose_w, current_goal_idx = update_target( + sim, scene, osc, root_pose_w, ee_target_set, current_goal_idx + ) + # set the osc command + osc.reset() + command, task_frame_pose_b = convert_to_task_frame(osc, command=command, ee_target_pose_b=ee_target_pose_b) + osc.set_command(command=command, current_ee_pose_b=ee_pose_b, current_task_frame_pose_b=task_frame_pose_b) + else: + # get the updated states + jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b = update_states( + sim, scene, robot, ee_frame_idx, arm_joint_ids, contact_forces + ) + # compute the joint commands + joint_efforts = osc.compute( + jacobian_b=jacobian_b, + current_ee_pose_b=ee_pose_b, + current_ee_vel_b=ee_vel_b, + current_ee_force_b=ee_force_b, + mass_matrix=mass_matrix, + gravity=gravity, + ) + # apply actions + robot.set_joint_effort_target(joint_efforts, joint_ids=arm_joint_ids) + robot.write_data_to_sim() + + # update marker positions + ee_marker.visualize(ee_pose_w[:, 0:3], ee_pose_w[:, 3:7]) + goal_marker.visualize(ee_target_pose_w[:, 0:3], ee_target_pose_w[:, 3:7]) + + # perform step + sim.step(render=True) + # update robot buffers + robot.update(sim_dt) + # update buffers + scene.update(sim_dt) + # update sim-time + count += 1 + + +# Update robot states +def update_states( + sim: sim_utils.SimulationContext, + scene: InteractiveScene, + robot: Articulation, + ee_frame_idx: int, + arm_joint_ids: list[int], + contact_forces, +): + """Update the robot states. + + Args: + sim: (SimulationContext) Simulation context. + scene: (InteractiveScene) Interactive scene. + robot: (Articulation) Robot articulation. + ee_frame_idx: (int) End-effector frame index. + arm_joint_ids: (list[int]) Arm joint indices. + contact_forces: (ContactSensor) Contact sensor. + + Returns: + jacobian_b (torch.tensor): Jacobian in the body frame. + mass_matrix (torch.tensor): Mass matrix. + gravity (torch.tensor): Gravity vector. + ee_pose_b (torch.tensor): End-effector pose in the body frame. + ee_vel_b (torch.tensor): End-effector velocity in the body frame. + root_pose_w (torch.tensor): Root pose in the world frame. + ee_pose_w (torch.tensor): End-effector pose in the world frame. + ee_force_b (torch.tensor): End-effector force in the body frame. + + Raises: + ValueError: Undefined target_type. + """ + # obtain dynamics related quantities from simulation + ee_jacobi_idx = ee_frame_idx - 1 + jacobian_w = robot.root_physx_view.get_jacobians()[:, ee_jacobi_idx, :, arm_joint_ids] + mass_matrix = robot.root_physx_view.get_mass_matrices()[:, arm_joint_ids, :][:, :, arm_joint_ids] + gravity = robot.root_physx_view.get_generalized_gravity_forces()[:, arm_joint_ids] + # Convert the Jacobian from world to root frame + jacobian_b = jacobian_w.clone() + root_rot_matrix = matrix_from_quat(quat_inv(robot.data.root_state_w[:, 3:7])) + jacobian_b[:, :3, :] = torch.bmm(root_rot_matrix, jacobian_b[:, :3, :]) + jacobian_b[:, 3:, :] = torch.bmm(root_rot_matrix, jacobian_b[:, 3:, :]) + + # Compute current pose of the end-effector + root_pose_w = robot.data.root_state_w[:, 0:7] + ee_pose_w = robot.data.body_state_w[:, ee_frame_idx, 0:7] + ee_pos_b, ee_quat_b = 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 = torch.cat([ee_pos_b, ee_quat_b], dim=-1) + + # Compute the current velocity of the end-effector + ee_vel_w = robot.data.body_vel_w[:, ee_frame_idx, :] # Extract end-effector velocity in the world frame + root_vel_w = robot.data.root_vel_w # Extract root velocity in the world frame + relative_vel_w = ee_vel_w - root_vel_w # Compute the relative velocity in the world frame + ee_lin_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 0:3]) # From world to root frame + ee_ang_vel_b = quat_rotate_inverse(robot.data.root_quat_w, relative_vel_w[:, 3:6]) + ee_vel_b = torch.cat([ee_lin_vel_b, ee_ang_vel_b], dim=-1) + + # Calculate the contact force + ee_force_w = torch.zeros(scene.num_envs, 3, device=sim.device) + sim_dt = sim.get_physics_dt() + contact_forces.update(sim_dt) # update contact sensor + # Calculate the contact force by averaging over last four time steps (i.e., to smoothen) and + # taking the max of three surfaces as only one should be the contact of interest + ee_force_w, _ = torch.max(torch.mean(contact_forces.data.net_forces_w_history, dim=1), dim=1) + + # This is a simplification, only for the sake of testing. + ee_force_b = ee_force_w + + return jacobian_b, mass_matrix, gravity, ee_pose_b, ee_vel_b, root_pose_w, ee_pose_w, ee_force_b + + +# Update the target commands +def update_target( + sim: sim_utils.SimulationContext, + scene: InteractiveScene, + osc: OperationalSpaceController, + root_pose_w: torch.tensor, + ee_target_set: torch.tensor, + current_goal_idx: int, +): + """Update the targets for the operational space controller. + + Args: + sim: (SimulationContext) Simulation context. + scene: (InteractiveScene) Interactive scene. + osc: (OperationalSpaceController) Operational space controller. + root_pose_w: (torch.tensor) Root pose in the world frame. + ee_target_set: (torch.tensor) End-effector target set. + current_goal_idx: (int) Current goal index. + + Returns: + command (torch.tensor): Updated target command. + ee_target_pose_b (torch.tensor): Updated target pose in the body frame. + ee_target_pose_w (torch.tensor): Updated target pose in the world frame. + next_goal_idx (int): Next goal index. + + Raises: + ValueError: Undefined target_type. + """ + + # update the ee desired command + command = torch.zeros(scene.num_envs, osc.action_dim, device=sim.device) + command[:] = ee_target_set[current_goal_idx] + + # update the ee desired pose + ee_target_pose_b = torch.zeros(scene.num_envs, 7, device=sim.device) + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + ee_target_pose_b[:] = command[:, :7] + elif target_type == "wrench_abs": + pass # ee_target_pose_b could stay at the root frame for force control, what matters is ee_target_b + else: + raise ValueError("Undefined target_type within update_target().") + + # update the target desired pose in world frame (for marker) + ee_target_pos_w, ee_target_quat_w = combine_frame_transforms( + root_pose_w[:, 0:3], root_pose_w[:, 3:7], ee_target_pose_b[:, 0:3], ee_target_pose_b[:, 3:7] + ) + ee_target_pose_w = torch.cat([ee_target_pos_w, ee_target_quat_w], dim=-1) + + next_goal_idx = (current_goal_idx + 1) % len(ee_target_set) + + return command, ee_target_pose_b, ee_target_pose_w, next_goal_idx + + +# Convert the target commands to the task frame +def convert_to_task_frame(osc: OperationalSpaceController, command: torch.tensor, ee_target_pose_b: torch.tensor): + """Converts the target commands to the task frame. + + Args: + osc: OperationalSpaceController object. + command: Command to be converted. + ee_target_pose_b: Target pose in the body frame. + + Returns: + command (torch.tensor): Target command in the task frame. + task_frame_pose_b (torch.tensor): Target pose in the task frame. + + Raises: + ValueError: Undefined target_type. + """ + command = command.clone() + task_frame_pose_b = ee_target_pose_b.clone() + + cmd_idx = 0 + for target_type in osc.cfg.target_types: + if target_type == "pose_abs": + command[:, :3], command[:, 3:7] = subtract_frame_transforms( + task_frame_pose_b[:, :3], task_frame_pose_b[:, 3:], command[:, :3], command[:, 3:7] + ) + cmd_idx += 7 + elif target_type == "wrench_abs": + # These are already defined in target frame for ee_goal_wrench_set_tilted_task (since it is + # easier), so not transforming + cmd_idx += 6 + else: + raise ValueError("Undefined target_type within _convert_to_task_frame().") + + return command, task_frame_pose_b + + +def main(): + """Main function.""" + # Load kit helper + sim_cfg = sim_utils.SimulationCfg(dt=0.01, device=args_cli.device) + sim = sim_utils.SimulationContext(sim_cfg) + # Set main camera + sim.set_camera_view([2.5, 2.5, 2.5], [0.0, 0.0, 0.0]) + # Design scene + scene_cfg = SceneCfg(num_envs=args_cli.num_envs, env_spacing=2.0) + scene = InteractiveScene(scene_cfg) + # Play the simulator + sim.reset() + # Now we are ready! + print("[INFO]: Setup complete...") + # Run the simulator + run_simulator(sim, scene) + + +if __name__ == "__main__": + # run the main function + main() + # close sim app + simulation_app.close() diff --git a/tools/per_test_timeouts.py b/tools/per_test_timeouts.py index 4310c632c7..a82b49daac 100644 --- a/tools/per_test_timeouts.py +++ b/tools/per_test_timeouts.py @@ -18,4 +18,5 @@ "test_rsl_rl_wrapper.py": 200, "test_sb3_wrapper.py": 200, "test_skrl_wrapper.py": 200, + "test_operational_space.py": 200, }