Changing parent of object changes its position (by a small numerical error)

Report crashes, strange behaviour, or apparent bugs
Post Reply
Uli_v
Posts: 30
Joined: 14 Dec 2018, 18:07

Changing parent of object changes its position (by a small numerical error)

Post by Uli_v » 16 Jan 2019, 19:38

Hi,

I noticed that changing the parent of an object changes its position (slightly), if the parent does not have orientation (0,0,0) (in world reference).

In my example I am using "Dummy" as parent for "Cuboid". My code below is running in a threaded child script (I call once ChangingParentCheckPosition('Cuboid')). The script changes the parent of "Cuboid" 100 times to "no parent" and back (with parent "Dummy"). It seems that the position error accumulates after setting the "Dummy" as parent. The total error for 100 iterations is 1.6876e-05. Doesn't seem much, but if this is done many 10000 times (e.g. by the OMPL planning plugin, see below for more details), then the error can be around 0.002. If I run the script below with 20000 iterations (without print out for speedup), then I get an error of around 0.003.

Why do I want to change the parent of the object to world, and then back? I my actual application, I want to use OMPL planning to find a path for the gripper to an object using holonomic path planning. I use 3d position of the gripper (relative to world) as the state. The gripper is attached to the robot arm (the parent). The OMPL planner modifies the position of the gripper many times (order of 10000?) for collision checking (see writeState and readState functions in plugin.cpp of the OMPL plugin). At the end of planning I notice a position error in the order of 0.001-0.002. If I remove the parent of the gripper (make it without parent), then the planning works well (no position error after planning).

In this post "OMPL planner changes the position of object after planning" I am providing an example scene to reproduce both issues.

Code: Select all

ChangingParentCheckPosition=function(objectName)
    --objectName = 'Cuboid'--'UR5_tip'--'Cuboid'
    objectHandle=sim.getObjectHandle(objectName)
    parenthandle = sim.getObjectParent(objectHandle)
    print("Start")
    pos_init = getAndPrintPosition(objectName) 
    print("Changing Parent Check Position")
    for i=1,100,1 do
        --print("i ",i)
        print("with parent")
        getAndPrintPosition(objectName)       
        
        --make parentless
        sim.setObjectParent(objectHandle,-1,true)
        print("without parent")
        getAndPrintPosition(objectName)

        -- restore parent
        sim.setObjectParent(objectHandle,parenthandle,true)
    end
    print("End")
    pos_final = getAndPrintPosition(objectName)
    error_dist = 0
    for i=1,3,1 do
        -- manhattan distance
        error_dist = error_dist + math.abs(pos_init[i]-pos_final[i])
    end
    print("error_dist (manhattan)", error_dist)
    return 0
end

getAndPrintPosition=function(objectName)
    objectHandle = sim.getObjectHandle(objectName)
    local position = sim.getObjectPosition(objectHandle,-1)
    print(objectName, 'Position (global) ',position)
    return position
end
The output of the script is:

Code: Select all

Cuboid,Position (global) ,{0.056809045374393, 0.098305709660053, 0.10295955091715}
Changing Parent Check Position
with parent
Cuboid,Position (global) ,{0.056809045374393, 0.098305709660053, 0.10295955091715}
without parent
Cuboid,Position (global) ,{0.056809045374393, 0.098305709660053, 0.10295955091715}
with parent
Cuboid,Position (global) ,{0.05680900067091, 0.098305739462376, 0.10295967012644}
without parent
Cuboid,Position (global) ,{0.05680900067091, 0.098305739462376, 0.10295967012644}
with parent
Cuboid,Position (global) ,{0.056808941066265, 0.098305776715279, 0.10295975953341}
without parent
Cuboid,Position (global) ,{0.056808941066265, 0.098305776715279, 0.10295975953341}
with parent
(some lines removed here)
Cuboid,Position (global) ,{0.056804649531841, 0.098308749496937, 0.1029684022069}
with parent
Cuboid,Position (global) ,{0.056804604828358, 0.098308764398098, 0.10296852141619}
without parent
Cuboid,Position (global) ,{0.056804604828358, 0.098308764398098, 0.10296852141619}
with parent
Cuboid,Position (global) ,{0.056804567575455, 0.098308779299259, 0.10296858102083}
without parent
Cuboid,Position (global) ,{0.056804567575455, 0.098308779299259, 0.10296858102083}
with parent
Cuboid,Position (global) ,{0.056804515421391, 0.098308824002743, 0.10296861827374}
without parent
Cuboid,Position (global) ,{0.056804515421391, 0.098308824002743, 0.10296861827374}
End
Cuboid,Position (global) ,{0.056804470717907, 0.098308853805065, 0.1029687076807}
error_dist (manhattan),1.6875565052032e-05
I guess that the reason might be that V-REP stores the pose of the objects internally with reference to the parent, not the world. So when the parent is changed, it needs to recalculate the position in world frame. And there is some numerical error due to matrix multiplications. If the orientation of the parent is (0,0,0) (the same as "world") then the calculation is simple (just a translation, no rotation) and there is no error.

What is the best way to solve my problem? Remember, the problem is that either the planner or changing the parent introduces a positional error.

Option 1: If I change the parent of the gripper before and after planning, then there is a small position error, due to changing the parents. This should not be too bad if I do not do this too often (but I might want to do planning many times). I could reset the scene, which restores everything.

Option 2: If I do not change the parent of the gripper. Then there might be an error after planning. I could remember the position before planning, and then restore it to correct for the error. But it concerns me that there is already an error accumulating during planning.

Option 3: I could define the state for planning as position in the grippers parent frame. I haven't tried that yet. But in this case I can't reuse the pre-computed roadmap (I use PRM) if the parent's frame position/orientation changes (I might want to move the gripper and then re-plan). Therefore I prefer planning in global coordinates.

Thanks,
Uli

Post Reply