WIP: Fix clone and restore for invalid non colliding cache

[snozawa]

summary

• This PR fixes the bug in Cloning of env/kinbody when _listNonCollidingLinksValid is false #1442

Issue

• When Clone or kinbodysaver's restoring is called while _isListNonCollidingLinksValid is false, Grabbed's internal savers are inconsistent.
• For example,
  1. The original Grabbed has savers at the configuration A.
  2. Later, set the configuration to B.
  3. Then, call Clone or KinBodyStateSaver::Restore(pbody).
  4. In such case, the new Grabbed instance is created for the cloned/restored body. But, the newly created Grabbed has savers at the configuration B. Since the original saver is at the configuration A, the derived listNonCollidingLinks are different between the original one and the new one.

Resolution

• Principle
  • The intention of the past PR is to introduce the lazy computation.
  • Thus, in principle, the computation results with lazy computation should be exactly same as the results without lazy computation.
• Fix in this PR
  • Add the special constructor for Grabbed to copy the saver's states from other Grabbed.
• To do so, add the special constructor for KinBodyStateSaver and RobotStateSaver, to copy the states from other savers.
• The usual user should use the existing constructor. The newly added one is introduced for very specific use case.
• The new constructor does not support all Saver options. It only supports the copy-able states without deep copy. If the state is pointer, the copying of it might not be always expected.

Discussion

• To directly copy the saver's states, we assume that the robot's kinematics are same between two kin bodies.
• Just in case, add the checking for it: 76e9662
• Instead of this PR's resolution, we might be able to restore the original kinbody, copy such states from the original kinbody to the cloned kinbody, and then, instantiate the Grabbed. But, it looks still tricky and the computation time for it is not small. So, in this PR, i'm trying to simply copy the KinBodyStateSaver's state itself.

[rdiankov]

@snozawa Is this pull request still active?

[snozawa]

@snozawa Is this pull request still active?

thanks for checking!

yes, this is still active.
initially, this PR was ready but i'll need to resolve conflict and double check if this PR is still working.
I set WIP for a while.