ParabolicSmoother fails when called directly

[mkoval]

I can successfully use ParabolicSmoother to smooth a trajectory using the SmoothTrajectory helper function:

openravepy.planningutils.SmoothTrajectory(traj, 1, 1, 'ParabolicSmoother', '')
# Output: openravepy._openravepy_.openravepy_int.PlannerStatus.HasSolution

However, it fails if I try calling the ParabolicSmoother planner directly on the same trajectory:

planner = openravepy.RaveCreatePlanner(traj.GetEnv(), 'ParabolicSmoother')
params = openravepy.Planner.PlannerParameters()
params.SetConfigurationSpecification(
    traj.GetConfigurationSpecification().GetTimeDerivativeSpecification(0))
planner.InitPlan(None, params)
planner.PlanPath(traj)
# Output: openravepy._openravepy_.openravepy_int.Failed
# [parabolicsmoother.cpp:331 PlanPath] original ramp 0 is in collision!

This code is directly translated (as far as possible) from the implementation of SmoothTrajectory, so I am surprised that it yields a different result. I took a look at the source of ParabolicSmoother, but the cause of the original ramp 0 is in collision warning is quite inscrutable.

@rdiankov Any ideas here? This has been driving @psigen and I crazy for weeks now.

[rdiankov]

to answer the real concern, i think in python you are forgetting to reset the _sPostProcessingPlanner. By default that is initialized to a linear smoother, which then calls a secondary parabolic smoother. You have to reset this value to empty.

Up to now, very few people have used PlannerParameters directly in python, so there's been no need to add python bindings. However, I just added one for setting post processing (5de24e4dd12c62f8e2f65aab1564921cdf2ceea5):

params.SetPostProcessing('','')

after you've resolve this, see if your initial position in the trajectory in collision free.

hope this helps.

[mkoval]

That doesn't help. I recorded the exact PlannerParmaeters being passed by SmoothTrajectory by writing a custom Planner that prints them. Then, I passed this exact XML string to InitPlan using the string overload.

The ParabolicSmoother planner still succeeds when called by SmoothTrajectory and still fails when called directly. Something else must be different between these calls (maybe in the Python bindings for one of them?).

If it helps, here is the exact XML we passed:

<plannerparameters>
    <configuration>
        <group name="joint_values ADA 0 1 2 3 4 5 6 7" offset="0" dof="8" interpolation="linear"/>
    </configuration>
    <_vinitialconfig>5.55112e-16 0 0 0 0 0 0 0 </_vinitialconfig>
    <_vgoalconfig></_vgoalconfig>
    <_vconfiglowerlimit>-0.01 -0.01 -10000 -3.84518 -0.958648 -10000 -10000 -10000 </_vconfiglowerlimit>
    <_vconfigupperlimit>1.35 1.35 10000 0.626586 4.10409 10000 10000 10000 </_vconfigupperlimit>
    <_vconfigvelocitylimit>0.785398 0.785398 0.9425 0.9425 0.9425 1.885 1.885 1.885 </_vconfigvelocitylimit>
    <_vconfigaccelerationlimit>1 1 1 1 1 1 1 1 </_vconfigaccelerationlimit>
    <_vconfigresolution>0.02 0.02 0.02 0.02 0.02 0.02 0.02 0.02 </_vconfigresolution>
    <_nmaxiterations>0</_nmaxiterations>
    <_fsteplength>0</_fsteplength>
    <_nrandomgeneratorseed>0</_nrandomgeneratorseed>
    <_postprocessing planner="">
        <_nmaxiterations>20</_nmaxiterations>
        <_postprocessing planner="parabolicsmoother">
            <_nmaxiterations>100</_nmaxiterations>
        </_postprocessing>
    </_postprocessing>
    <interpolation></interpolation>
    <hastimestamps>0</hastimestamps>
    <hasvelocities>0</hasvelocities>
    <pointtolerance>0.2</pointtolerance>
    <outputaccelchanges>1</outputaccelchanges>
    <multidofinterp>0</multidofinterp>
    <verifyinitialpath>1</verifyinitialpath>
</plannerparameters>

[rdiankov]

ok, then i have one more suggestion.

by default, RRT planners and smoothers use the CO_ActiveDOFs collision option to make sure that things that don't move aren't checked for collision. for example the base is ignored. there are some helper functions in python to do this inside a scope

with CollisionOptionsStateSaver(env.GetCollisionChecker(),CollisionOptions.ActiveDOFs):
    planner.InitPlan(None, params)
    planner.PlanPath(traj)

[mkoval]

No dice. Same error. :frowning:

[rdiankov]

ok, well the fact is your original trajectory is in collision somewhere. i'm not sure where or why, you'll have to figure this out yourself. Because of the discretized way openrave does line collision checking, sometimes the robot will graze an obstacle.

In order to ignore such small collisions, i set the option to 0, meaning skip any collision checks for the original trajectory. That's what controls the error message you are getting. If you do:

params.SetExtraParameters('<verifyinitialpath>0</verifyinitialpath>')

this should work...

there is one more thing....

[mkoval]

Setting verifyinitialpath to zero fixed it!

However, I really think this is a bug in the verification code. I am planning in a completely empty environment, so it's unlikely that the planner is running into collision. I verified this by disabling everything in the environment before calling ParabolicSmoother:

for body in env.GetBodies():
    body.Enable(False)

The planner still fails with the warning:

[parabolicsmoother.cpp:331 PlanPath] original ramp 0 is in collision!

[rdiankov]

hmm... can we try to debug this?

the best way to figure out is to go inside parabolicsmoother.cpp and put a breakpoint around

 if( bCheck && checker.Check2(rampndtrimmed, 0xffff, outramps).retcode != 0 ) {

you'll see there' s a retcode. see what the value of that is, and then step through and check what paths it is going through.

i'm curious where it would fail..

[rdiankov]

by the way, about two months ago there were several bugs fixed in this file...

[rdiankov]

can you send a small python script that reproduces this problem so i can look at it on my end? thanks