SetLinkGeometriesFromGroup is slow because FCL geometries are always re-computed from scratch for all the geometry groups

[eisoku9618]

Here is a snippet to reproduce this issue.

from openravepy.databases import convexdecomposition
cdmodel = convexdecomposition.ConvexDecompositionModel(robot=robot, padding=0.010)
if not cdmodel.load():
   cdmodel.generate(padding=0.010)
linkGeometryGroups = [None] * len(robot.GetLinks())
for ilink, link in enumerate(robot.GetLinks()):
   linkGeometryGroups[link.GetIndex()] = cdmodel.GetGeometryInfosFromLink(ilink)
robot.SetLinkGroupGeometries('padding=0.010', linkGeometryGroups)

robot.SetLinkGeometriesFromGroup('padding=0.010') # time consumping part

When we call SetLinkGeometriesFromGroup with a geometry group name (padding=0.010 in this case), _ResetCurrentGeometryCallback in _vlistRegisteredCallbacks is called. There are following 2 problems and it is time consuming.

1. _ResetCurrentGeometryCallback calls InitKinBody, and InitKinBody always computes FCL geometries from scratch.
2. _ResetCurrentGeometryCallback is called for all the geometry groups (padding=0.010 and self in this case) even though we have specified a certain geometry group name (padding=0.010 in this case)

In my case, roughly speaking, 1 call of InitKinBody takes 50ms and SetLinkGeometriesFromGroup takes 100ms = 50ms x 2 (= len(['padding=0.010', 'self'])) in total.

For 1, using a cache will help, but it is prone to bugs. For 2, we should call _ResetCurrentGeometryCallback once for the specified geometry group name.

cc @kanbouchou @yoshikikanemoto