==========================================================
Tutorial for Integrating Custom Manipulators with cuMotion
==========================================================

Overview
--------

This tutorial walks through the process of integrating a custom manipulator with cuMotion. Users using
a Franka, UR5e, or UR10e manipulator do not need to follow this tutorial and can directly proceed to
the :doc:`cuMotion quickstart guide </repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/index>`.
Users with a Universal Robot (UR) manipulator other than UR5e or UR10e will need to create an
:ref:`XRDF <concepts/manipulation/xrdf:Extended Robot Description Format (XRDF)>` file for their robot but
can skip the other sections of this tutorial; see `Creating an XRDF file for a Custom Manipulator`_.
Users with other manipulators must follow the full tutorial.

This tutorial has been tested with Isaac Sim 4.2.


Prerequisites
-------------

Before starting this tutorial, you must satisfy the following prerequisites:

* Your manipulator has a "MoveIt config" ROS 2 package.
* You have Isaac Sim 4.0 or later installed.


Set Up Development Environment
------------------------------

#. Set up your development environment by following the instructions in :doc:`Compute Setup </getting_started/hardware_setup/compute/index>` and :doc:`Getting Started </getting_started/dev_env_setup>`.

#. Clone ``isaac_ros_common`` under ``${ISAAC_ROS_WS}/src``.

   .. code:: bash

      cd ${ISAAC_ROS_WS}/src && \
         git clone :ir_clone:`<isaac_ros_common>`


Creating USD for a Custom Manipulator
-------------------------------------

`Isaac Sim <https://developer.nvidia.com/isaac-sim>`_ provides access to an asset library including many robot arms.
If your robot isn't in that library, you will need to create your own robot USD as follows:

#. Clone your robot's MoveIt config ROS 2 package in ``${ISAAC_ROS_WS}/src``. The package name is often of the form
   ``<ROBOT>_moveit_config`` or ``<ROBOT>_description`` and may be provided by your robot manufacturer or by the
   `ROS-Industrial <https://rosindustrial.org/>`_ project.

#. Find or generate a URDF file for your robot based on the files in your robot's MoveIt config ROS 2 package.

#. Convert the URDF to USD using Isaac Sim's
   `URDF Importer <https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_advanced_import_urdf.html>`_
   extension.

#. The USD prim created by the URDF Importer tool will have its ``root_joint`` set as the articulation root.  In the
   ``Property`` tab, navigate to the ``Physics`` drop-down and delete the ``Articulation Root`` from the ``root_joint``.

#. Right-click on the robot's Xform and select :menuselection:`Add --> Physics --> Articulation Root`.

#. Save the file as ``<ROBOT>.usd``.


Creating an XRDF file for a Custom Manipulator
----------------------------------------------

.. note::

   If your robot is a Franka (original model once called "Franka Panda"), a UR10e, or a UR5e, this section may be
   skipped, since preconfigured XRDF files for those robots are included with Isaac ROS cuMotion.

cuMotion requires a URDF as well as an :ref:`XRDF <concepts/manipulation/xrdf:Extended Robot Description Format (XRDF)>`
file for the robot.  URDFs are available for most common robot arms, but it may be necessary to create an XRDF file.
This can be done by hand (e.g., using an
:ir_repo:`existing XRDF file <isaac_ros_cumotion> <isaac_ros_cumotion_robot_description/xrdf/ur10e.xrdf>`
as a reference) or by using the
`Robot Description Editor <https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_motion_generation_robot_description_editor.html>`_
in Isaac Sim 4.0 or later, as follows.

#. If Isaac Sim's robot assets folder doesn't have the USD for your robot, follow the `Creating USD for a Custom Manipulator`_ section above to create a USD file.

#. Follow the `Robot Description Editor <https://docs.omniverse.nvidia.com/isaacsim/latest/advanced_tutorials/tutorial_motion_generation_robot_description_editor.html>`_
   instructions to generate the XRDF file using Isaac Sim. Export the XRDF file as ``<ROBOT>.xrdf``.


Modifying a Robot's MoveIt Configuration
----------------------------------------

Follow the instructions in this section if you want to use cuMotion with Isaac Sim and your manipulator is not a
Franka or UR.

To run cuMotion with Isaac Sim, you will need to make slight modifications to your manipulator's MoveIt configuration
files (often found in a ``<ROBOT>_moveit_config`` or ``<ROBOT>_description`` package).  Create a ``<ROBOT>_config``
directory in your ROS workspace to store the files created below.

When creating the files below, the
:ir_repo:`configuration files <isaac_ros_cumotion> <isaac_ros_cumotion_examples/ur_config>` and
:ir_repo:`launch file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py>`
for the UR10e robot may be used as a reference.

.. note::

    In all file names below, replace ``<ROBOT>`` with your robot's name (e.g., ``ur3e``).


``ros2_controllers.yaml``
~~~~~~~~~~~~~~~~~~~~~~~~~

The ``ros2_controllers.yaml`` file is used to configure the ``ros2_control`` controller manager with all possible
controllers for your robot (e.g., for the combination of the arm and gripper, the arm alone, the gripper alone, etc.).

#. Create a copy of the ``controllers.yaml`` file from your robot's MoveIt configuration package.

#. Make minor modifications so that it looks like the provided
   :ir_repo:`sample file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/ur_config/ros2_controllers.yaml>`
   for the UR10e robot.


``<ROBOT>.ros2_control.xacro``
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

The ``<ROBOT>.ros2_control.xacro`` file configures the initial position and position limits for each robot joint.

#. Create a copy of the ``ros2_control.xacro`` file from your robot's MoveIt configuration package.  If such a file
   doesn't exist, create one by copying the contents of the ``<ros2_control>`` section of your URDF file.

#. Remove the ``<ros2_control>`` section from your URDF file, if present.

#. Replace the ``<hardware>`` subsection with the code below to properly set up topic-based ROS 2 control; the
   ROS topics are those expected by the provided Isaac Sim USD scene.  The
   :ir_repo:`sample file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/ur_config/ur.ros2_control.xacro>`
   for the UR10e robot may be used as a reference.

.. code:: xml

    <hardware>
      <plugin>topic_based_ros2_control/TopicBasedSystem</plugin>
      <param name="joint_commands_topic">/isaac_joint_commands</param>
      <param name="joint_states_topic">/isaac_joint_states</param>
    </hardware>


``<ROBOT>.urdf.xacro``
~~~~~~~~~~~~~~~~~~~~~~

The ``<ROBOT>.urdf.xacro`` file contains the robot description and should include (e.g., via a ``<xacro::include>`` tag)
both the xacro required to generate the robot URDF and the robot's ``ros2_control`` configuration (e.g.,
``<ROBOT>.ros2_control.xacro``). If a ``<ROBOT>.urdf.xacro`` file was provided with your robot description or MoveIt
configuration package, it may contain only the former.  In this case, a wrapper ``<ROBOT>.urdf.xacro`` file should be
created.  In the following, the
:ir_repo:`sample file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro>`
for the UR10e robot is used as a reference.

#. Create a copy of the provided
   :ir_repo:`sample xacro file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/ur_config/ur.urdf.xacro>`.

#. Replace ``$(arg ur_type)`` in the following line with your robot name:

   .. code:: xml

      <robot xmlns:xacro="http://www.ros.org/wiki/xacro" name="$(arg ur_type)">

#. Replace the following lines with any needed to set arguments required by your URDF or ``ros2_control`` xacro files:

   .. code:: xml

       <xacro:arg name="ur_type" default="ur"/>
       <xacro:arg name="use_fake_hardware" default="true" />
       <xacro:arg name="name" default="ur10e" />

       <!-- Do not add ROS2 control from default UR package, add TopicBasedROS2 Control -->
       <xacro:arg name="generate_ros2_control_tag" default="false" />

#. Modify the following line so it imports your URDF xacro file:

   .. code:: xml

      <xacro:include filename="$(find isaac_ros_cumotion_examples)/ur_config/ur_sim.urdf.xacro" />

#. Modify the following line so it imports your ``ros2_control`` xacro file:

   .. code:: xml

      <xacro:include filename="ur.ros2_control.xacro" />

#. Remove the following line or adjust as needed:

   .. code:: xml

      <xacro:ur_ros2_control name="URFakeSystem" tf_prefix="" />


``moveit_config.rviz``
~~~~~~~~~~~~~~~~~~~~~~

The ``moveit_config.rviz`` file configures an RViz window for use with cuMotion and MoveIt. Your MoveIt config package
might already come with such a file. There is no need to follow these instructions unless that file doesn't work as
intended.

#. Create a copy of the provided
   :ir_repo:`sample RViz file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/rviz/ur_moveit_config.rviz>`.

#. There are six occurrences of each robot link name. You can find them by searching for ``base_link``. Replace them
   with lists of links specific to your robot.


``<ROBOT>.launch.py``
~~~~~~~~~~~~~~~~~~~~~

The ``<ROBOT>.launch.py`` file is a ROS 2 launch file that runs everything necessary to control your robot.
The following instructions will result in a launch file that is also suitable for controlling a simulated robot in
Isaac Sim.

#. Create a copy of the provided
   :ir_repo:`sample launch file <isaac_ros_cumotion> <isaac_ros_cumotion_examples/launch/ur_isaac_sim.launch.py>`.

#. Update ``generate_launch_description()`` to declare any required launch arguments, removing the following:

   .. code:: python

      DeclareLaunchArgument(
          'ur_type',
          default_value='ur10e',
          description='UR robot type',
          choices=['ur3', 'ur3e', 'ur5', 'ur5e', 'ur10', 'ur10e', 'ur16e', 'ur20', 'ur30']
      )

#. In ``launch_setup()``, remove the definition of ``ur_type`` and replace any references thereto.

#. In ``launch_setup()``, set the ``package_name`` keyword argument of the ``MoveItConfigsBuilder()`` constructor
   to the name of your robot's MoveIt configuration package, replacing ``ur_moveit_config``.

   .. code:: python

      MoveItConfigsBuilder(ur_type, package_name='ur_moveit_config')

#. In ``launch_setup()``, replace the following lines to set the file path to the ``<ROBOT>.urdf.xacro`` file created
   earlier in this tutorial:

   .. code:: python

      file_path=os.path.join(
          get_package_share_directory('isaac_ros_cumotion_examples'),
          'ur_config',
          'ur.urdf.xacro'
      ),

#. In ``launch_setup()``, remove the following argument or add your own mappings as needed.

   .. code:: python

      mappings={'ur_type': ur_type}

#. In ``launch_setup()``, modify the file paths and mappings as needed.  Note that file paths are relative to the root
   of your MoveIt configuration package.

   .. code:: python

      .robot_description_semantic(file_path='srdf/ur.srdf.xacro', mappings={'name': 'ur'})
      .robot_description_kinematics(file_path='config/kinematics.yaml')
      .trajectory_execution(file_path='config/controllers.yaml')

#. In ``launch_setup()``, the following customizations are specific to the MoveIt configuration package for UR robots
   (``ur_moveit_config``) and may either be removed or customized to suit the configuration files for your robot:

   .. code:: python

      # the workarounds below are based on the following ur_moveit_config and ur_description versions
      # ur_moveit_config: github.com/UniversalRobots/Universal_Robots_ROS2_Driver/tree/2.2.14
      # ur_description: github.com/UniversalRobots/Universal_Robots_ROS2_Description/tree/2.1.5

      moveit_config.trajectory_execution = {
          'moveit_simple_controller_manager': moveit_config.trajectory_execution
      }
      del moveit_config.trajectory_execution['moveit_simple_controller_manager'][
          'moveit_manage_controllers'
      ]
      moveit_config.trajectory_execution['moveit_manage_controllers'] = True
      moveit_config.trajectory_execution['trajectory_execution'] = {
          'allowed_execution_duration_scaling': 1.2,
          'allowed_goal_duration_margin': 0.5,
          'allowed_start_tolerance': 0.01
      }
      moveit_config.trajectory_execution['moveit_controller_manager'] = (
          'moveit_simple_controller_manager/MoveItSimpleControllerManager'
      )

      moveit_config.robot_description_kinematics['robot_description_kinematics']['ur_manipulator'] \
          = moveit_config.robot_description_kinematics['robot_description_kinematics']['/**'][
              'ros__parameters']['robot_description_kinematics']['ur_manipulator']
      del moveit_config.robot_description_kinematics['robot_description_kinematics']['/**']

      moveit_config.joint_limits['robot_description_planning'] = load_yaml(os.path.join(
          get_package_share_directory('ur_description'),
          'config',
          ur_type,
          'joint_limits.yaml'
      ))

      # add limits from ur_moveit_config joint_limits.yaml to limits from ur_description
      for joint in moveit_config.joint_limits['robot_description_planning']['joint_limits']:
          moveit_config.joint_limits['robot_description_planning']['joint_limits'][joint][
              'has_acceleration_limits'
          ] = True
          moveit_config.joint_limits['robot_description_planning']['joint_limits'][joint][
              'max_acceleration'
          ] = 5.0

#. In ``launch_setup()``, replace the following lines to set ``rviz_config_file`` to the correct path for your RViz
   configuration file:

   .. code:: python

      rviz_config_file = os.path.join(
          get_package_share_directory('isaac_ros_cumotion_examples'),
          'rviz',
          'ur_moveit_config.rviz',
      )

#. In the following ``Node()`` constructor, replace ``'base_link'`` with the name of your robot's base link:

   .. code:: python

      world2robot_tf_node = Node(
          package='tf2_ros',
          executable='static_transform_publisher',
          name='static_transform_publisher',
          output='log',
          arguments=['--frame-id', 'world', '--child-frame-id', 'base_link'],
      )

#. In ``launch_setup()``, replace the following lines to set ``ros2_controllers_path`` to the correct path for your
   ``ros2_controllers.yaml`` file:

   .. code:: python

      ros2_controllers_path = os.path.join(
           get_package_share_directory('isaac_ros_cumotion_examples'),
           'ur_config',
           'ros2_controllers.yaml'
      )

#. In ``launch_setup()``, modify the following lines as needed based on the controllers in your ``ros2_controllers.yaml``
   file:

   .. code:: python

      scaled_joint_trajectory_controller_spawner = Node(
          package='controller_manager',
          executable='spawner',
          arguments=['scaled_joint_trajectory_controller', '-c', '/controller_manager'],
      )

      joint_trajectory_controller_spawner = Node(
          package='controller_manager',
          executable='spawner',
          arguments=['joint_trajectory_controller', '-c', '/controller_manager'],
      )


Troubleshooting
~~~~~~~~~~~~~~~

* Ensure that all link and joint names in ``ros2_controller.yaml``, ``moveit_config.rviz``,
  ``<ROBOT>.ros2_control.xacro``, and ``<ROBOT>.xrdf`` are consistent.
* Ensure that the group name in your SRDF file is the same as the value produced when evaluating
  ``moveit_config.robot_description_kinematics['robot_description_kinematics']`` in ``<ROBOT>.launch.py``.
* Remove the ``<ros2_control>`` section from the URDF file if such a section exists.