From a5fa5f686f88bcf3c355adce1691000421f3d5f4 Mon Sep 17 00:00:00 2001 From: Sebastian Castro Date: Mon, 9 Oct 2023 11:59:46 -0400 Subject: [PATCH] Port TRAC-IK tutorial to ROS 2 --- .gitignore | 1 + doc/examples/examples.rst | 1 - doc/examples/trac_ik/trac_ik_tutorial.rst | 36 ---------------- doc/how_to_guides/how_to_guides.rst | 1 + .../trac_ik/trac_ik_tutorial.rst | 43 +++++++++++++++++++ 5 files changed, 45 insertions(+), 37 deletions(-) delete mode 100644 doc/examples/trac_ik/trac_ik_tutorial.rst create mode 100644 doc/how_to_guides/trac_ik/trac_ik_tutorial.rst diff --git a/.gitignore b/.gitignore index 112fa6b2f8..cae91a5a97 100644 --- a/.gitignore +++ b/.gitignore @@ -1,5 +1,6 @@ *~ *.pyc +doc/api/python_api/_autosummary/ doc/manifest.yaml doc/html build diff --git a/doc/examples/examples.rst b/doc/examples/examples.rst index 47c8a31b03..7f68fe8b2c 100644 --- a/doc/examples/examples.rst +++ b/doc/examples/examples.rst @@ -62,7 +62,6 @@ Before attempting to integrate a new robot with MoveIt 2, check whether your rob perception_pipeline/perception_pipeline_tutorial hand_eye_calibration/hand_eye_calibration_tutorial ikfast/ikfast_tutorial - trac_ik/trac_ik_tutorial Configuration ------------- diff --git a/doc/examples/trac_ik/trac_ik_tutorial.rst b/doc/examples/trac_ik/trac_ik_tutorial.rst deleted file mode 100644 index d8c88e34ef..0000000000 --- a/doc/examples/trac_ik/trac_ik_tutorial.rst +++ /dev/null @@ -1,36 +0,0 @@ -:moveit1: - -.. - Once updated for MoveIt 2, remove all lines above title (including this comment and :moveit1: tag) - -TRAC-IK Kinematics Solver -========================= - -`TRAC-IK `_ is an inverse kinematics solver developed by TRACLabs that combines two IK implementations via threading to achieve more reliable solutions than common available open source IK solvers. From their documentation: - - (TRAC-IK) provides an alternative Inverse Kinematics solver to the popular inverse Jacobian methods in KDL. Specifically, KDL's convergence algorithms are based on Newton's method, which does not work well in the presence of joint limits --- common for many robotic platforms. TRAC-IK concurrently runs two IK implementations. One is a simple extension to KDL's Newton-based convergence algorithm that detects and mitigates local minima due to joint limits by random jumps. The second is an SQP (Sequential Quadratic Programming) nonlinear optimization approach which uses quasi-Newton methods that better handle joint limits. By default, the IK search returns immediately when either of these algorithms converges to an answer. Secondary constraints of distance and manipulability are also provided in order to receive back the "best" IK solution. - -The package `trac_ik_kinematics_plugin `_ provides a KinematicsBase MoveIt interface that can replace the default KDL solver. Currently mimic joints are *not* supported. - -Install -------- - -As of v1.5.1, **trac_ik** is part of the ROS Kinetic/Melodic binaries: :: - - sudo apt-get install ros-melodic-trac-ik-kinematics-plugin - -Binaries for Noetic are not yet available. You can track the progress `with this ticket `_. - -Usage ------ - -- Install **trac_ik_kinematics_plugin** and **trac_ik_lib package** or add to your catkin workspace. -- Find the MoveIt :doc:`kinematics.yaml ` file created for your robot. -- Replace ``kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin`` (or similar) with ``kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin`` -- Set parameters as desired: - - - **kinematics\_solver\_timeout** (timeout in seconds, e.g., 0.005) and **position\_only\_ik** **ARE** supported. - - **solve\_type** can be Speed, Distance, Manipulation1, Manipulation2 (see trac\_ik\_lib documentation for details). Default is Speed. - - **kinematics\_solver\_attempts** parameter is unneeded: unlike KDL, TRAC-IK solver already restarts when it gets stuck - - **kinematics\_solver\_search\_resolution** is not applicable here. - - Note: The Cartesian error distance used to determine a valid solution is **1e-5**, as that is what is hard-coded into MoveIt's KDL plugin. diff --git a/doc/how_to_guides/how_to_guides.rst b/doc/how_to_guides/how_to_guides.rst index 7b21efdfa9..7569723fc1 100644 --- a/doc/how_to_guides/how_to_guides.rst +++ b/doc/how_to_guides/how_to_guides.rst @@ -22,6 +22,7 @@ Configuring and Using MoveIt persistent_scenes_and_states/persistent_scenes_and_states isaac_panda/isaac_panda_tutorial pick_ik/pick_ik_tutorial + trac_ik/trac_ik_tutorial benchmarking/benchmarking_tutorial Developing and Documenting MoveIt diff --git a/doc/how_to_guides/trac_ik/trac_ik_tutorial.rst b/doc/how_to_guides/trac_ik/trac_ik_tutorial.rst new file mode 100644 index 0000000000..30f9c0ba74 --- /dev/null +++ b/doc/how_to_guides/trac_ik/trac_ik_tutorial.rst @@ -0,0 +1,43 @@ +TRAC-IK Kinematics Solver +========================= + +`TRAC-IK `_ is an inverse kinematics solver developed by TRACLabs that combines two IK implementations via threading to achieve more reliable solutions than common available open source IK solvers. +From their documentation: + + (TRAC-IK) provides an alternative Inverse Kinematics solver to the popular inverse Jacobian methods in KDL. + Specifically, KDL's convergence algorithms are based on Newton's method, which does not work well in the presence of joint limits --- common for many robotic platforms. TRAC-IK concurrently runs two IK implementations. One is a simple extension to KDL's Newton-based convergence algorithm that detects and mitigates local minima due to joint limits by random jumps. The second is an SQP (Sequential Quadratic Programming) nonlinear optimization approach which uses quasi-Newton methods that better handle joint limits. By default, the IK search returns immediately when either of these algorithms converges to an answer. Secondary constraints of distance and manipulability are also provided in order to receive back the "best" IK solution. + +The package `trac_ik_kinematics_plugin `_ provides a ``KinematicsBase`` MoveIt interface that can replace the default KDL solver. +Currently, mimic joints are *not* supported. + +Install +------- + +The ``rolling-devel`` branch of the TRAC-IK repository has the latest ROS 2 implementation. +For now, the repository must be built from source in your ROS 2 workspace, for example ``~/moveit2_ws``. + +``` +cd ~/moveit2_ws/src +git clone -b rolling-devel https://bitbucket.org/traclabs/trac_ik.git +``` + +Usage +----- + +- Find the MoveIt :doc:`kinematics.yaml ` file created for your robot. +- Replace ``kinematics_solver: kdl_kinematics_plugin/KDLKinematicsPlugin`` (or similar) with ``kinematics_solver: trac_ik_kinematics_plugin/TRAC_IKKinematicsPlugin`` +- Make sure you add a ``trac_ik_kinematics_plugin`` tag to your package's corresponding ``package.xml`` file. +- Set parameters as desired: + + - **kinematics\_solver\_timeout** (timeout in seconds, e.g., ``0.005``) and **position\_only\_ik** **ARE** supported. + - **solve\_type** can be ``Speed``, ``Distance``, ``Manip1``, ``Manip2`` (see below for details). Defaults to ``Speed``. + - **epsilon** is the Cartesian error distance used to determine a valid solution. Defaults to ``1e-5``. + - **kinematics\_solver\_attempts** parameter is unneeded: unlike KDL, TRAC-IK solver already restarts when it gets stuck. + - **kinematics\_solver\_search\_resolution** is not applicable here. + +From the `trac_ik_lib `_ package documentation (slightly modified), here is some information about the solve type parameter: + + ``Speed``: returns very quickly the first solution found. + ``Distance``: runs for the full timeout, then returns the solution that minimizes sum of squares error (SSE) from the seed. + ``Manip1``: runs for full timeout, returns solution that maximizes ``sqrt(det(J*J^T))`` (the product of the singular values of the Jacobian). + ``Manip2``: runs for full timeout, returns solution that minimizes the ratio of min to max singular values of the Jacobian.