How to use collision terms efficiently?

[haudren]

Hello,

I have a few questions regarding the correct use of collision terms within Trajopt. If they have been answered elsewhere, I apologize...

• Could you explain the difference between the various evaluator types? I have been using DISCRETE_CONTINUOUS because I think it considers both discrete points (based on max_segment_length) and continuous collision in-between those points. Is this correct? In that case is SINGLE_TIMESTEP discrete collision only at the knots, and CAST_CONTINUOUS continuous checking over every segment?
• What about the safety_margin and safety_margin_buffer? I seem to understand that basically any contact with distance smaller than safety_margin will contribute a cost that's proportional (?) to the penetration distance. But I am not sure how safety_margin_buffer works. Is it only used to compute continuous gradients around safety_margin?
• I have been trying to use both a constraint and a cost to ensure that the robot would always steer clear of collisions. When using the cost only, things seem to work well, but in convoluted scenarios, the optimized trajectory is still in collision. Thus, I tried to add a constraint with safety_margin of 0.0, but that seemed to lead to numerous solver failures. Is there an example somewhere on how to do this properly?

Thanks a lot.

[Levi-Armstrong]

• DISCRETE_CONTINUOUS - This perform discrete collision check at intermediate states between two trajectory state. The number of interpolated state is determined by the longest valid segment fraction.
• CAST_CONTINUOUS This does the same interpolate as DISCRETE_CONTINUOUS but it creates a casted collision object between the interpolated states.
• SINGLE_TIMESTEP This only check collision at the trajectory states

The safety_margin is used to calculate error for cost or constraint. The safety_margin_buffer is used when calculating the jacobian and error used for the quadratic function. This allows it to even though you may not be violating a constraint to still add a quadratic function to the problem so it understand the direction that will cause it to move closer to the violation. Ideally you would want to set the contact distance to infinite but this would come at a computation cost so this allows to you set the contact distance threshold used which is safety_margin + safety_margin_buffer.

Adding it as a constraint for convoluted scenarios can be problematic because it is a local optimal planner and at least for the open source solvers it does not allow it move away increasing the constraint violation and then come back to new local minimum. Though when adding them as a cost the solvers allows it to do this. We solve this two ways. The first is to leverage another planner to create a better seed like descartes or ompl. The second is to run a two stage trajopt but running the collision with cost and then after it finish solving running it using constraints. I have thought about update trajopt solver to support this. Also the commercial solver seem to handle these condition better.

[haudren]

Thanks for the clarification. If you don't mind I have a couple of follow-up questions:

• DISCRETE_CONTINUOUS does not perform continuous collision checks? It's simply sampling the trajectory and doing discrete checks?
• When you say casted, you mean continuous collision check?
• How big do you set the safety_margin_buffer? About twice the safety_margin?

Thanks for the precisions on the margin & constraint, it's really clear now! I suppose the filtering step in the SQP step rejection is a little too strict.

[Levi-Armstrong]

When you say casted, you mean continuous collision check?

Sure. In the context of Bullet Continuous collision checking it accomplishes this by taking the collision geometry at the start state and the same collision geometry at the end state and creates essential a new convex hull and then performs collision checking. Now it does not actually create the convex hull it just exploits the fact that Bullet only operates on the vertices so it create simple wrapper collision object so to Bullet it only sees on collision geometry. Because it is creating a casted collision object the longest valid segment length is a critical component. If this is large then the casted collision object does not accurately represent the path the object takes.

DISCRETE_CONTINUOUS does not perform continuous collision checks? It's simply sampling the trajectory and doing discrete checks?

Your understanding is correct.

How big do you set the safety_margin_buffer? About twice the safety_margin?

Yep, that is typically what I set it to.

Thanks for the precisions on the margin & constraint, it's really clear now! I suppose the filtering step in the SQP step rejection is a little too strict.

Yea, though to be honest I have not dug into the OSQP to see if there are some option to relax this so that may be something to explore if you have time.

[marip8]

I've run into some similar issues as described here where planning with collision avoidance as a constraint fails but collision avoidance as a cost succeeds (and is still collision-free)

The safety_margin is used to calculate error for cost or constraint. The safety_margin_buffer is used when calculating the jacobian and error used for the quadratic function. This allows it to even though you may not be violating a constraint to still add a quadratic function to the problem so it understand the direction that will cause it to move closer to the violation.

My understanding of this statement is that both the collision avoidance cost and constraint compute non-zero gradients when any contact is detected with distance < safety_margin_buffer (or possibly the safety_margin + safety_margin_buffer).

If this is true, then the collision constraint and the collision cost should have the same behavior w.r.t. the gradients produced during the optimization for any given problem. The only difference is that the collision avoidance constraint will make the optimization return failure if it's violated, whereas the cost would not.

Adding it as a constraint for convoluted scenarios can be problematic because it is a local optimal planner and at least for the open source solvers it does not allow it move away increasing the constraint violation and then come back to new local minimum. Though when adding them as a cost the solvers allows it to do this.

How is this true? If the cost and constraint versions produce the same gradient, then they should produce the same result on the optimization. Are the gradients of constraints somehow weighted or prioritized over the gradients of costs in the solvers we are using?

[Levi-Armstrong]

In a SQP solver a cost and constraint are different even if they produce the same gradient. The cost is associated with the objective function and a constraint associated with the constraint matrix.