Add EnvCollision constraint.
Add SelfCollision constraint.