Trajopt: confusion about safety_margin and safety_margin_buffer

[marip8]

I'm running into some strange behavior around the safety_margin and safety_margin_buffer parameters. First, this is my understanding of both parameters (which is possibly incorrect):

safety_margin

• the distance within which an identified contact vector will be turned into a cost/constraint equation for collision avoidance during the optimization. Any contact within this distance identified after the completion of the optimization will be considered a violation of the collision avoidance constraint

safety_margin_buffer

• an additional distance, beyond the safety_margin distance, within which an identified contact vector will be turned into a cost equation for collision avoidance during the optimization. Any contacts identified within this distance, but greater than the safety_margin distance, at the end of the optimization do not constitute a violation of the collision avoidance constraint

I have a test case with a 6-DOF robot motion planning around a tall skinny obstacle in front of it. Different specifications of the safety_margin and safety_margin_buffer produce different results that I don't know how to explain:

Case 1

safety_margin = 0.1
safety_margin_buffer = 0.1
collision_as_constraint = true

Expected behavior: successful, smooth, intuitive, collision-free motion plan

case_1.mp4

Result: success

Case 2

safety_margin = 0.0
safety_margin_buffer = 0.2
collision_as_constraint = true

Expected behavior: same as Case 1

case_2.mp4

Result: Erratic motion at the beginning and end of the trajectory that "stretches" the waypoints out such that no individual state is in collision with the obstacle, but motion between the waypoints is not feasible

Case 3

safety_margin = 0.2
safety_margin_buffer = 0.0
collision_as_constraint = true

Expected behavior: same as Case 1, robot stays a little further away from the obstacle because the constraint is 0.2m away from collision

case_3.mp4

Result: meets expectation

Case 4

safety_margin = 0.2
safety_margin_buffer = 0.0
collision_as_constraint = false

Expected behavior: Same as Case 1

case_4.mp4

Result: Same as Case 3; robot stays fairly far away from obstacle even though collision avoidance is not a constraint

Case 5

safety_margin = 0.0
safety_margin_buffer = 0.2
collision_as_constraint = false

Expected behavior: same as Case 1

case_5.mp4

Result: meets expectation

Questions

• Why does Case 2 not match the result of Case 1?
  • It seems like the problems should end up being identical: any collision vector within 0.2m of the part should contribute to the optimization and the constraint for Case 2 is less restrictive than for Case 1
• Why does Case 4 not match the result of Case 1/5?
  • It seems like Case 4 and Case 5 should result in the same optimization since both only use collision as a cost and consider collisions within 0.2m of the obstacle