Fix _setGrabberLinkIndicesToIgnore gets invalidated

[Puttichai]

Summary

In function KinBody::RegrabAll, we accidentally invalidate _setGrabberLinkIndicesToIgnore of elements in (original) _vGrabbedBodies. By itself, this does not cause any problems since by the end of RegrabAll, the vector _vGrabbedBodies will get re-initialized correctly. However, in the situation where RegrabAll is called somewhere in the scope of a kinbody state saver with Save_GrabbedBodies options, the function RegrabAll will accidentally invalidate _setGrabberLinkIndicesToIgnore stored by the state saver. Once the state saver's Restore gets called, the body will be restored with incorrect states.

This pull request changes the code from swapping _setGrabberLinkIndicesToIgnore to making a copy instead.

---

To illustrate the behavior before this change, consider the following example. First, from src/robots directory, run

openrave.py -i puma.robot.xml

Then from the IPython terminal,

dog = env.ReadKinBodyURI('../models/objects/dog.wrl')
env.Add(dog)

robot.Grab(dog, robot.GetLink('LClaw'), range(len(robot.GetLinks())), None)
# The following will print [u'Base', u'LClaw', u'Puma1', u'Puma2', u'Puma3', u'Puma4', u'Puma5', u'Puma6', u'RClaw']
print( robot.GetGrabbedInfo(dog.GetName()).SerializeJSON().get('ignoreRobotLinkNames') )

with robot.CreateRobotStateSaver(KinBody.SaveParameters.GrabbedBodies):
    robot.RegrabAll()
    # The following will print [u'Base', u'LClaw', u'Puma1', u'Puma2', u'Puma3', u'Puma4', u'Puma5', u'Puma6', u'RClaw']
    print( robot.GetGrabbedInfo(dog.GetName()).SerializeJSON().get('ignoreRobotLinkNames') ) 

# At this point, the robot has been restored with the (corrupted) grabbed bodies in the saver
# The following will print None
print( robot.GetGrabbedInfo(dog.GetName()).SerializeJSON().get('ignoreRobotLinkNames') )

[rdiankov]

thanks~