Skip to content

Commit c7a0cb3

Browse files
wyattreesAndyZe
andauthored
Add explanation about IK plugins for servo (#474)
* 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>
1 parent 02f784f commit c7a0cb3

File tree

1 file changed

+35
-0
lines changed

1 file changed

+35
-0
lines changed

doc/examples/realtime_servo/realtime_servo_tutorial.rst

Lines changed: 35 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -107,6 +107,41 @@ Servo includes a number of nice features:
107107
5. Joint position and velocity limits enforced
108108
6. Inputs are generic ROS messages
109109

110+
Inverse Kinematics in Servo
111+
^^^^^^^^^^^^^^^^^^^^^^^^^^^
112+
113+
Inverse Kinematics may be handled internally by MoveIt Servo via inverse Jacobian calculations. However, you may also use an IK plugin.
114+
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
115+
in the `Panda config package <https://github.com/ros-planning/moveit_resources/blob/master/panda_moveit_config/config/kinematics.yaml>`_.
116+
Several IK plugins are available `within MoveIt <https://github.com/ros-planning/moveit2/tree/main/moveit_kinematics>`_,
117+
as well as `externally <https://github.com/PickNikRobotics/bio_ik/tree/ros2>`_.
118+
:code:`bio_ik/BioIKKinematicsPlugin` is the most common choice.
119+
120+
Once your :code:`kinematics.yaml` file has been populated, include it with the ROS parameters passed to the servo node in your launch file:
121+
122+
.. code-block:: python
123+
124+
moveit_config = (
125+
MoveItConfigsBuilder("moveit_resources_panda")
126+
.robot_description(file_path="config/panda.urdf.xacro")
127+
.to_moveit_configs()
128+
)
129+
servo_node = Node(
130+
package="moveit_servo",
131+
executable="servo_node_main",
132+
parameters=[
133+
servo_params,
134+
moveit_config.robot_description,
135+
moveit_config.robot_description_semantic,
136+
moveit_config.robot_description_kinematics, # here is where kinematics plugin parameters are passed
137+
],
138+
)
139+
140+
141+
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.
142+
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`.
143+
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`.
144+
110145
Setup on a New Robot
111146
--------------------
112147

0 commit comments

Comments
 (0)