ROS2 – Make some research on IK solvers

martinaxgloria commented 1 year ago

This is a follow-up of #3.

As reported here:

rviz is lagging a lot during the movement, and this could be a problem regarding the chosen IK solver (KDL). We could think to change this solver using the other offered by MoveIt

We could make some research on possible alternatives to KDL which is the standard IK solver recommended by MoveIT and try to reduce the lag as much as possible

pattacini commented 1 year ago

I would give a try to TRAC-IK to start with.

martinaxgloria commented 1 year ago

Hi @pattacini,

TRAC is the one I was looking at. I will write here the result of this study. The first thing that I noticed this is not part of ROS2, while it was included with ROS 1. So, if we decided to move in this direction, we will compile it

pattacini commented 1 year ago

TRAC is the one I was looking at. I will write here the result of this study.

Great 👍🏻

The first thing that I noticed this is not part of ROS2, while it was included with ROS 1. So, if we decided to move in this direction, we will compile it

Ok! Let's go on to see whether we can quickly improve timing. Later, we will decide what to do.

There's also IKFAST. Don't know if it's included in ROS2 though.

martinaxgloria commented 1 year ago

There's also IKFAST. Don't know if it's included in ROS2 though.

I found this documentation page of MoveIt Humble version but it seems that IKFast was only tested with ros melodic, which is ROS1. I think the documentation is not updated

martinaxgloria commented 1 year ago

Behind the choice of the proper IK solver, I found out that when all the nodes are running at the same time with gazebo simulation and motion planning plugin, the rviz frame rate was very low (~5 fps). By checking the Windows task manager and running:

glxinfo -B

I saw that only the Invidia integrated GPU was being used by WLS2. I found this guide that illustrates how to do GPU selection within WSL environment. Running:

export MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA

I set the NVIDIA GPU usage as the default one in order to see if the rviz frame rate improves but nothing changes, currently I have something like 2/3 fps and during the movement, I see 0 fps.

cc @pattacini @Nicogene

traversaro commented 1 year ago

Did you tried to use CPU rendering? In general it is slower than GPU but if GPU has problems can be faster. To force CPU use, export LIBGL_ALWAYS_SOFTWARE=true

martinaxgloria commented 1 year ago

Hi @traversaro, thanks for the suggestion! I'm going to try this way and I'll let you know

traversaro commented 1 year ago

To debug more the issue, you can also run:

MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA glxgears
MESA_D3D12_DEFAULT_ADAPTER_NAME=Intel glxgears
LIBGL_ALWAYS_SOFTWARE=true glxgears

And compare the FPS for the different combinations, in a way completly indipendent from ROS. Unfortunatly there are cases in which d3d12 is slower then software rendering (see https://github.com/microsoft/wslg/issues/368). In some cases, updating d3d12 (by updating mesa) can help.

martinaxgloria commented 1 year ago

Hi @traversaro, sorry I thought I answered. By the way, I tried with export LIBGL_ALWAYS_SOFTWARE=true, then I launched all the ros2 nodes I need and nothing seems to change, always ~2 fps in rviz and ~60 fps in gazebo.

I tried also with glxgears to see the differences between the 3 cases you suggested and I obtained:

$ MESA_D3D12_DEFAULT_ADAPTER_NAME=NVIDIA glxgears
445 frames in 5.0 seconds = 88.849 FPS
452 frames in 5.0 seconds = 90.251 FPS
443 frames in 5.0 seconds = 88.430 FPS
458 frames in 5.0 seconds = 91.361 FPS
463 frames in 5.0 seconds = 92.464 FPS
430 frames in 5.0 seconds = 85.911 FPS
424 frames in 5.0 seconds = 84.716 FPS

$ MESA_D3D12_DEFAULT_ADAPTER_NAME=Intel glxgears
404 frames in 5.0 seconds = 80.631 FPS
441 frames in 5.0 seconds = 88.193 FPS
446 frames in 5.0 seconds = 89.081 FPS
402 frames in 5.0 seconds = 80.238 FPS
423 frames in 5.0 seconds = 84.492 FPS
414 frames in 5.0 seconds = 82.701 FPS
415 frames in 5.0 seconds = 82.961 FPS

$ LIBGL_ALWAYS_SOFTWARE=true glxgears
5396 frames in 5.0 seconds = 1079.151 FPS
5462 frames in 5.0 seconds = 1092.304 FPS
5212 frames in 5.0 seconds = 1041.994 FPS
5340 frames in 5.0 seconds = 1067.869 FPS
5604 frames in 5.0 seconds = 1120.675 FPS
5414 frames in 5.0 seconds = 1082.626 FPS
5445 frames in 5.0 seconds = 1088.985 FPS

So definitely CPU rendering gives the best performance, but it's not so efficient when multiple graphics programs run at the same time. Moreover, yesterday I tried to launch the project on a Linux machine using NVIDIA GPU and I obtained ~27 fps in rviz with a very smooth movement during execution, so the problem was probably related to WSL

cc @Nicogene

martinaxgloria commented 1 year ago

After some research, I'm going to write here some pros and cons of the IK solvers that I think will be suitable for our application:

KDL kinematics plugin: it's the default MoveIt solver since it's probably the most widely spread. It works with serial chain composed by only few joints. It is not so robust with singularities.
IKFast: this is used also for complex kinematic chains and it allows to find the best solution in a very short time with respect to the previous one.
TRAC-IK: this is the best alternative to the KDL in presence of joint limits. It includes two IK implementations (an extension of the KDL algorithm and a Sequential Quadratic Programming optimization) which allows to find the best solution in a short time. However, since its complexity, it is slower than IKFast, but more accurate for robots with complex kinematics

The first is already implemented in MoveIt, while the last two are not part of it since ROS1, so could be integrated by compiling them from sources. I'd give a chance to TRAC-IK which seems to be the best solution.

cc @Nicogene @pattacini

pattacini commented 1 year ago

I'd give a chance to TRAC-IK which seems to be the best solution.

I agree 👍🏻

martinaxgloria commented 1 year ago

Ok, so I think we can close this issue if you agree. I can add to the README.md the instruction to integrate and compile TRAC-IK in the ros2 workspace

cc @Nicogene @pattacini

pattacini commented 1 year ago

I would keep it open until the doc is updated. Do you agree @martinaxgloria?

Alternatively, we may close this issue and then open a follow-up.

martinaxgloria commented 1 year ago

Do you agree @martinaxgloria?

Sure, let's keep it open

martinaxgloria commented 12 months ago

Hi @Nicogene, @pattacini

In order to close this activity, I think that I have to add to the documentation how to install TRAC-IK inside the ros workspace right? There's something else I'm missing?

pattacini commented 12 months ago

I'd say yep.

martinaxgloria commented 12 months ago

There's also the issue https://github.com/icub-tech-iit/study-moveit/issues/9 that concerns the documentation, so I'm a little bit confused about what is missing besides what I've said in the previous comment

pattacini commented 12 months ago

There's also the issue #9 that concerns the documentation, so I'm a little bit confused about what is missing besides what I've said in the previous comment

If you deem that the current documentation covers how to install the entire package, then we're good to go.

martinaxgloria commented 12 months ago

I added some other information and I opened a PR here

https://github.com/icub-tech-iit/study-moveit/pull/11

cc @Nicogene @pattacini

icub-tech-iit / xcub-moveit2

ROS2 – Make some research on IK solvers #5