pick_ik Tutorial (#698)

* Document pick_ik installation and basic usage

* Add a note about trying pick_ik

* Shorten sentences

* Move pick_ik tutorial to How-To section

* Fix formatting issue

* Change 'ROS2' to 'ROS 2'

Co-authored-by: Sebastian Castro <[email protected]>

* Include pick_ik tutorial in moveit2_tutorials build

* Add files to launch a demo configured with pick_ik

* Add instructions to launch a demo with pick_ik

* Fix formatting

* Add links to pick_ik and bio_ik github repos

* Fix a typo

Co-authored-by: Stephanie Eng <[email protected]>

* Link to the kinematics_pick_ik.yaml file

Co-authored-by: Stephanie Eng <[email protected]>

* Add full link to the pick_ik_parameters.yaml file

Co-authored-by: Stephanie Eng <[email protected]>

* Add pick_ik to the dependencies

* Add comments explaining configuration parameters

* Correct language errors

Co-authored-by: Sebastian Castro <[email protected]>

* Include the position_scale parameter

* Improve minimal_displacement_weight comment

* Add link to pick_ik in the first mention

---------

Co-authored-by: Sebastian Castro <[email protected]>
Co-authored-by: Stephanie Eng <[email protected]>

CMakeLists.txt

@@ -71,6 +71,7 @@ add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner)
 add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
 add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
 add_subdirectory(doc/tutorials/quickstart_in_rviz)
+add_subdirectory(doc/how_to_guides/pick_ik)
 
 ament_export_dependencies(
   ${THIS_PACKAGE_INCLUDE_DEPENDS}

doc/how_to_guides/how_to_guides.rst

@@ -20,6 +20,7 @@ Configuring and Using MoveIt
    controller_teleoperation/controller_teleoperation
    persistent_scenes_and_states/persistent_scenes_and_states
    isaac_panda/isaac_panda_tutorial
+   pick_ik/pick_ik_tutorial
 
 Developing and Documenting MoveIt
 ---------------------------------

doc/how_to_guides/pick_ik/CMakeLists.txt

@@ -0,0 +1,6 @@
+install(DIRECTORY launch
+  DESTINATION share/${PROJECT_NAME}
+)
+install(DIRECTORY config
+  DESTINATION share/${PROJECT_NAME}
+)

doc/how_to_guides/pick_ik/config/kinematics_pick_ik.yaml

@@ -0,0 +1,14 @@
+
+# Configure pick_ik as the solver to use with panda_arm
+panda_arm:
+  kinematics_solver: pick_ik/PickIkPlugin  # The IK solver plugin to use
+  kinematics_solver_timeout: 0.05  # Maximum duration in seconds the solver is allowed to run per attempt
+  kinematics_solver_attempts: 3  # Maximum number of solver attempts to find a solution
+  mode: global  # Solver mode 'global' enables evolutionary algorithm with gradient descent, and 'local' uses only gradient descent
+  position_scale: 1.0  # A scale factor for the position cost, setting this to 0.0 means rotation-only IK
+  rotation_scale: 0.5  # A scale factor for the orientation cost, setting this to 0.0 means position-only IK
+  position_threshold: 0.001  # Threshold for position difference in meters to consider a solution valid
+  orientation_threshold: 0.01  # Threshold for orientation difference in radians to consider a solution valid
+  cost_threshold: 0.001  # Maximum cost function value for a valid solution (all cost functions must return a value lower than this)
+  minimal_displacement_weight: 0.0  # Weight for joint angle difference cost function. Leave it low or zero for far goals, and make higher for interpolation or jogging
+  gd_step_size: 0.0001  # Step size for the gradient descent

doc/how_to_guides/pick_ik/launch/demo_pick_ik.launch.py

@@ -0,0 +1,145 @@
+import os
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument, OpaqueFunction
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch.conditions import IfCondition, UnlessCondition
+from launch_ros.actions import Node
+from launch_ros.substitutions import FindPackageShare
+from launch.actions import ExecuteProcess
+from ament_index_python.packages import get_package_share_directory
+from moveit_configs_utils import MoveItConfigsBuilder
+
+
+def generate_launch_description():
+
+    declared_arguments = []
+    declared_arguments.append(
+        DeclareLaunchArgument(
+            "rviz_config",
+            default_value="panda_moveit_config_demo.rviz",
+            description="RViz configuration file",
+        )
+    )
+
+    return LaunchDescription(
+        declared_arguments + [OpaqueFunction(function=launch_setup)]
+    )
+
+
+def launch_setup(context, *args, **kwargs):
+
+    moveit_config = (
+        MoveItConfigsBuilder("moveit_resources_panda")
+        .robot_description(file_path="config/panda.urdf.xacro")
+        .trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
+        .planning_scene_monitor(
+            publish_robot_description=True, publish_robot_description_semantic=True
+        )
+        .planning_pipelines(
+            pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
+        )
+        .robot_description_kinematics(
+            file_path=os.path.join(
+                get_package_share_directory("moveit2_tutorials"),
+                "config",
+                "kinematics_pick_ik.yaml",
+            )
+        )
+        .to_moveit_configs()
+    )
+
+    # Start the actual move_group node/action server
+    run_move_group_node = Node(
+        package="moveit_ros_move_group",
+        executable="move_group",
+        output="screen",
+        parameters=[moveit_config.to_dict()],
+    )
+
+    rviz_base = LaunchConfiguration("rviz_config")
+    rviz_config = PathJoinSubstitution(
+        [FindPackageShare("moveit2_tutorials"), "launch", rviz_base]
+    )
+
+    # RViz
+    rviz_node = Node(
+        package="rviz2",
+        executable="rviz2",
+        name="rviz2",
+        output="log",
+        arguments=["-d", rviz_config],
+        parameters=[
+            moveit_config.robot_description,
+            moveit_config.robot_description_semantic,
+            moveit_config.robot_description_kinematics,
+            moveit_config.planning_pipelines,
+            moveit_config.joint_limits,
+        ],
+    )
+
+    # Static TF
+    static_tf = Node(
+        package="tf2_ros",
+        executable="static_transform_publisher",
+        name="static_transform_publisher",
+        output="log",
+        arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
+    )
+
+    # Publish TF
+    robot_state_publisher = Node(
+        package="robot_state_publisher",
+        executable="robot_state_publisher",
+        name="robot_state_publisher",
+        output="both",
+        parameters=[moveit_config.robot_description],
+    )
+
+    # ros2_control using FakeSystem as hardware
+    ros2_controllers_path = os.path.join(
+        get_package_share_directory("moveit_resources_panda_moveit_config"),
+        "config",
+        "ros2_controllers.yaml",
+    )
+    ros2_control_node = Node(
+        package="controller_manager",
+        executable="ros2_control_node",
+        parameters=[moveit_config.robot_description, ros2_controllers_path],
+        output="both",
+    )
+
+    joint_state_broadcaster_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=[
+            "joint_state_broadcaster",
+            "--controller-manager-timeout",
+            "300",
+            "--controller-manager",
+            "/controller_manager",
+        ],
+    )
+
+    arm_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_arm_controller", "-c", "/controller_manager"],
+    )
+
+    hand_controller_spawner = Node(
+        package="controller_manager",
+        executable="spawner",
+        arguments=["panda_hand_controller", "-c", "/controller_manager"],
+    )
+    nodes_to_start = [
+        rviz_node,
+        static_tf,
+        robot_state_publisher,
+        run_move_group_node,
+        ros2_control_node,
+        joint_state_broadcaster_spawner,
+        arm_controller_spawner,
+        hand_controller_spawner,
+    ]
+
+    return nodes_to_start

doc/how_to_guides/pick_ik/pick_ik_tutorial.rst

@@ -0,0 +1,144 @@
+pick_ik Kinematics Solver
+=========================
+
+`pick_ik <https://github.com/PickNikRobotics/pick_ik>`_ is an inverse kinematics (IK) solver compatible with MoveIt 2,
+developed by PickNik Robotics. It is designed to provide robust and customizable IK solutions,
+offering a wide range of features.
+
+The solver in ``pick_ik`` is a reimplementation of `bio_ik <https://github.com/TAMS-Group/bio_ik>`_,
+integrating a global optimizer and a local optimizer. The global optimizer utilizes evolutionary algorithms to explore
+alternative solutions within the solution space and identify global optima. Building upon the results obtained by the global optimizer,
+the local optimizer applies gradient descent for iterative refinement of the solution. It takes a potential optimum solution provided
+by the global optimizer as input and aims to improve its accuracy and ultimately convergence to the most optimum solution.
+
+Getting Started
+---------------
+Before proceeding, please ensure that you have completed the steps outlined in the :doc:`Getting Started Guide </doc/tutorials/getting_started/getting_started>`.
+
+Additionally, it is required to have a MoveIt configuration package specifically tailored to your robot.
+This package can be created using the :doc:`MoveIt Setup Assistant </doc/examples/setup_assistant/setup_assistant_tutorial>`.
+
+Installation
+------------
+
+From binaries
+^^^^^^^^^^^^^
+Make sure your ROS 2 installation is sourced and run the following command ::
+
+    sudo apt install ros-$ROS_DISTRO-pick-ik
+
+From source
+^^^^^^^^^^^
+
+Create a colcon workspace. ::
+
+    export COLCON_WS=~/ws_moveit2/
+    mkdir -p $COLCON_WS/src
+
+Clone this repository in the src directory of your workspace. ::
+
+    cd $COLCON_WS/src
+    git clone -b main https://github.com/PickNikRobotics/pick_ik.git
+
+Set up colcon mixins. ::
+
+    sudo apt install python3-colcon-common-extensions
+    sudo apt install python3-colcon-mixin
+    colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
+    colcon mixin update default
+
+Build the workspace. ::
+
+    cd /path/to/your/workspace
+    colcon build --mixin release
+
+Usage
+-----
+
+Using pick_ik as a Kinematics Plugin
+^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
+
+You can use :doc:`MoveIt Setup Assistant </doc/examples/setup_assistant/setup_assistant_tutorial>` to create
+the configuration files for your robot to use it with MoveIt, or edit the ``kinematics.yaml`` file in your
+robot's config directory to use pick_ik as the IK solver. ::
+
+    panda_arm:
+        kinematics_solver: pick_ik/PickIkPlugin
+        kinematics_solver_timeout: 0.05
+        kinematics_solver_attempts: 3
+        mode: global
+        position_scale: 1.0
+        rotation_scale: 0.5
+        position_threshold: 0.001
+        orientation_threshold: 0.01
+        cost_threshold: 0.001
+        minimal_displacement_weight: 0.0
+        gd_step_size: 0.0001
+
+
+.. note::
+
+   You can launch a preconfigured demo with ``pick_ik`` using the following command:
+
+   .. code-block::
+
+      ros2 launch moveit2_tutorials demo_pick_ik.launch.py
+
+   The command starts a demo similar to the one in :doc:`MoveIt Quickstart in RViz </doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial>`
+   tutorial. However, this demo specifically utilizes the robot kinematics configuration file ``kinematics_pick_ik.yaml``
+   located at the ``moveit2_tutorials/doc/pick_ik/config`` directory :codedir:`here<how_to_guides/pick_ik/config/kinematics_pick_ik.yaml>`.
+
+Parameter Description
+^^^^^^^^^^^^^^^^^^^^^
+
+For an exhaustive list of parameters, refer to the `parameters YAML file <https://github.com/PickNikRobotics/pick_ik/blob/main/src/pick_ik_parameters.yaml>`__.
+
+Some key parameters you may want to start with are:
+
+- ``mode``: If you choose ``local``, this solver will only do local gradient descent; if you choose ``global``,
+  it will also enable the evolutionary algorithm. Using the global solver will be less performant, but if you're
+  having trouble getting out of local minima, this could help you. We recommend using ``local`` for things like
+  relative motion / Cartesian interpolation / endpoint jogging, and ``global`` if you need to solve for goals
+  with a far-away initial condition.
+
+- ``memetic_<property>``: All the properties that only kick in if you use the ``global`` solver.
+  The key one is ``memetic_num_threads``, as we have enabled the evolutionary algorithm to solve on multiple threads.
+
+- ``position_threshold`` / ``orientation_threshold``: Optimization succeeds only if the pose difference is less than
+  these thresholds in meters and radians respectively. A ``position_threshold`` of 0.001 would mean a 1 mm accuracy and
+  an ``orientation_threshold`` of 0.01 would mean a 0.01 radian accuracy.
+
+- ``cost_threshold``: This solver works by setting up cost functions based on how far away your pose is,
+  how much your joints move relative to the initial guess, and custom cost functions you can add.
+  Optimization succeeds only if the cost is less than ``cost_threshold``. Note that if you're adding custom cost functions,
+  you may want to set this threshold fairly high and rely on ``position_threshold`` and ``orientation_threshold`` to be your deciding factors,
+  whereas this is more of a guideline.
+
+- ``approximate_solution_position_threshold`` / ``approximate_solution_orientation_threshold``:
+  When using approximate IK solutions for applications such as endpoint servoing, ``pick_ik`` may sometimes return solutions
+  that are significantly far from the goal frame. To prevent issues with such jumps in solutions,
+  these parameters define maximum translational and rotation displacement.
+  We recommend setting this to values around a few centimeters and a few degrees for most applications.
+
+- ``position_scale``: If you want rotation-only IK, set this to 0.0. If you want to solve for a custom ``IKCostFn``
+  (which you provide in your ``setFromIK()`` call), set both ``position_scale`` and ``rotation_scale`` to 0.0.
+  You can also use any other value to weight the position goal; it's part of the cost function.
+  Note that any checks using ``position_threshold`` will be ignored if you use ``position_scale = 0.0``.
+
+
+- ``rotation_scale``: If you want position-only IK, set this to 0.0. If you want to treat position and orientation equally,
+  set this to 1.0. You can also use any value in between; it's part of the cost function. Note that any checks using ``orientation_threshold``
+  will be ignored if you use ``rotation_scale = 0.0``.
+
+- ``minimal_displacement_weight``: This is one of the standard cost functions that checks for the joint angle difference between
+  the initial guess and the solution. If you're solving for far-away goals, leave it to zero or it will hike up your cost function for no reason.
+  Have this to a small non-zero value (e.g., 0.001) if you're doing things like Cartesian interpolation along a path or endpoint jogging for servoing.
+
+You can test out this solver live in RViz, as this plugin uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_
+package to respond to parameter changes at every solve. This means that you can change values on the fly using the ROS 2 command-line interface, e.g.,
+
+.. code-block::
+
+    ros2 param set /rviz2 robot_description_kinematics.panda_arm.mode global
+
+    ros2 param set /rviz2 robot_description_kinematics.panda_arm.minimal_displacement_weight 0.001

package.xml

@@ -36,6 +36,7 @@
   <depend>tf2_geometry_msgs</depend>
   <depend>tf2_ros</depend>
   <depend>moveit_task_constructor_core</depend>
+  <depend>pick_ik</depend>
 
   <build_depend>eigen</build_depend>
   <build_depend>geometric_shapes</build_depend>