Skip to content

Commit f689924

Browse files
Robotawisea-bassstephanie-eng
authored
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]>
1 parent e1044d2 commit f689924

File tree

7 files changed

+312
-0
lines changed

7 files changed

+312
-0
lines changed

‎CMakeLists.txt‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -71,6 +71,7 @@ add_subdirectory(doc/how_to_guides/pilz_industrial_motion_planner)
7171
add_subdirectory(doc/how_to_guides/using_ompl_constrained_planning)
7272
add_subdirectory(doc/tutorials/pick_and_place_with_moveit_task_constructor)
7373
add_subdirectory(doc/tutorials/quickstart_in_rviz)
74+
add_subdirectory(doc/how_to_guides/pick_ik)
7475

7576
ament_export_dependencies(
7677
${THIS_PACKAGE_INCLUDE_DEPENDS}

‎doc/how_to_guides/how_to_guides.rst‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -20,6 +20,7 @@ Configuring and Using MoveIt
2020
controller_teleoperation/controller_teleoperation
2121
persistent_scenes_and_states/persistent_scenes_and_states
2222
isaac_panda/isaac_panda_tutorial
23+
pick_ik/pick_ik_tutorial
2324

2425
Developing and Documenting MoveIt
2526
---------------------------------

‎doc/how_to_guides/pick_ik/CMakeLists.txt‎

Lines changed: 6 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,6 @@
1+
install(DIRECTORY launch
2+
DESTINATION share/${PROJECT_NAME}
3+
)
4+
install(DIRECTORY config
5+
DESTINATION share/${PROJECT_NAME}
6+
)

‎doc/how_to_guides/pick_ik/config/kinematics_pick_ik.yaml‎

Lines changed: 14 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,14 @@
1+
2+
# Configure pick_ik as the solver to use with panda_arm
3+
panda_arm:
4+
kinematics_solver: pick_ik/PickIkPlugin # The IK solver plugin to use
5+
kinematics_solver_timeout: 0.05 # Maximum duration in seconds the solver is allowed to run per attempt
6+
kinematics_solver_attempts: 3 # Maximum number of solver attempts to find a solution
7+
mode: global # Solver mode 'global' enables evolutionary algorithm with gradient descent, and 'local' uses only gradient descent
8+
position_scale: 1.0 # A scale factor for the position cost, setting this to 0.0 means rotation-only IK
9+
rotation_scale: 0.5 # A scale factor for the orientation cost, setting this to 0.0 means position-only IK
10+
position_threshold: 0.001 # Threshold for position difference in meters to consider a solution valid
11+
orientation_threshold: 0.01 # Threshold for orientation difference in radians to consider a solution valid
12+
cost_threshold: 0.001 # Maximum cost function value for a valid solution (all cost functions must return a value lower than this)
13+
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
14+
gd_step_size: 0.0001 # Step size for the gradient descent

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

Lines changed: 145 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,145 @@
1+
import os
2+
from launch import LaunchDescription
3+
from launch.actions import DeclareLaunchArgument, OpaqueFunction
4+
from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
5+
from launch.conditions import IfCondition, UnlessCondition
6+
from launch_ros.actions import Node
7+
from launch_ros.substitutions import FindPackageShare
8+
from launch.actions import ExecuteProcess
9+
from ament_index_python.packages import get_package_share_directory
10+
from moveit_configs_utils import MoveItConfigsBuilder
11+
12+
13+
def generate_launch_description():
14+
15+
declared_arguments = []
16+
declared_arguments.append(
17+
DeclareLaunchArgument(
18+
"rviz_config",
19+
default_value="panda_moveit_config_demo.rviz",
20+
description="RViz configuration file",
21+
)
22+
)
23+
24+
return LaunchDescription(
25+
declared_arguments + [OpaqueFunction(function=launch_setup)]
26+
)
27+
28+
29+
def launch_setup(context, *args, **kwargs):
30+
31+
moveit_config = (
32+
MoveItConfigsBuilder("moveit_resources_panda")
33+
.robot_description(file_path="config/panda.urdf.xacro")
34+
.trajectory_execution(file_path="config/gripper_moveit_controllers.yaml")
35+
.planning_scene_monitor(
36+
publish_robot_description=True, publish_robot_description_semantic=True
37+
)
38+
.planning_pipelines(
39+
pipelines=["ompl", "chomp", "pilz_industrial_motion_planner"]
40+
)
41+
.robot_description_kinematics(
42+
file_path=os.path.join(
43+
get_package_share_directory("moveit2_tutorials"),
44+
"config",
45+
"kinematics_pick_ik.yaml",
46+
)
47+
)
48+
.to_moveit_configs()
49+
)
50+
51+
# Start the actual move_group node/action server
52+
run_move_group_node = Node(
53+
package="moveit_ros_move_group",
54+
executable="move_group",
55+
output="screen",
56+
parameters=[moveit_config.to_dict()],
57+
)
58+
59+
rviz_base = LaunchConfiguration("rviz_config")
60+
rviz_config = PathJoinSubstitution(
61+
[FindPackageShare("moveit2_tutorials"), "launch", rviz_base]
62+
)
63+
64+
# RViz
65+
rviz_node = Node(
66+
package="rviz2",
67+
executable="rviz2",
68+
name="rviz2",
69+
output="log",
70+
arguments=["-d", rviz_config],
71+
parameters=[
72+
moveit_config.robot_description,
73+
moveit_config.robot_description_semantic,
74+
moveit_config.robot_description_kinematics,
75+
moveit_config.planning_pipelines,
76+
moveit_config.joint_limits,
77+
],
78+
)
79+
80+
# Static TF
81+
static_tf = Node(
82+
package="tf2_ros",
83+
executable="static_transform_publisher",
84+
name="static_transform_publisher",
85+
output="log",
86+
arguments=["0.0", "0.0", "0.0", "0.0", "0.0", "0.0", "world", "panda_link0"],
87+
)
88+
89+
# Publish TF
90+
robot_state_publisher = Node(
91+
package="robot_state_publisher",
92+
executable="robot_state_publisher",
93+
name="robot_state_publisher",
94+
output="both",
95+
parameters=[moveit_config.robot_description],
96+
)
97+
98+
# ros2_control using FakeSystem as hardware
99+
ros2_controllers_path = os.path.join(
100+
get_package_share_directory("moveit_resources_panda_moveit_config"),
101+
"config",
102+
"ros2_controllers.yaml",
103+
)
104+
ros2_control_node = Node(
105+
package="controller_manager",
106+
executable="ros2_control_node",
107+
parameters=[moveit_config.robot_description, ros2_controllers_path],
108+
output="both",
109+
)
110+
111+
joint_state_broadcaster_spawner = Node(
112+
package="controller_manager",
113+
executable="spawner",
114+
arguments=[
115+
"joint_state_broadcaster",
116+
"--controller-manager-timeout",
117+
"300",
118+
"--controller-manager",
119+
"/controller_manager",
120+
],
121+
)
122+
123+
arm_controller_spawner = Node(
124+
package="controller_manager",
125+
executable="spawner",
126+
arguments=["panda_arm_controller", "-c", "/controller_manager"],
127+
)
128+
129+
hand_controller_spawner = Node(
130+
package="controller_manager",
131+
executable="spawner",
132+
arguments=["panda_hand_controller", "-c", "/controller_manager"],
133+
)
134+
nodes_to_start = [
135+
rviz_node,
136+
static_tf,
137+
robot_state_publisher,
138+
run_move_group_node,
139+
ros2_control_node,
140+
joint_state_broadcaster_spawner,
141+
arm_controller_spawner,
142+
hand_controller_spawner,
143+
]
144+
145+
return nodes_to_start

‎doc/how_to_guides/pick_ik/pick_ik_tutorial.rst‎

Lines changed: 144 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,144 @@
1+
pick_ik Kinematics Solver
2+
=========================
3+
4+
`pick_ik <https://github.com/PickNikRobotics/pick_ik>`_ is an inverse kinematics (IK) solver compatible with MoveIt 2,
5+
developed by PickNik Robotics. It is designed to provide robust and customizable IK solutions,
6+
offering a wide range of features.
7+
8+
The solver in ``pick_ik`` is a reimplementation of `bio_ik <https://github.com/TAMS-Group/bio_ik>`_,
9+
integrating a global optimizer and a local optimizer. The global optimizer utilizes evolutionary algorithms to explore
10+
alternative solutions within the solution space and identify global optima. Building upon the results obtained by the global optimizer,
11+
the local optimizer applies gradient descent for iterative refinement of the solution. It takes a potential optimum solution provided
12+
by the global optimizer as input and aims to improve its accuracy and ultimately convergence to the most optimum solution.
13+
14+
Getting Started
15+
---------------
16+
Before proceeding, please ensure that you have completed the steps outlined in the :doc:`Getting Started Guide </doc/tutorials/getting_started/getting_started>`.
17+
18+
Additionally, it is required to have a MoveIt configuration package specifically tailored to your robot.
19+
This package can be created using the :doc:`MoveIt Setup Assistant </doc/examples/setup_assistant/setup_assistant_tutorial>`.
20+
21+
Installation
22+
------------
23+
24+
From binaries
25+
^^^^^^^^^^^^^
26+
Make sure your ROS 2 installation is sourced and run the following command ::
27+
28+
sudo apt install ros-$ROS_DISTRO-pick-ik
29+
30+
From source
31+
^^^^^^^^^^^
32+
33+
Create a colcon workspace. ::
34+
35+
export COLCON_WS=~/ws_moveit2/
36+
mkdir -p $COLCON_WS/src
37+
38+
Clone this repository in the src directory of your workspace. ::
39+
40+
cd $COLCON_WS/src
41+
git clone -b main https://github.com/PickNikRobotics/pick_ik.git
42+
43+
Set up colcon mixins. ::
44+
45+
sudo apt install python3-colcon-common-extensions
46+
sudo apt install python3-colcon-mixin
47+
colcon mixin add default https://raw.githubusercontent.com/colcon/colcon-mixin-repository/master/index.yaml
48+
colcon mixin update default
49+
50+
Build the workspace. ::
51+
52+
cd /path/to/your/workspace
53+
colcon build --mixin release
54+
55+
Usage
56+
-----
57+
58+
Using pick_ik as a Kinematics Plugin
59+
^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^^
60+
61+
You can use :doc:`MoveIt Setup Assistant </doc/examples/setup_assistant/setup_assistant_tutorial>` to create
62+
the configuration files for your robot to use it with MoveIt, or edit the ``kinematics.yaml`` file in your
63+
robot's config directory to use pick_ik as the IK solver. ::
64+
65+
panda_arm:
66+
kinematics_solver: pick_ik/PickIkPlugin
67+
kinematics_solver_timeout: 0.05
68+
kinematics_solver_attempts: 3
69+
mode: global
70+
position_scale: 1.0
71+
rotation_scale: 0.5
72+
position_threshold: 0.001
73+
orientation_threshold: 0.01
74+
cost_threshold: 0.001
75+
minimal_displacement_weight: 0.0
76+
gd_step_size: 0.0001
77+
78+
79+
.. note::
80+
81+
You can launch a preconfigured demo with ``pick_ik`` using the following command:
82+
83+
.. code-block::
84+
85+
ros2 launch moveit2_tutorials demo_pick_ik.launch.py
86+
87+
The command starts a demo similar to the one in :doc:`MoveIt Quickstart in RViz </doc/tutorials/quickstart_in_rviz/quickstart_in_rviz_tutorial>`
88+
tutorial. However, this demo specifically utilizes the robot kinematics configuration file ``kinematics_pick_ik.yaml``
89+
located at the ``moveit2_tutorials/doc/pick_ik/config`` directory :codedir:`here<how_to_guides/pick_ik/config/kinematics_pick_ik.yaml>`.
90+
91+
Parameter Description
92+
^^^^^^^^^^^^^^^^^^^^^
93+
94+
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>`__.
95+
96+
Some key parameters you may want to start with are:
97+
98+
- ``mode``: If you choose ``local``, this solver will only do local gradient descent; if you choose ``global``,
99+
it will also enable the evolutionary algorithm. Using the global solver will be less performant, but if you're
100+
having trouble getting out of local minima, this could help you. We recommend using ``local`` for things like
101+
relative motion / Cartesian interpolation / endpoint jogging, and ``global`` if you need to solve for goals
102+
with a far-away initial condition.
103+
104+
- ``memetic_<property>``: All the properties that only kick in if you use the ``global`` solver.
105+
The key one is ``memetic_num_threads``, as we have enabled the evolutionary algorithm to solve on multiple threads.
106+
107+
- ``position_threshold`` / ``orientation_threshold``: Optimization succeeds only if the pose difference is less than
108+
these thresholds in meters and radians respectively. A ``position_threshold`` of 0.001 would mean a 1 mm accuracy and
109+
an ``orientation_threshold`` of 0.01 would mean a 0.01 radian accuracy.
110+
111+
- ``cost_threshold``: This solver works by setting up cost functions based on how far away your pose is,
112+
how much your joints move relative to the initial guess, and custom cost functions you can add.
113+
Optimization succeeds only if the cost is less than ``cost_threshold``. Note that if you're adding custom cost functions,
114+
you may want to set this threshold fairly high and rely on ``position_threshold`` and ``orientation_threshold`` to be your deciding factors,
115+
whereas this is more of a guideline.
116+
117+
- ``approximate_solution_position_threshold`` / ``approximate_solution_orientation_threshold``:
118+
When using approximate IK solutions for applications such as endpoint servoing, ``pick_ik`` may sometimes return solutions
119+
that are significantly far from the goal frame. To prevent issues with such jumps in solutions,
120+
these parameters define maximum translational and rotation displacement.
121+
We recommend setting this to values around a few centimeters and a few degrees for most applications.
122+
123+
- ``position_scale``: If you want rotation-only IK, set this to 0.0. If you want to solve for a custom ``IKCostFn``
124+
(which you provide in your ``setFromIK()`` call), set both ``position_scale`` and ``rotation_scale`` to 0.0.
125+
You can also use any other value to weight the position goal; it's part of the cost function.
126+
Note that any checks using ``position_threshold`` will be ignored if you use ``position_scale = 0.0``.
127+
128+
129+
- ``rotation_scale``: If you want position-only IK, set this to 0.0. If you want to treat position and orientation equally,
130+
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``
131+
will be ignored if you use ``rotation_scale = 0.0``.
132+
133+
- ``minimal_displacement_weight``: This is one of the standard cost functions that checks for the joint angle difference between
134+
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.
135+
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.
136+
137+
You can test out this solver live in RViz, as this plugin uses the `generate_parameter_library <https://github.com/PickNikRobotics/generate_parameter_library>`_
138+
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.,
139+
140+
.. code-block::
141+
142+
ros2 param set /rviz2 robot_description_kinematics.panda_arm.mode global
143+
144+
ros2 param set /rviz2 robot_description_kinematics.panda_arm.minimal_displacement_weight 0.001

‎package.xml‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -36,6 +36,7 @@
3636
<depend>tf2_geometry_msgs</depend>
3737
<depend>tf2_ros</depend>
3838
<depend>moveit_task_constructor_core</depend>
39+
<depend>pick_ik</depend>
3940

4041
<build_depend>eigen</build_depend>
4142
<build_depend>geometric_shapes</build_depend>

0 commit comments

Comments
 (0)