MoveIt Servo - Configuring pick_ik parameters

[peterdavidfagan]

I am currently working on running MoveIt Servo on the UFactory Lite6 (via the Python library) in order to collect demonstration data. I have it working with the inverse jacobian method but I am now trying to configure the pick_ik plugin as the former method is struggling with encountering singularities. While trying to configure pick_ik even when I choose high threshold values I am encountering errors in servo_calc:

[servo_node_main-2] [WARN] [1676545657.980554653] [moveit_servo.servo_calcs]: Could not find IK solution for requested motion, got error code -31

Here is my current config:

lite6:
  kinematics_solver: pick_ik/PickIkPlugin
  kinematics_solver_timeout: 0.1
  kinematics_solver_attempts: 5
  mode: local
  rotation_scale: 0.5
  position_threshold: 5.0
  orientation_threshold: 10.0
  cost_threshold: 100.0
  minimal_displacement_weight: 0.001
  gd_step_size: 0.001

For my own reference:

• int32 NO_IK_SOLUTION=-31
• https://github.com/PickNikRobotics/pick_ik/blob/main/src/pick_ik_parameters.yaml
• https://github.com/PickNikRobotics/pick_ik/blob/main/doc/USAGE.md
• https://github.com/ros-planning/moveit2/blob/694e1f178b71bfe26d0f5d7e5e64301008870d40/moveit_ros/moveit_servo/src/servo_calcs.cpp#L636

Current Performance with KDL kinematics plugin:

IMG_1069.1.MOV

[AndyZe]

Very cool! Might be smoother if you are able to control it via velocity instead of position.

Tagging @sea-bass to make sure he sees the pick_ik question.

[peterdavidfagan]

Thanks @AndyZe, I'll go ahead and make this change.

[peterdavidfagan]

It seems like the shaking issue with MoveIt Servo is not uncommon for robots from UFactory: xArm-Developer/xarm_ros2#2

[sea-bass]

Looking at the pick_ik configs:

• Those thresholds are huge!!! Position and orientation thresholds are in meters and radians, respectively, so you're basically saying anything within 5 meters and 10 radians of the target is good. I suspect this is why there is often no IK solution because it's probably picking some really weird target poses.
• In my experience, a "good" servo configuration has a nonzero weight for minimal joint displacement (which you do), with a slightly above-average cost threshold.
• Try using the global solver and bring down the gradient descent step? It's possible that this gradient step is too large and causing the solution to not converge, and lead to constant timeouts.

I'd suggest doing something like:

  mode: global
  position_threshold: 0.01
  orientation_threshold: 0.01
  cost_threshold: 0.1
  gd_step_size: 0.0001

Also: I found a potential bug in the rotation calculations and joint bounds, so could you also try with this branch and see if it makes a difference? PickNikRobotics/pick_ik#31

[peterdavidfagan]

Thanks @sea-bass,

Those thresholds are huge!!! Position and orientation thresholds are in meters and radians, respectively, so you're basically saying anything within 5 meters and 10 radians of the target is good. I suspect this is why there is often no IK solution because it's probably picking some really weird target poses.

I need to read the bio-ik paper. I started with thresholds for position and orientation that were reasonably close to the values I saw in the README.md. As these didn't work either I thought increasing the threshold to some ridiculously large threshold should at least work, in this case I think my lack of insight into how bio-ik is using these thresholds is showing. I'll read the paper so I am more informed.

In my experience, a "good" servo configuration has a nonzero weight for minimal joint displacement (which you do), with a slightly above-average cost threshold.

Thanks for this piece of advice.

Try using the global solver and bring down the gradient descent step? It's possible that this gradient step is too large and causing the solution to not converge, and lead to constant timeouts.

Ok perfect I will try this.

Also: I found a potential bug in the rotation calculations and joint bounds, so could you also try with this branch and see if it makes a difference? PickNikRobotics/pick_ik#31

Thanks for making me aware of this I will test against the changes in this pr.

[peterdavidfagan]

Worked great with the following settings, thanks for sharing insights I am also reading the bio-ik paper to gain better intuition:

lite6:
  kinematics_solver: pick_ik/PickIkPlugin
  kinematics_solver_timeout: 0.2
  kinematics_solver_attempts: 3
  mode: global
  rotation_scale: 0.5
  position_threshold: 0.01
  orientation_threshold: 0.01
  cost_threshold: 0.1
  minimal_displacement_weight: 0.001
  gd_step_size: 0.0001

[sea-bass]

Yes, if you can go back to the old branch and check the issue, I would appreciate it.

And if the new branch does help, would also appreciate a review on PickNikRobotics/pick_ik#31 :)

[peterdavidfagan]

Confirming that when I switch back to the main branch on the repo I get:

[servo_node_main-2] [WARN] [1676645890.784155581] [moveit_servo.servo_calcs]: Could not find IK solution for requested motion, got error code -31

[sea-bass]

Thank you so much for testing that!

[peterdavidfagan]

Thanks for the pr you submitted which fixes the errors I was facing.

I need to finish some work for my lab on an Isaac sim env by eod, once this is done I will look to review this pr + take a look at @MarqRazz's isaac pr.

[peterdavidfagan]

I performed a coarse review of the pr, looks good to me but I also need to gain greater familiarity with both pick_ik and bio_ik repos so it may be worth getting further reviews from others who are more familiar with both code bases.