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
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
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