IK Solver failing sporadically with PILZ

[sandhya-svs]

Description

I'm running PILZ for a 6 DoF robot Kuka arm, previously reachable points have planning failing with the error code "no IK solution".
I have tried with the KDL kinematics solver as well as the Pick IK solver. OMPL planning succeeds for the same Cartesian goal, and a direct call to the IK solver using the GetPositionIK also returns a solution.
With Pick IK, it's planning around 10% of the time.

ROS Distro

Jazzy

OS and version

24.04

Source or binary build?

Binary

If binary, which release version?

No response

If source, which branch?

No response

Which RMW are you using?

FastRTPS

Steps to Reproduce

I'm running this on a Kuka Robot arm: URDFs found here: https://github.com/ros-industrial/kuka_experimental/tree/melodic-devel/kuka_kr210_support

The tcp link "tcp_tool" is set at from the last link of the arm:
xyz="0.59844 0.02478 -0.32464" rpy="0 ${radians(-90)} 0"

The goal pose is:

ros2 service call /plan_kinematic_path moveit_msgs/srv/GetMotionPlan "{motion_plan_request: {workspace_parameters: {header: {frame_id: 'base'}}, start_state: {joint_state: {name: ['joint_1', 'joint_2', 'joint_3', 'joint_4', 'joint_5', 'joint_6'], position: [0.0, -2.356194490192345, 1.7453292519943295, 0.0, 0.6108652381980153, 0.0]}}, goal_constraints: [{position_constraints: [{header: {frame_id: 'base'}, link_name: 'tcp_tool', target_point_offset: {x: 0.0, y: 0.0, z: 0.0}, constraint_region: {primitive_poses: [{position: {x: 0.9966349795258316, y: -2.192965241204046, z: 1.1694382033758675}, orientation: {x: 0.0016633241021293997, y: -0.00011394823261380917, z: 0.9993333957532706, w: -0.036468952561988496}}], primitives: [{type: 1, dimensions: [0.01, 0.01, 0.01]}]}, weight: 1.0}], orientation_constraints: [{header: {frame_id: 'base'}, link_name: 'tcp_tool', orientation: {x: 0.0016633241021293997, y: -0.00011394823261380917, z: 0.9993333957532706, w: -0.036468952561988496}, absolute_x_axis_tolerance: 0.01, absolute_y_axis_tolerance: 0.01, absolute_z_axis_tolerance: 0.01, weight: 1.0}]}], pipeline_id: 'pilz', planner_id: 'PTP', group_name: 'manipulator', num_planning_attempts: 1, allowed_planning_time: 5.0, max_velocity_scaling_factor: 1.0, max_acceleration_scaling_factor: 1.0}}"

To run with ompl, just change the planner name.

Kinematics configuration:
manipulator:
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

Expected behavior

PILZ at least tries to plan and IK solution is computed

Actual behavior

PILZ fails saying no IK solution, does not attempt planning. Sometimes finds a solution for the same plan request.

Backtrace or Console output

[move_group-2] [INFO] [1738334454.779232617] [move_group.moveit.moveit.ros.move_group.plan_service]: Received new planning service request...
[move_group-2] [INFO] [1738334454.779361705] [move_group.moveit.moveit.ros.move_group.capability]: Using planning pipeline 'pilz'
[move_group-2] [INFO] [1738334454.779405155] [move_group]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[move_group-2] [INFO] [1738334454.779416541] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-2] [WARN] [1738334454.779419191] [move_group.moveit.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[move_group-2] [INFO] [1738334454.779427857] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-2] [INFO] [1738334454.779444375] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-2] [INFO] [1738334454.779499962] [move_group.moveit.moveit.planners.pilz.trajectory_generator.ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-2] [INFO] [1738334454.779513585] [move_group]: Calling Planner 'Pilz Industrial Motion Planner'
[move_group-2] [INFO] [1738334454.779521026] [move_group.moveit.moveit.planners.pilz.trajectory_generator]: Generating PTP trajectory...
[move_group-2] [ERROR] [1738334454.832607222] [move_group.moveit.pilz_trajectory_functions]: Unable to find IK solution.
[move_group-2] [ERROR] [1738334454.832781757] [move_group.moveit.moveit.planners.pilz.trajectory_generator]: Failed to compute inverse kinematics for link: tcp_tool of goal pose
[move_group-2] [ERROR] [1738334454.832883014] [move_group]: Planner 'Pilz Industrial Motion Planner' failed with error code NO_IK_SOLUTION
[move_group-2] [ERROR] [1738334454.832923104] [move_group.moveit.moveit.ros.move_group.plan_service]: Generating a plan with planning pipeline failed.
[move_group-2] [INFO] [1738334458.475965901] [move_group.moveit.moveit.ros.move_group.plan_service]: Received new planning service request...
[move_group-2] [INFO] [1738334458.476063110] [move_group.moveit.moveit.ros.move_group.capability]: Using planning pipeline 'pilz'
[move_group-2] [INFO] [1738334458.476139827] [move_group]: Calling PlanningRequestAdapter 'ResolveConstraintFrames'
[move_group-2] [INFO] [1738334458.476153143] [move_group]: Calling PlanningRequestAdapter 'ValidateWorkspaceBounds'
[move_group-2] [WARN] [1738334458.476155609] [move_group.moveit.moveit.ros.validate_workspace_bounds]: It looks like the planning volume was not specified. Using default values.
[move_group-2] [INFO] [1738334458.476164316] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateBounds'
[move_group-2] [INFO] [1738334458.476181576] [move_group]: Calling PlanningRequestAdapter 'CheckStartStateCollision'
[move_group-2] [INFO] [1738334458.476239409] [move_group.moveit.moveit.planners.pilz.trajectory_generator.ptp]: Initialized Point-to-Point Trajectory Generator.
[move_group-2] [INFO] [1738334458.476268741] [move_group]: Calling Planner 'Pilz Industrial Motion Planner'
[move_group-2] [INFO] [1738334458.476279861] [move_group.moveit.moveit.planners.pilz.trajectory_generator]: Generating PTP trajectory...
[move_group-2] [INFO] [1738334458.490501233] [move_group]: Calling PlanningResponseAdapter 'AddTimeOptimalParameterization'
[move_group-2] [INFO] [1738334458.492692837] [move_group]: Calling PlanningResponseAdapter 'ValidateSolution'
[move_group-2] [INFO] [1738334458.493200798] [move_group]: Calling PlanningResponseAdapter 'DisplayMotionPath'

[sea-bass]

PickIK is generally not a good IK solver unless you have a special set of custom cost functions that don't have analytical derivatives, and are willing to sacrifice on other metrics. Otherwise, I would advise just using the KDL or TRAC-IK solvers for best results.

I couldn't quite figure this out from the issue description, but did KDL successfully give you a valid joint state solution for the target pose at the end of the PTP motion?

If it did not, consider trying out TRAC-IK. It's a really nice IK solver that combines KDL in parallel with another optimizing solver.

https://moveit.picknik.ai/main/doc/how_to_guides/trac_ik/trac_ik_tutorial.html

[sandhya-svs]

Hi, so it wasn't finding an IK with KDL. Which is why I was confused about how OMPL was working - won't it be using the same IK solver configured for the robot? I'll try TRAC-IK and update here. Thanks!

[sea-bass]

Well, there are differences.

OMPL will only solve IK at the end point and then plan a joint space path to that end point. So the path won't necessarily be a straight line in the world.

Pilz LIN/CIRC planners will solve multiple IK problems along the entire straight-line/circular path, which can be tricky for some non-redundant manipulators with 6DOF or less, like the KUKA robot you are using.

I would also check whether your desired straight-line path maybe goes outside the robot's work envelope? You could tell as well based on how "curvy" the valid OMPL paths may look.