initialize __mapReadableInterfaces on InitFromXXX functions

[eisoku9618]

@rdiankov Thank you for checking and merging https://github.com/rdiankov/openrave/pull/1315 But I had not pushed one more commit to that PR.

• https://github.com/rdiankov/openrave/pull/1315: clear readable interfaces on InitFromKinBodyInfo()
• this PR: clear readable interfaces on Destroy() which is called by InitFromXXX such as InitFromKinBodyInfo() and InitFromBoxes()

Here is the diff between the code before https://github.com/rdiankov/openrave/pull/1315 and this PR.

diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
index c8a1d88ac..64b71a5cf 100644
--- a/src/libopenrave/kinbody.cpp
+++ b/src/libopenrave/kinbody.cpp
@@ -491,6 +491,7 @@ void KinBody::Destroy()
     _selfcollisionchecker.reset();

     __hashKinematicsGeometryDynamics.resize(0);
+    ClearReadableInterfaces();
 }

[rdiankov]

Thanks

[eisoku9618]

@rdiankov There are some places in our internal repositories that expect KinBody::Init() does not clear readable interfaces. These places are not working as expected because of this change. I'm fixing these places to use KinBody::InitFromKinBodyInfo() instead. cc @kanbouchou @yoshikikanemoto

[rdiankov]

sounds like we were relying on undefined behavior, let's fix them asap. thanks

[yoshikikanemoto]

@rdiankov yes, they were relying on undefined behavior. eisoku made fixes for two places and they are already in master. but there might be other places. still checking

[eisoku9618]

@rdiankov I fixed almost all the places where we were relying on undefined behavior, but there is one more place I need to ask your opinion to fix.

@rdiankov cc @yoshikikanemoto @kanbouchou @ziyan Since d841c7633ec34cbd95c6486811eb2d5401b198ad commit, users can call KinBody::ExtractInfo() whenever body is not added to environment. This has a side effect that the transform is reset to identity because

1. KinBody::KinBodyStateSaver::~KinBodyStateSaver() does not do anything when body is not added to environment
2. KinBody::ExtractInfo() interanally set tranform to identity and DOFs to zeros in order to collect KinBodyInfo relying on KinBodyStateSaver

As a result, just calling KinBody::ExtractInfo() to a body which is not added to env resets its transform.

body = RaveCreateKinBody(env, '')
body.InitFromBoxes([[0,0,0,1,1,1]])
body.SetTransform([1,0,0,0,1,2,3])
print('before: %r' % body.GetTransformPose())
body.ExtractInfo(ExtractInfoOptions.SkipDOFValues)
print('after: %r' % body.GetTransformPose())

before: array([1., 0., 0., 0., 1., 2., 3.])
after: array([1., 0., 0., 0., 0., 0., 0.])

1. Should this behavior be fixed in OpenRAVE side? Or users must be aware of this side effect?
2. If this should be fixed in OpenRAVE, could you let me know how we can fix?

[eisoku9618]

My idea for 2 is restoring at least transform and warning about DOF. https://github.com/rdiankov/openrave/commit/5a00991c6710e7e47fdf39d5455c1aaf15c4880e

diff --git a/src/libopenrave/kinbody.cpp b/src/libopenrave/kinbody.cpp
index 64b71a5cf..df0710fa9 100644
--- a/src/libopenrave/kinbody.cpp
+++ b/src/libopenrave/kinbody.cpp
@@ -6072,6 +6072,12 @@ void KinBody::ExtractInfo(KinBodyInfo& info, ExtractInfoOptions options)

     // in order for link transform comparision to make sense
     KinBody::KinBodyStateSaver stateSaver(shared_kinbody(), Save_LinkTransformation);
+    boost::shared_ptr<void> restoreTransformEvenWhenBodyIsNotAddedToEnv = nullptr;
+    if( (options & EIO_SkipDOFValues) && GetEnvironmentBodyIndex() == 0 ) {
+        RAVELOG_WARN("resetting DOF to zeros");
+        restoreTransformEvenWhenBodyIsNotAddedToEnv = boost::shared_ptr<void>((void*) 0, std::bind(&KinBody::SetTransform, this, std::cref(info._transform)));
+    }
+
     SetTransform(Transform()); // so that base link is at exactly _baseLinkInBodyTransform
     vector<dReal> vZeros(GetDOF(), 0);
     SetDOFValues(vZeros, KinBody::CLA_Nothing);