==============
|package_name|
==============

:ir_github:`<isaac_ros_cumotion> <isaac_ros_cumotion_moveit>`

Quickstart
----------

Set Up Development Environment
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. include:: /_snippets/set_up_dev_env.rst

Install |package_name| and Examples
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

.. include:: _snippets/install.rst


Set Up MoveIt 2 with cuMotion
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Complete the following steps for your selected robot.

.. tabs::

   .. tab:: Universal Robot (UR)

      #. Launch the robot driver with the following command, substituting the robot's IP address for ``<ROBOT_IP_ADDRESS>``, and replace ``ur10e`` with the correct
         robot model (both here and in the steps below) if using a UR robot other than UR10e.

         .. code:: bash

            ros2 launch ur_robot_driver ur_control.launch.py ur_type:=ur10e robot_ip:=<ROBOT_IP_ADDRESS> launch_rviz:=false

      #. Open a new terminal inside the Isaac ROS environment and generate a URDF for the UR10e from the corresponding xacro:

         .. code:: bash

            isaac-ros activate

         .. code:: bash

            mkdir -p ${ISAAC_ROS_WS}/isaac_ros_assets/urdf && \
                xacro -o ${ISAAC_ROS_WS}/isaac_ros_assets/urdf/ur10e.urdf /opt/ros/:ir_ros_distro:/share/ur_description/urdf/ur.urdf.xacro ur_type:=ur10e name:=ur10e

      #. Open another terminal inside the Isaac ROS environment and launch MoveIt (including RViz).

         .. code:: bash
            
            isaac-ros activate
            
         .. code:: bash

            ros2 launch isaac_ros_cumotion_examples ur.launch.py ur_type:=ur10e

      #. Open another terminal inside the Isaac ROS environment and run the cuMotion planner node.

         .. code:: bash
            
            isaac-ros activate
            
         .. code:: bash

            ros2 launch isaac_ros_cumotion isaac_ros_cumotion.launch.py \
            cumotion_planner.robot:=$(ros2 pkg prefix --share isaac_ros_cumotion_robot_description)/xrdf/ur10e.xrdf \
            cumotion_planner.urdf_path:=${ISAAC_ROS_WS}/isaac_ros_assets/urdf/ur10e.urdf

      #. Open another terminal inside the Isaac ROS environment and enable the ``allow_nonzero_velocity_at_trajectory_end`` parameter for the ``scaled_joint_trajectory_controller``.

         .. code:: bash
            
            isaac-ros activate
            
         .. code:: bash

            ros2 param set /scaled_joint_trajectory_controller allow_nonzero_velocity_at_trajectory_end true

      #. On the UR teach pendant: Press **play** to enable the robot if it is not already enabled.


   .. tab:: Franka

      #. Follow the instructions in the `franka_ros2 repository <https://github.com/frankarobotics/franka_ros2>`__ to set up the Franka ROS2 package and the Franka driver.

      #. Launch MoveIt (including RViz).

         .. code:: bash

            ros2 launch isaac_ros_cumotion_examples franka.launch.py

      #. In a separate terminal, run the cuMotion planner node.  Remember to first ``source install/setup.bash`` if package was built from source.

         .. code:: bash

            isaac-ros activate
            
         .. code:: bash

            ros2 launch isaac_ros_cumotion isaac_ros_cumotion.launch.py \
            cumotion_planner.robot:=$(ros2 pkg prefix --share isaac_ros_cumotion_robot_description)/xrdf/franka.xrdf \
            cumotion_planner.urdf_path:=$(ros2 pkg prefix --share moveit_resources_panda_description)/urdf/panda.urdf


   .. tab:: Another Robot

      #. Create a MoveIt configuration for your robot using the MoveIt setup assistant.

      #. 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.  Follow the
         :ref:`Creating an XRDF file for a Custom Manipulator <concepts/manipulation/cumotion_moveit/tutorial_custom_manipulator:Creating an XRDF file for a Custom Manipulator>` section of the
         :doc:`Custom Manipulator tutorial </concepts/manipulation/cumotion_moveit/tutorial_custom_manipulator>`.

      #. Copy cuMotion's planning configuration file to the MoveIt ``config`` directory for your robot:

         .. code:: bash

            cp ${ISAAC_ROS_WS}/src/isaac_ros_cumotion/isaac_ros_cumotion_moveit/config/isaac_ros_cumotion_planning.yaml \
                PATH/TO/ROBOT_moveit_config/config/

      #. In the MoveIt launch file for your robot (e.g., ``<ROBOT>_moveit_config/launch/<ROBOT>_moveit.launch.py``),
         add the following entry to the ``parameters`` list of the ``Node()`` constructor for ``move_group_node``:

         .. code:: python

            { "planning_pipelines": ["ompl", "isaac_ros_cumotion"] }

      #. Install any additional required packages for your robot, and run ``source install/setup.bash`` if building from source.

      #. Open another terminal inside the Isaac ROS environment and launch MoveIt (including RViz) using a suitable launch file for your robot, e.g.:

         .. code:: bash
            
            isaac-ros activate
            
         .. code:: bash

            ros2 launch ROBOT_moveit_config ROBOT_moveit.launch.py

      #. Open another terminal inside the Isaac ROS environment and run the cuMotion planner node.  Remember to first run ``source install/setup.bash`` if package was built from source.

         .. code:: bash

            isaac-ros activate
            
         .. code:: bash

            ros2 launch isaac_ros_cumotion isaac_ros_cumotion.launch.py \
            cumotion_planner.robot:=/path/to/ROBOT.xrdf \
            cumotion_planner.urdf_path:=/path/to/ROBOT.urdf


Using the cuMotion Planner in MoveIt
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~

Enable cuMotion by ensuring that ``isaac_ros_cumotion`` and ``cuMotion`` are selected within the **"Planning Library"** pane within the **"Context"** tab in
the bottom left corner of the RViz window.

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/moveit_context.png>`
    :align: center

Then select a target pose for the robot end effector, and click the **"Plan"** button in the **"Planning"** tab.
For a demonstration of collision-aware planning, first add one or more obstacles in the **"Scene Objects"** tab.

.. note::

  The **"Execute"** and **"Plan & Execute"** buttons in RViz will only work if a physical robot is connected or if the
  robot is emulated in the corresponding robot driver or via an external simulator.  The MoveIt configuration for Franka
  includes such emulation via a "fake hardware" interface, but the same is not true for UR, which would require an
  external simulator such as URSim.

.. figure:: :ir_lfs:`<resources/isaac_ros_docs/repositories_and_packages/isaac_ros_cumotion/isaac_ros_cumotion_moveit/moveit_planning.gif>`
    :align: center

.. warning::

  In normal operation, only a single MoveIt ``move_group`` node should be running at a time.  If the node fails to
  exit cleanly, however, it may result in two nodes running when the graph is next launched.  RViz then connects
  to both instances, which results in both requesting a plan from the cuMotion planner node via a ``MoveGroup`` action
  at nearly the same time.  The second to arrive preempts the first without waiting for in-flight CUDA operations to
  complete, possibly resulting in "CUDA invalid" errors.

  If such errors are observed, please ensure that only a single ``move_group`` node is running.  The cuMotion planner
  node will be made more robust in a future release.

Try More Examples
-----------------

To continue your exploration, check out the following suggested examples:

* :doc:`Tutorial for integrating custom manipulators with cuMotion </concepts/manipulation/cumotion_moveit/tutorial_custom_manipulator>`
* :doc:`Tutorial for cuMotion MoveIt plugin with Isaac Sim </concepts/manipulation/cumotion_moveit/tutorial_isaac_sim>`
* :doc:`Tutorial for obstacle avoidance and object following using cuMotion with perception </reference_workflows/isaac_for_manipulation/tutorials/tutorial_e2e>`
* :doc:`Tutorial for pick and place using cuMotion with perception </reference_workflows/isaac_for_manipulation/tutorials/tutorial_pick_and_place>`


Troubleshooting
---------------

Isaac ROS Troubleshooting
~~~~~~~~~~~~~~~~~~~~~~~~~

For solutions to problems with Isaac ROS, see :doc:`troubleshooting </troubleshooting/index>`.

.. |package_name| replace:: ``isaac_ros_cumotion_moveit``