Update constraint

Now we can delete and update the constraint in `NonlinearProgram`. Each `Constraint` object has a unique ID in the program. The user can delete or update the constraint using that ID.
You should be able to replace these lines with

```matlab
r_foot_contact_pts = r.getBody(r_foot).getTerrainContactPoints();
l_foot_contact_pts = r.getBody(l_foot).getTerrainContactPoints()
```