Skip to content

Commit

Permalink
Add explanation about IK plugins for servo (#474)
Browse files Browse the repository at this point in the history
* Add explanation to servo tutorial for how to configure an IK plugin for servo

* Make bio_ik suggestion

* fix formatting

Co-authored-by: AndyZe <andyz@utexas.edu>
  • Loading branch information
wyattrees and AndyZe committed Jul 22, 2022
1 parent 02f784f commit c7a0cb3
Showing 1 changed file with 35 additions and 0 deletions.
35 changes: 35 additions & 0 deletions doc/examples/realtime_servo/realtime_servo_tutorial.rst
Original file line number Diff line number Diff line change
Expand Up @@ -107,6 +107,41 @@ Servo includes a number of nice features:
5. Joint position and velocity limits enforced
6. Inputs are generic ROS messages

Inverse Kinematics in Servo
^^^^^^^^^^^^^^^^^^^^^^^^^^^

Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin.
To configure an IK plugin for use in Servo, your robot config package must define one in a :code:`kinematics.yaml` file, such as the one
in the `Panda config package <https://github.com/ros-planning/moveit_resources/blob/master/panda_moveit_config/config/kinematics.yaml>`_.
Several IK plugins are available `within MoveIt <https://github.com/ros-planning/moveit2/tree/main/moveit_kinematics>`_,
as well as `externally <https://github.com/PickNikRobotics/bio_ik/tree/ros2>`_.
:code:`bio_ik/BioIKKinematicsPlugin` is the most common choice.

Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file:

.. code-block:: python
moveit_config = (
MoveItConfigsBuilder("moveit_resources_panda")
.robot_description(file_path="config/panda.urdf.xacro")
.to_moveit_configs()
)
servo_node = Node(
package="moveit_servo",
executable="servo_node_main",
parameters=[
servo_params,
moveit_config.robot_description,
moveit_config.robot_description_semantic,
moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed
],
)
The above excerpt is taken from `servo_example.launch.py <https://github.com/ros-planning/moveit2/blob/main/moveit_ros/moveit_servo/launch/servo_example.launch.py>`_ in MoveIt.
In the above example, the :code:`kinematics.yaml` file is taken from the `moveit_resources <https://github.com/ros-planning/moveit_resources>`_ repository in the workspace, specifically :code:`moveit_resources/panda_moveit_config/config/kinematics.yaml`.
The actual ROS parameter names that get passed by loading the yaml file are of the form :code:`robot_description_kinematics.<group_name>.<param_name>`, e.g. :code:`robot_description_kinematics.panda_arm.kinematics_solver`.

Setup on a New Robot
--------------------

Expand Down

0 comments on commit c7a0cb3

Please sign in to comment.