Isaac Grasp File
================

To execute a grasp on an identified object, it is useful to keep a reference file of known
high quality grasps and to choose a grasp from that file that works the best in context.  An
``isaac_grasp`` file is a YAML file that follows a human-readable format for storing pre-defined
grasps.

Isaac Grasp Specification
--------------------------

A single ``isaac_grasp`` file stores a list of grasps for a specific gripper and object pair.  The file uses the
following format:

.. code-block:: yaml
    :linenos:

    format: isaac_grasp
    format_version: 1.0

    # These reference frames come from a grasp authored in USD, but grasps authored elsewhere
    # will have alternative metadata.
    object_frame: /World/mug
    gripper_frame: /World/panda_hand

    grasps:
      "grasp_0":
        confidence: 1.0
        position: [-0.04346, 0.06759, 0.19895]
        orientation: {w: 0.00332, xyz: [0.98453, 0.16837, 0.04837]}
        cspace_position:
          panda_finger_joint1: 0.00943
        pregrasp_cspace_position:
          panda_finger_joint1: 0.04

A grasp is defined by the relative pose of the gripper with respect to the object. For this relative
pose to have meaning, a representative frame must be chosen for the gripper and object.
The ``gripper_frame`` and ``object_frame`` fields are a reference for human readers of what
frames were used when defining a grasp.

.. note:: Do not rely on these fields in code, because the context for the authored grasp may differ.

In the example YAML snippet above, the grasp was authored using USD objects and the corresponding
frames are USD paths.  In other contexts, the reference frames might correspond to the implicit
coordinate system of the object mesh and a frame from the robot URDF file.

.. note:: It is up to the consumer of an Isaac Grasp file to ensure that the gripper and object frames used to interpret a grasp exactly match the frames used when authoring or generating the grasp.

Each grasp in an ``isaac_grasp`` file has a unique (but otherwise arbitrary) name, e.g. ``"grasp_0"`` in the example above. The fields for a named
grasp are:

- **confidence**: A parameter between ``0.0`` and ``1.0`` describing the quality of a grasp.
- **position**: The translation of the gripper frame relative to the object frame.
- **orientation**: The orientation of the gripper frame relative to the object frame.
- **cspace_position**: A dictionary of joint positions for every joint that is used to control the gripper.
  These joint positions are the state of the gripper as it is actively grasping the object.
- **pregrasp_cspace_position**: A dictionary of joint positions for every joint that is used to control the gripper.
  These joint positions represent the open position of the gripper.

All together, a grasp may be applied, in practice, by moving the gripper to the correct relative position and orientation
while in the ``pregrasp_cspace_position``, then closing the gripper until the joints are in the ``cspace_position``.
If the object's position and orientation in the world frame of reference is given by :math:`T_o, R_o`, with
the ``position`` and ``orientation`` fields specifying relative transformation :math:`^oT_g, ^o\!\!R_g`
(that is, the translation and rotation of the gripper according to the object frame of reference),
the desired position of the gripper in the world frame :math:`T_g , R_g` is given by:

.. math::
    T_g = R_o \cdot {^oT_g + T_o} \\
    R_g = R_o \cdot {^o\!R_g}