如何修复“Toppra 未能找到最大路径加速度”

Question

我正在使用 Drake 的 Toppra 实现对 GcsTrajectoryOptimization 生成的路径进行后处理。在我设置加速度限制之前，一切都正常。

grid_points = Toppra.CalcGridPoints(gcs_traj, CalcGridPointsOptions())
toppra = Toppra(gcs_traj, plant, grid_points)
toppra.AddJointVelocityLimit(velocity_lb, velocity_ub)

# uncommenting the line below sometimes leads to a crash!
# toppra.AddJointAccelerationLimit(accel_lb, accel_ub) 
   
toppra_times = toppra.SolvePathParameterization()

产生的错误是

ERROR:drake:Toppra failed to find the maximum path acceleration at knot 9/172.

我不确定为什么这会导致失败。根据经验，我发现当我收紧加速度限制时，更多的轨迹无法解决。这违背了我的物理直觉，因为人们总是可以通过足够慢的速度得到可行的解决方案。

我该怎么做才能了解出了什么问题以及如何解决？

更新

我已经更新了我的代码以尝试使用Hung Pham 的实现，并且我遇到了一些类似的错误。

WARNING:toppra.algorithm.reachabilitybased.reachability_algorithm:A numerical error occurs: The controllable set at step [165 / 187] can't be computed.
WARNING:toppra.algorithm.reachabilitybased.reachability_algorithm:An error occurred when computing controllable velocities. The path is not controllable, or is badly conditioned.
WARNING:toppra.algorithm.algorithm:Fail to parametrize path. Return code: <ParameterizationReturnCode.FailUncontrollable: 'Error: Instance is not controllable'>

我已经确保我的路径可微分到 2 阶。 这是路径一阶导数的图，每个关节都有不同的颜色。

这是路径二阶导数的图。

这个问题显然看起来是可行的，任何加速度和速度限制都可以通过微不足道的时间膨胀来满足。

Answer 1

问题似乎与一阶导数轮廓中的前导和尾随平坦部分有关。任何路径重定时参数化都必须无限快地穿过这些区域才能取得进展。

顺便说一句，这些前导和尾随平面求解器是由 Drake 的 GcsTrajectoryOptimization 求解器生成的，该求解器遍历第一个和最后一个顶点为 Points 的图，然后在生成的轨迹上运行 NormalizeSegmentTimes 为 Toppra 做准备。据我所知，在将轨迹传递到 Toppra 之前修剪轨迹的前导和尾随部分可以解决这个问题。