Fix nonlinearprogram

1. Previously `NonlinearProgram` gives incorrect gradient if the `xind` in `addConstraint` contains duplicate variables, for example
```
cnstr = FunctionHandleConstraint(-1,1,2,@(x,y) x+y);
prog = prog.addNonlinearConstraint(cnstr,[1;1])
```
I fix this in a not very clean way (uses for loop)
2. If we add an empty linear constraint in `NonlinearProgram` and then delete it, the program will fail. Now it accepts empty linear constraint.