rdiankov / openrave

Open Robotics Automation Virtual Environment: An environment for testing, developing, and deploying robotics motion planning algorithms.
http://www.openrave.org
Other
687 stars 340 forks source link

restore transform when EIO_SkipDOFValues and not added to env #1317

Closed eisoku9618 closed 9 months ago

eisoku9618 commented 9 months ago

@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.])

With this PR, we can restore at least transform and warn about DOF resetting.

related discussion

rdiankov commented 9 months ago

thanks~