learned grasp obj pose is not constant during planning

[JINXER000]

As I have tried, if I am using learned grasps that generate grasp poses in the world frame, the output plan will go wrong. I guess the reason is that during the planning, obj.observed_pose will be modified from time to time, as it is actually a get_pose() function returning obj position in the pybullet.

To fix this, I add one line set_pose(obj, obj.initial_pose, **kwargs) at the beginning of the get_fn() of get_grasp_gen_fn(). The initial pose is an extra field added to record the object pose before execution Accordingly, at the end of get_grasp_candidates(), I revised the line into grasps_tool = [multiply(invert(grasp), obj.initial_pose) for grasp in grasps_world] , meaning that the grasp pose in the world frame is transformed to the object frame.

Did anyone meet similar issues? Could anyone provide a more elegant solution?