Trajot cannot converge

[Jm20010201]

Hello, I am using trajot to optimize the trajectory of a mobile robotic arm. The above image shows the running results and settings. Why can't it converge? Is there a problem with my settings? Could you please help me take a look~ The version of tesseract_planning and trajopt is 0.23.1.

[Jm20010201]

When I reduce collision_comstraint_comfig.safety_margin, it can converge. Does trajot have strict requirements for the input trajectory? I read in the paper that only the starting and ending points need to be input to generate a trajectory, but trajot cannot converge when I use simple interpolation. I want to use it for motion planning of a mobile manipulator. I use the A-star algorithm to initialize the base path, and the manipulator uses simple interpolation. Moreover, it needs to run for about 6 seconds to converge. Is there any good way to perform real-time 9-degree-of-freedom motion planning~ Thank you~

[Levi-Armstrong]

I am not sure how familiar you are with SQP solvers so I may be providing more information than necessary. In the case of trajopt constraints are added with slack variables so QP solve will succeed and then the Trust Region implementation within TrajOpt increases the weight on these variables throughout the solve to slowly enforce the constraint. As a result if the weight * error of a cost is high it can compete with a constraint causing it to never solve. As a result, enabling velocity smoothing directly competes with the collision constraint. I usually disable velocity smoothing and only have acceleration and jerk smoothing enable. If this does not improve convergence, then I would also added the collision as a cost. This is usually my default setup on any trajopt motion planning solve.

[Jm20010201]

I am not sure how familiar you are with SQP solvers so I may be providing more information than necessary. In the case of trajopt constraints are added with slack variables so QP solve will succeed and then the Trust Region implementation within TrajOpt increases the weight on these variables throughout the solve to slowly enforce the constraint. As a result if the weight * error of a cost is high it can compete with a constraint causing it to never solve. As a result, enabling velocity smoothing directly competes with the collision constraint. I usually disable velocity smoothing and only have acceleration and jerk smoothing enable. If this does not improve convergence, then I would also added the collision as a cost. This is usually my default setup on any trajopt motion planning solve.

Thank you. I have indeed made improvements after trying~