ModifyPlanningScene: different transformations for PlanningScene & RobotState

[JafarAbdi]

When I add a collision object to the scene using ModifyPlanningScene it gets added relative to the start state of the robot not the stage's current state, to reproduce checkout this branch and run the demo roslaunch moveit_task_constructor_demo demo.launch & roslaunch moveit_task_constructor_demo pickplace.launch

The reason is on my machine scene->getTransforms().getTransform("panda_hand") see returns different transformation than scene->getCurrentState().getGlobalLinkTransform("panda_hand") see and processCollisionObjectMsg uses the first one internally see

Is this an expected behaviour, or am I misunderstanding something.?

• System info:
  • Ubuntu 18.04 / Melodic
  • MoveIt branch: master

Thanks!

[v4hn]

Sounds like you found a bug in MoveIt's planning scenes with respect to diff scenes.

Can you provide a test for the main moveit repository?

[rhaschke]

I wouldn't even expect panda_hand (as a link transform) to be part of getTransforms()!
Actually, I would expect that processCollisionObjectMsg would use scene->getTransform().

[v4hn]

Very true indeed.

The Transforms code is rather neglected in MoveIt and there might also be a few other places using the wrong API.

@JafarAbdi could we get you to write a patch for this in MoveIt and add tests to check whether the correct transforms are used?

[JafarAbdi]

@JafarAbdi could we get you to write a patch for this in MoveIt and add tests to check whether the correct transforms are used?

I would be really happy to open a PR to fix it, but I still don't know what causes this bug

The Transforms code is rather neglected in MoveIt and there might also be a few other places using the wrong API.

Sorry for asking this question but I'm not very familiar with how PlanningScene handle transforms internally, could you please give an example where the wrong API is being used .?

[v4hn]

As @rhaschke pointed out, these lines use getTransforms(), which looks up an arbitrary set of stored transformations in the PlanningScene.
However, there is no guarantee that this set includes any of the links of the robot!
Instead they should use getTransform(frame_id) which looks up the links and attached objects of the robot, objects in the planning scene and falls back to looking at getTransforms().

[rhaschke]

I debugged the issue and filed a PR: moveit/moveit#2113.
Closing here.