Reduced tracking performance when calling IK solver alone

[mmurooka]

Hoping to use a consistent IK solver between planner and controller, I implemented a sample program that uses the mc_rtc solver by itself and tried it out: https://github.com/mmurooka/tmp_sources/tree/master/mc_rtc_ik

However, I specified tasks for the transformation of the limbs, and the generated motion does not fully satisfy these tasks as shown in the movie below. I expect both feet to be fixed at the specified poses on the xy-plane and the right hand to follow a circule trajectory.

Is there anything in the sample code that should be modified to address this?

https://github.com/jrl-umi3218/mc_rtc/assets/6636600/f69a4489-35d7-44ef-84a6-77e1552d932f

[mmurooka]

On another note, mc_rtc's solver only provides an acceleration-based IK interface, whereas planners often use velocity-based IK due to its faster convergence, etc., and I wonder if mc_rtc's solver might not necessarily be suitable for use in a planner. Any thoughts on this would also be appreciated.

[gergondet]

Hi @mmurooka

Regarding the first issue, I have produced a similar example using mc_rtc for the visualization and I get what you would expect:

https://github.com/jrl-umi3218/mc_rtc/assets/473543/24fc88e0-db59-4bbd-8509-30ad6576bee8

The code for my example is here.

In your example, I think the issue lies here

Instead of const sva::PTransformd & pose = mbc.bodyPosW[mb.successor(0)]; you should use const sva::PTransformd & pose = mbc.bodyPosW[0]; but you might have to use the rotation's transpose here

I wonder if mc_rtc's solver might not necessarily be suitable for use in a planner. Any thoughts on this would also be appreciated.

That's a good question, I think it should be relatively easy to build a velocity-based solver using TVM + mc_tvm and benchmark it against the acceleration-based solver (and other velocity-based solvers if you have some suggestions) for IK purposes.

[mmurooka]

Thank you @gergondet

The following change you suggested solved the problem: https://github.com/mmurooka/tmp_sources/commit/e1688e6b04b2b099cb9c887f014756101aca9597

At least in my sample (with JVRC1), mb.successor(0) was 0, so the change of this part had no effect.

That's a good question, I think it should be relatively easy to build a velocity-based solver using TVM + mc_tvm and benchmark it against the acceleration-based solver (and other velocity-based solvers if you have some suggestions) for IK purposes.

That's great! I don't have any plans to implement a planner with mc_rtc's IK solver at this time, so the benchmark is not urgent, but it must be useful in the future.