Improve initial guess of acceleration limits for dynamic-limited robots in parabolicsmoother2

[snozawa]

Summary

• Improve parabolicsmoother2 computation for the robot with dynamic limits.
• In some condition of the recent big robot, the computed trajectory duration becomes very long. This PR intends to improve such worse-case outlier from parabolicmoother2.

Issue

• With the recent robot with dynamic limits, we observe that parabolicsmoother2 computes the slow trajectory.
• The following is the hand-made example to show the extreme case.
  • Wihtout this PR
    • 
  • With this PR
    • 
• Without this PR, the velocity does not reach to the max.
• The cause is:
  • In the first iteration in parabolicsmoother2 (_ComputeRampWithZeroVelEndpoints, _MergeConsecutiveSegments, and _Shortcut), the computed acceleration violates the dynamic limits. If the violation is large, the fTimeBasedSurpassMult becomes small. It also multiplied both to the velocity limits and the acceleration limits.
  • If it multiplied to the velocity limits, it cannot reach to the max.
  • In some case, the gap between GetDOFAccelerationLimits and GetDOFDynamicAccelerationJerkLimits might be very large. Thus, the slowing down becomes significant.

Resolution

• For the robot with dynamic limits, compute the initial guess of ramp acceleration limits based on dynamic limits.
• The dynamic limit is computed based on the current position and velocity.
• In each iteration in parabolicsmoother2, it computes the ramp for t0 and t1. Thus, the boundary conditions for positions and velocities are fixed: x0, v0, x1, v1.
• Thus, we can compute the dynamic acceleration limits at t0 and t1. This will be a necessary condition for ramp computation, e.g. good initial guess of acceleration.
• As a result:
  • If there is no dynamic limits violation observed by Check2 between t0 and t1, we don't need to conduct slowing down iteration due to dynamic limits. In this case, velocity limits are not reduced at all.
  • If there is dynamic limits violation observed by Check2 between t0 and t1, we still need to conduct slowing down iteration as before. But, the value of fTimeBasedSurpassMult becomes closer to 1.0, and no unnecessary velocity / acceleration reduction.

Experiment

• The main purpose of this PR is to mitigate the worst-case outlier. Confirmed that such worst case outlier is improved from the simulation. For example of the above example, the old trajectory duration was more than 10 sec and it becomes around 5 sec.
• Conducted several tests 1) to move the robot to random joint positions, 2) to do the pick/place simulation. For both cases, the worst outliers are improved. There is slight improvement on average trajectory duration like 50msec - 200msec. Due to the non-convex optimization nature of parabolicsmoother2, there are the cases which duration become worse. but, the amount of deterioration was not that high.

Future todos

• We need to revisit to the slowing down logic. For example, when acceleration is violated, it's nice to reduce velocity limit in some case, but not for other case. Need more investigation for this.
• This PR improves the robot with dynamic limits. Similarly, I wonder if we can have a better initial guess for 1) compute inverse dynamics, and 2) manip constraints. Need more investigation for this.
• There seem other situations that parabolicsmoother2 tend to be slow. for examples
  • fStartTimeVelMult and fStartTimeAccelMult carry over the mult from the previous iterations. but, once these become small, the later iterations remain slow.
  • If _neightstatefn is called in Check, somehow, large acceleration is created and it might cause much slowing down.

[Puttichai]

@snozawa Thank you for the discussion!

In _Shortcut function, let's make _dynamicLimitInfo a shared ptr instead so that we don't allocate anything in case the robot does not have dynamic limits. KinBodyConstPtr usedBody can also be part of DynamicLimitInfo itself.

[snozawa]

@snozawa Thank you for the discussion!

In _Shortcut function, let's make _dynamicLimitInfo a shared ptr instead so that we don't allocate anything in case the robot does not have dynamic limits. KinBodyConstPtr usedBody can also be part of DynamicLimitInfo itself.

thanks for your reviewing and comment!

I made a fix for it!
7c6ec3a

[snozawa]

I finished testing this PR on the real cell and simulated tests.
the detail is shared in the internal page.

Thanks!

[rdiankov]

@snozawa the MR looks good, but can we add a test somewhere to test that smoother is hitting this condition correctly (even your hand-made example is okay).

[snozawa]

@snozawa the MR looks good, but can we add a test somewhere to test that smoother is hitting this condition correctly (even your hand-made example is okay).

thanks for your comment!

sure, i'll prepare such automated test case!

[rdiankov]

@snozawa great, let me know when that's done.

[rdiankov]

@snozawa @kanbouchou Were you able to get a unit test for this case? It is holding up merging this fix. Thanks

[snozawa]

@snozawa @kanbouchou Were you able to get a unit test for this case? It is holding up merging this fix. Thanks

sorry for being late.
I still could not have time for it yet.

[rdiankov]

@snozawa what was the data you used to internally test this feature and reproduce the problem?

[snozawa]

@snozawa what was the data you used to internally test this feature and reproduce the problem?

sorry for being late, I just made a test and now waiting for results.