Cartesian MotionPlanning and Rviz plugin issue.

[swiz23]

Hi,

I was trying to test out this new solver (using the defaults specified in the readme) with MoveIt2 earlier this month, and ran into a couple of issues:

1. While planning joint_interpolation or sampling based motion (via MTC) works just fine, cartesian planning fails.
2. While trying to drag the interactive marker (from the MotionPlanning plugin) in Rviz to a different position, the 'Query Goal State' robot would not update.

If I changed back to using the KDL kinematics plugin (just a drop-in replacement in the kinematics.yaml file), both of those issues were resolved. This leads me to believe that there is some integration issue with the pick_ik solver.

Thanks!

[sea-bass]

Could you share more about your MoveIt environment? Distro, building from source/binaries, etc.

And do you see any errors printed on the terminal?

Lastly, if you check the "use approximate solutions" box in RViz, does pick_ik respond at all to your commands?

[swiz23]

ROS Distro: Humble Building from: source Platform: Ubuntu 22.04 within Docker

I do not see any errors in the terminal while attempting to drag the interactive marker. If I try to compute a cartesian path using the MotionPlanning plugin, I get the errors below.

[move_group-1] [INFO] [1674783509.010658156] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Received request to compute Cartesian path
[move_group-1] [INFO] [1674783509.010832794] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Attempting to follow 1 waypoints for link 'left_m07_eoat_pin_link' using a step of 0.010000 m and jump threshold 0.000000 (in global reference frame)
[move_group-1] [INFO] [1674783509.013171048] [moveit_move_group_default_capabilities.cartersian_path_service_capability]: Computed Cartesian path with 1 points (followed 0.000000% of requested trajectory)

If I check the 'use approximate solution' box in Rviz when not using cartesian control, i can plan/execute to a random valid goal successfully. But if I check the 'use cartesian path' box and and set the goal pose to something that should be achievable, then it doesn't matter if I have 'use approximate solution' checked or not. It fails to plan.

Something else I discovered. If I have one planning group using the pick_ik solver, and a different planning group using the lma_kinematics solver, both of the solvers fail to do cartesian motion on their respective groups, and their interactive markers cannot be dragged.

However, if I have one planning group using kdl_kinematics, and the other using lma_kinematics, then I can plan cartesian motion successfully using both planning groups, as well as drag their interactive markers.

[swiz23]

Something else I discovered. If I have one planning group using the pick_ik solver, and a different planning group using the lma_kinematics solver, both of the solvers fail to do cartesian motion on their respective groups, and their interactive markers cannot be dragged.

It turns out that the PR for MoveIt here addresses the concern in the quote above. Now if I specify one planning group with the pick_ik solver and another with the LMA solver, the LMA solver works as expected (cartesian motion can be planned and the interactive marker moves), hower the pick_ik solver still fails (cartesian motion cannot be planned and the interactive marker does not move).

[swiz23]

FWIW, I've also tried the bio_ik plugin, and that seems to work as expected.

[sea-bass]

Thank you for all the details -- that's very helpful. Will take a look and report back with what I find.

[sea-bass]

So, I was able to get things running with pick_ik on Humble as is... some paths are successful, some are not.

https://user-images.githubusercontent.com/4603398/215121786-77b05c33-91f4-4259-89e9-f10b1e22c5c4.mp4

I did have to relax some of the tolerances (position / rotation / cost) and then crank up the minimal displacement cost to try reduce joint flips (I'd rather fail). But it seems like the interface itself is fine; just that the solution rate doesn't seem as good with this solver with the default parameters + the goal pose we're trying to get to.

Anyways, you can play around with the parameters live and see the changes live by doing e.g.

ros2 param set /move_group robot_description_kinematics.panda_arm.minimal_displacement_weight 0.005 

You could also try swapping the mode parameter from global to local because for these small incremental IK motions I don't know if the extra overhead of using a global optimizer is truly required.

I'd be curious to see if changing these values can at least get your solves farther along so they don't consistently fail at 0%?

[swiz23]

Thanks! I'll try modifying the params a bit and let you know. Did you see any issue with dragging the interactive marker?

[sea-bass]

New development: Seems like if I set the rotation_scale parameter to 0.0, the solves with pick_ik are very fast and seem consistent for the Cartesian paths. So I think perhaps the rotation cost function might have some issues.

Could you also check this on your end?

Also, the interactive marker works fine for me (with the same caveats with cost functions and tuning values as above).

[sea-bass]

Oh! I found a potential issue with the rotation cost, which made things work way better on my end: https://github.com/PickNikRobotics/pick_ik/pull/30

I have questions about whether this actually helps, but with that change I started getting way less 0% solves.

Can you try with this branch?

[sea-bass]

Also this is the Panda config that worked best for me in my tests, in case it helps:

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

[swiz23]

Still seeing 0% consistently even with the change in that branch.

FWIW, the way I've reproduced the issue is by doing:

# Pull in the latest moveit2 branch
docker pull moveit/moveit2:humble-source

# This allows docker to launch GUIs
xhost +si:localuser:root

# Run the docker container
docker run -d -it --gpus all --env DISPLAY=$DISPLAY --network host --name moveit_testing -v /tmp/.X11-unix:/tmp/.X11-unix moveit/moveit2:humble-source /bin/bash

# Enter the docker container
docker exec -it moveit_testing bash

# Within the container...

# Install the pick-ik plugin and nano
apt update
apt install ros-humble-pick-ik nano

# Navigate to the kinematics.yaml file in the panda moveit config directory and open it with nano
cd ~/ws_moveit/src/moveit_resources/panda_moveit_config/config/
nano kinematics.yaml

# Within the kinematics.yaml file...

# Replace the contents with the example configs shown from the Usage section.
panda_arm:
  kinematics_solver: pick_ik/PickIkPlugin
  kinematics_solver_timeout: 0.05
  kinematics_solver_attempts: 3
  mode: global
  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

# Exit the file

# Launch MoveIt with Rviz
cd ~/ws_moveit
source install/setup.bash
ros2 launch moveit_resources_panda_moveit_config demo.launch.py rviz_tutorial:=true

# Add MotionPlanning Rviz plugin and change the group to 'panda_arm'
# Try dragging the marker. It should not work. And cartesian motion also seems to fail to plan.

[sea-bass]

So I just tried on the Docker container and I can reproduce this issue you pointed out ... but on local builds it's fine. I think perhaps there is some issue in the container preventing this from working correctly, and I know we have a backport to Humble to fix some things...

[swiz23]

When you say that this works as expected on local builds, are you talking about local Humble or local Rolling builds?

[sea-bass]

I was using a local Humble build following this tutorial. Will need to keep digging as to why the other setups seem to break pick_ik.

[swiz23]

Let me know if you find anything! Thanks for all your help.

[sea-bass]

Still no update on the Docker issues, but I found a bug in calculating orientation cost / angle, which https://github.com/PickNikRobotics/pick_ik/pull/31 fixes.

[sea-bass]

@swiz23 I think there was a recent issue in the moveit2 tutorials that was causing the Docker image to use bad sources. Could you try the latest versions of that image and pick_ik again and let me know how it goes? Thanks!

[swiz23]

Hi @sea-bass,

I tried pulling again but still ran into the same issue (cannot drag interactive marker). Note that I also ran into an rviz2 issue where I got a 'black screen'. The solution proposed at https://github.com/ros2/rviz/issues/948#issuecomment-1428979499 for 'rviz2 on humble' fixed that issue. However, the original issue still remains.

[swiz23]

Decided to try this again today, but the moveit/moveit2:humble-source image seems to be building with rolling. I tested this by sourcing the ws_moveit2 workspace, then typing env | grep ROS, and saw that it was using the 'rolling' distro. Shouldn't it be using 'humble'?

[sea-bass]

Funny enough, there is a PR to address this under review right now! https://github.com/ros-planning/moveit2_tutorials/pull/644

[swiz23]

As I was writing that comment above, I actually thought that maybe someone had already posted an issue about it. But I wasn't sure if that issue would be in moveit2, moveit2_tutorials, or some other moveit related repo, so I figured I'd just comment about it here. Thanks for pointing me to that though!

[adlarkin]

@sea-bass @swiz23: as an update regarding the Docker issue where moveit/moveit2:humble-source image has rolling installed, it actually appears that the root cause of the issue has been discovered here: https://github.com/ros-planning/moveit2/pull/2035#issuecomment-1480082496

Feel free to keep an eye on that discussion, I'm hoping to resolve this soon. Once we get this sorted out and have all of the moveit 2 humble images fixed, hopefully this will resolve the problems being discussed in this issue as well :crossed_fingers:

[adlarkin]

@swiz23 the moveit/moveit2:humble-source should be fixed now to have humble installed instead of rolling. Would you mind pulling the latest image and checking if that helps resolve any issues you're facing?

[swiz23]

So the humble issue seems to be fixed. But for some reason, I cannot install pick_ik from the apt store by doing 'apt install ros-humble-pick-ik' anymore. So I tried to build from source within the moveit2 humble docker container. This involved first installing 'librange-v3-dev'. However, then I ran into a compilation error towards the end of the build. The error was for every mention of 'lower_bounds'. It seems like the 'rsl' ros2 package has deprecated this function in favor of rsl::gt_eq.

root@re2-pgh-nb-84:~/ws_moveit# colcon build --mixin release --packages-select pick_ik
Starting >>> pick_ik 
--- stderr: pick_ik                               
In file included from /root/ws_moveit/src/pick_ik/src/pick_ik_plugin.cpp:7:
/root/ws_moveit/build/pick_ik/pick_ik_parameters/include/pick_ik_parameters.hpp: In member function ‘rcl_interfaces::msg::SetParametersResult pick_ik::ParamListener::update(const std::vector<rclcpp::Parameter>&)’:
/root/ws_moveit/build/pick_ik/pick_ik_parameters/include/pick_ik_parameters.hpp:196:61: error: ‘auto rsl::lower_bounds(const rclcpp::Parameter&, const T&) [with T = double]’ is deprecated: Replace with rsl::gt_eq [-Werror=deprecated-declarations]
  196 |             if(auto validation_result = lower_bounds<double>(param, 1e-12);
      |                                         ~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~~~~~
In file included from /root/ws_moveit/build/pick_ik/pick_ik_parameters/include/pick_ik_parameters.hpp:25,
                 from /root/ws_moveit/src/pick_ik/src/pick_ik_plugin.cpp:7:
/opt/ros/humble/include/rsl/rsl/parameter_validators.hpp:221:59: note: declared here
  221 | [[deprecated("Replace with rsl::gt_eq"), nodiscard]] auto lower_bounds(
      |                                                           ^~~~~~~~~~~~
In file included from /root/ws_moveit/src/pick_ik/src/pick_ik_plugin.cpp:7:
/root/ws_moveit/build/pick_ik/pick_ik_parameters/include/pick_ik_parameters.hpp:203:62: error: ‘auto rsl::lower_bounds(const rclcpp::Parameter&, const T&) [with T = long int]’ is deprecated: Replace with rsl::gt_eq [-Werror=deprecated-declarations]
  203 |             if(auto validation_result = lower_bounds<int64_t>(param, 1);
      |                                         ~~~~~~~~~~~~~~~~~~~~~^~~~~~~~~~
In file included from /root/ws_moveit/build/pick_ik/pick_ik_parameters/include/pick_ik_parameters.hpp:25,
                 from /root/ws_moveit/src/pick_ik/src/pick_ik_plugin.cpp:7:
/opt/ros/humble/include/rsl/rsl/parameter_validators.hpp:221:59: note: declared here
  221 | [[deprecated("Replace with rsl::gt_eq"), nodiscard]] auto lower_bounds(

[sea-bass]

Ah yes, we found and have since fixed some conflicts with that RSL package as we are using some of its latest features.

I believe you may have to apt-get remove ros-humble-rsl and pull https://github.com/PickNikRobotics/rsl down from source, then rebuild.

[swiz23]

If you look at https://github.com/PickNikRobotics/RSL/pull/78, it seems the commit deprecating this function was made yesterday and the rsl package was released as a debian (dated March 23). This is a breaking change with pick_ik.

EDIT: Just saw your comment. I don't think that should be necessary since the latest rsl debian installed is from yesteday.

[sea-bass]

Oh, I see -- thank you! I'll have to go fix that, then.

[sea-bass]

@swiz23 OK so I think I fixed it in https://github.com/PickNikRobotics/pick_ik/pull/37 -- thank you so much for continuing to help us with this. I went through your exact steps but just changed to the branch to this PR.

Could you try the following inside the container, along with your other steps?

apt-get update
cd ~/ws_moveit/src
git clone -b update-rsl-bounds https://github.com/PickNikRobotics/pick_ik.git
cd ..
rosdep install -r --from-paths src --ignore-src --rosdistro $ROS_DISTRO -y
colcon build

Also note that planning will work, but you will then get an execution error from time optimal trajectory generation requiring joint limits. To fix this, you can do

nano ~/ws_moveit/src/moveit_resources/panda_moveit_config/launch/demo.launch.py

and add the joint limits to all sections that try load moveit params as follows

parameters=[
            moveit_config.robot_description,
            moveit_config.robot_description_semantic,
            moveit_config.planning_pipelines,
            moveit_config.robot_description_kinematics,
            moveit_config.joint_limits,  #  <-- Add this
        ],

Then rebuild and go forth!

... or you can checkout my moveit_resources fork from https://github.com/ros-planning/moveit_resources/pull/165 where I ended up fixing that.

[swiz23]

Hi @sea-bass,

I followed your instructions and cloned your particular branch. I also modified the moveit params as you suggested. Pick Ik now builds successfully. I am also able to drag the interactive marker in Rviz as well as plan/execute cartesian motion using the MotionPlanning plugin.

It's still a bit curious to me as to why I can't install ros-humble-pick-ik from the apt store. It used to be able to work... In any event, once PR #37 lands, I'll consider this issue closed.

[sea-bass]

Glad this got you up and running!

I think there was an issue with the most recent release of MoveIt where some binaries, like pick_ik, didn't make it through.

This is also being looked at, so will be resolved in the near future...

[sea-bass]

Update: Looked into this, seems that this image uses the ros2-testing apt repo (see here), which for some reason doesn't have the ros-humble-pick-ik package... so that's the issue.

If you were using rolling, or the regular (non-testing) apt repos, you would be able to install pick_ik as normal from binaries.

We'll get to resolving this too!

[swiz23]

Thanks for getting this merged! I'm going to do one final test and let you know how it goes.

[swiz23]

Tested and things look good! I'm still seeing the Cartesian planner fail occasionally, but that might be a tuning thing.